diff --git a/source/RobotAPI/components/ArViz/CMakeLists.txt b/source/RobotAPI/components/ArViz/CMakeLists.txt
index f71f921683ed44fc55b34fb8284d4d3226300272..59d7a077e5f23def8cb11f55edd15d3877dc4360 100644
--- a/source/RobotAPI/components/ArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/ArViz/CMakeLists.txt
@@ -59,11 +59,13 @@ Coin/Visualizer.h
Client/Layer.h
Client/Elements.h
Client/Client.h
+Client/ClientCGALExtensions.h
Client/Color.h
Client/elements/Color.h
Client/elements/ElementOps.h
Client/elements/Mesh.h
+Client/elements/MeshCGALExtensions.h
Client/elements/PointCloud.h
Client/elements/point_cloud_type_traits.hpp
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 5e5bf8a0c9acc81a4dcc5bddb5d32c502efe8165..7d81b2d422458edf436b05648c8c443acd7c449a 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -45,55 +45,93 @@ namespace armarx::viz
return client;
}
+ // ////////////////////////////////////////////////////////////////// //
+ //layer
Layer layer(std::string const& name)
{
return Layer(componentName, name);
}
- void commit(std::initializer_list<Layer> const& layers)
+ template<class...Ts>
+ Layer layerContaining(std::string const& name, Ts&& ...elems)
{
- updates.clear();
- std::transform(layers.begin(), layers.end(), std::back_inserter(updates),
- [](const auto & layer)
+ auto l = layer(name);
+ l.add(std::forward<Ts>(elems)...);
+ return l;
+ }
+
+ // ////////////////////////////////////////////////////////////////// //
+ //enqueue
+ void enqueueLayer(const Layer& l)
+ {
+ updates.emplace_back(l.data_);
+ }
+ void enqueueLayer(const Layer* l)
+ {
+ updates.emplace_back(l->data_);
+ }
+ void enqueueLayer(std::initializer_list<Layer> const& layers)
+ {
+ for (const auto& l : layers)
{
- return layer.data_;
- });
- topic->updateLayers(updates);
+ enqueueLayer(l);
+ }
+ }
+ void enqueueLayer(const std::vector<const Layer*>& layers)
+ {
+ for (const auto& l : layers)
+ {
+ enqueueLayer(l);
+ }
+ }
+ void enqueueLayer(const std::vector<Layer>& layers)
+ {
+ for (const auto& l : layers)
+ {
+ enqueueLayer(l);
+ }
+ }
+ template<class...Ts>
+ void enqueueLayerContaining(std::string const& name, Ts&& ...elems)
+ {
+ enqueueLayer(layerContaining(name, std::forward<Ts>(elems)...));
+ }
+ // ////////////////////////////////////////////////////////////////// //
+ //commit
+ void commit(std::initializer_list<Layer> const& layers)
+ {
+ enqueueLayer(layers);
+ commit();
}
void commit(Layer const& layer)
{
- commit({layer});
+ enqueueLayer(layer);
+ commit();
}
void commit(const std::vector<const Layer*>& layers)
{
- updates.clear();
- std::transform(layers.begin(), layers.end(), std::back_inserter(updates),
- [](const auto & layerRef)
- {
- return layerRef->data_;
- });
- topic->updateLayers(updates);
+ enqueueLayer(layers);
+ commit();
}
void commit(const std::vector<Layer>& layers)
{
- updates.clear();
- std::transform(layers.begin(), layers.end(), std::back_inserter(updates),
- [](const auto & layer)
- {
- return layer.data_;
- });
+ enqueueLayer(layers);
+ commit();
+ }
+
+ void commit()
+ {
topic->updateLayers(updates);
+ updates.clear();
}
template<class...Ts>
void commitLayerContaining(std::string const& name, Ts&& ...elems)
{
- auto l = layer(name);
- l.add(std::forward<Ts>(elems)...);
- commit(l);
+ commit(layerContaining(name, std::forward<Ts>(elems)...));
}
private:
std::string componentName;
diff --git a/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h b/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h
new file mode 100644
index 0000000000000000000000000000000000000000..4888b73435befbab0e67f400d95c497d7ef19384
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/ClientCGALExtensions.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#include "Client.h"
+#include "elements/MeshCGALExtensions.h"
+
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
index 31cca73a22fdc087a8acda05afdb0c13629f30f2..a6ac8ae67326b3fe43ab5cd855791917b985dbc6 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
@@ -5,10 +5,23 @@
#include <SimoxUtility/EigenStdVector.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
#include <RobotAPI/interface/ArViz/Elements.h>
#include "ElementOps.h"
+namespace CGAL
+{
+ template < class PolyhedronTraits_3,
+ class PolyhedronItems_3,
+ template < class T, class I, class A>
+ class T_HDS,
+ class Alloc>
+ class Polyhedron_3;
+ template<class> class Surface_mesh;
+}
namespace armarx::viz
{
@@ -69,7 +82,20 @@ namespace armarx::viz
return this->faces(fs.data(), fs.size());
}
-
+ template<class T>
+ Mesh& mesh(const CGAL::Surface_mesh<T>& sm);
+
+ template < class PolyhedronTraits_3,
+ class PolyhedronItems_3,
+ template < class T, class I, class A>
+ class T_HDS,
+ class Alloc>
+ Mesh& mesh(const CGAL::Polyhedron_3 <
+ PolyhedronTraits_3,
+ PolyhedronItems_3,
+ T_HDS,
+ Alloc
+ > & p3);
/**
* @brief Builds a regular 2D grid in the xy-plane.
* @param extents The full extents in x and y direction.
diff --git a/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h b/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
new file mode 100644
index 0000000000000000000000000000000000000000..577ba25a95805bf2b9e10b7df56da568c8eca0ac
--- /dev/null
+++ b/source/RobotAPI/components/ArViz/Client/elements/MeshCGALExtensions.h
@@ -0,0 +1,109 @@
+#pragma once
+
+#include <CGAL/Polyhedron_3.h>
+#include <CGAL/Surface_mesh/Surface_mesh.h>
+#include <CGAL/boost/graph/iterator.h>
+
+#include "Mesh.h"
+
+namespace armarx::viz
+{
+ template<class T>
+ inline Mesh& Mesh::mesh(const CGAL::Surface_mesh<T>& sm)
+ {
+ using sm_t = CGAL::Surface_mesh<T>;
+ using vidx_t = typename sm_t::Vertex_index;
+
+ auto& vertices = data_->vertices;
+ auto& faces = data_->faces;
+ vertices.clear();
+ faces.clear();
+ vertices.reserve(sm.number_of_vertices());
+ faces.reserve(sm.number_of_faces());
+ std::map<vidx_t, std::size_t> index;
+ for (const auto& vidx : sm.vertices())
+ {
+ index[vidx] = vertices.size();
+ const auto& p = sm.point(vidx);
+ armarx::Vector3f visp;
+ visp.e0 = p.x();
+ visp.e1 = p.y();
+ visp.e2 = p.z();
+ vertices.emplace_back(visp);
+ }
+ for (const auto& fidx : sm.faces())
+ {
+ data::Face f;
+ const auto hf = sm.halfedge(fidx);
+ std::size_t i = 0;
+ for (auto hi : CGAL::halfedges_around_face(hf, sm))
+ {
+ const auto vidx = CGAL::target(hi, sm);
+ switch (i++)
+ {
+ case 0:
+ f.v0 = index.at(vidx);
+ break;
+ case 1:
+ f.v1 = index.at(vidx);
+ break;
+ case 2:
+ f.v2 = index.at(vidx);
+ break;
+ default:
+ break; // error handling below
+ }
+ }
+ ARMARX_CHECK_EQUAL(3, i) << "One face is no triangle!";
+ faces.emplace_back(f);
+ }
+ return *this;
+ }
+
+ template < class PolyhedronTraits_3,
+ class PolyhedronItems_3,
+ template < class T, class I, class A>
+ class T_HDS,
+ class Alloc>
+ inline Mesh& Mesh::mesh(const CGAL::Polyhedron_3 <
+ PolyhedronTraits_3,
+ PolyhedronItems_3,
+ T_HDS,
+ Alloc
+ > & p3)
+ {
+ auto& vertices = data_->vertices;
+ auto& faces = data_->faces;
+ vertices.clear();
+ faces.clear();
+ vertices.reserve(p3.size_of_vertices());
+ faces.reserve(p3.size_of_facets());
+ auto vbeg = p3.vertices_begin();
+ for (const auto& v : IteratorRange{vbeg, p3.vertices_end()})
+ {
+ const auto& p = v.point();
+ armarx::Vector3f visp;
+ visp.e0 = p.x();
+ visp.e1 = p.y();
+ visp.e2 = p.z();
+ vertices.emplace_back(visp);
+ }
+ for (const auto& fidx : IteratorRange{p3.facets_begin(), p3.facets_end()})
+ {
+ auto circ = fidx.facet_begin();
+ ARMARX_CHECK_EQUAL(3, CGAL::circulator_size(circ))
+ << "One face is no triangle!";
+ data::Face f;
+ f.v0 = std::distance(vbeg, circ->vertex());
+ ++circ;
+ f.v1 = std::distance(vbeg, circ->vertex());
+ ++circ;
+ f.v2 = std::distance(vbeg, circ->vertex());
+ ++circ;
+ ARMARX_CHECK_EXPRESSION(circ == fidx.facet_begin())
+ << "Internal error while circulating a facet";
+ faces.emplace_back(f);
+ }
+ return *this;
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 67405541ac6bd334191fee0f5d740c609e0a3e46..d5f45c229d9595643638eb7965354c2591afe952 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -314,7 +314,7 @@ namespace armarx
rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
rt2nonrtBuf.elb = _rtElbow->getPoseInRootFrame();
rt2nonrtBuf.tcpTarg = _rtPosController->getCurrentTarget();
- rt2nonrtBuf.elbTarg = math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(), Eigen::Matrix3f::Identity());
+ rt2nonrtBuf.elbTarg = ::math::Helpers::CreatePose(_rtPosController->getCurrentElbowTarget(), Eigen::Matrix3f::Identity());
if (_rtStopConditionReached)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 5b70a1ce221766fad8011baca299961464d2e9c4..54726a23a718c55f550923cfc0d9f9d1e06e434a 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -37,7 +37,7 @@ module armarx
bool forceThresholdInRobotRootZ = true;
bool optimizeNullspaceIfTargetWasReached = false;
};
-
+
class NJointCartesianWaypointControllerConfig extends NJointControllerConfig
{
string rns;