diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index daec8bc655e0f6431dc531f0401478838e7f053b..ab0998d12c36ad0145b4da76ce1ba6872664edec 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -4,10 +4,43 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <SimoxUtility/math/normal/normal_to_mat4.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include <SimoxUtility/math/pose/transform.h>
+
namespace armarx::viz
{
+ struct Convert
+ {
+ static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
+ {
+ dir = dir.normalized();
+ Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
+ Eigen::Vector3f cross = naturalDir.cross(dir);
+ float dot = naturalDir.dot(dir);
+ float angle = std::acos(dot);
+ if (cross.squaredNorm() < 1.0e-12)
+ {
+ // Directions are almost colinear ==> Do no rotation
+ cross = Eigen::Vector3f::UnitX();
+ if (dot < 0)
+ {
+ angle = M_PI;
+ }
+ else
+ {
+ angle = 0.0f;
+ }
+ }
+ Eigen::Vector3f axis = cross.normalized();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+
+ return ori;
+ }
+ };
+
const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
@@ -33,6 +66,103 @@ namespace armarx::viz
}
return *this;
}
+
+ Box& Box::set(const simox::OrientedBoxBase<float>& b)
+ {
+ size(b.dimensions());
+ return pose(b.transformation_centered());
+ }
+
+ Box& Box::set(const simox::OrientedBoxBase<double>& b)
+ {
+ size(b.dimensions<float>());
+ return pose(b.transformation_centered<float>());
+ }
+
+ Cylinder& Cylinder::fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
+ {
+ position((to + from) / 2);
+ orientation(Convert::directionToQuaternion((to - from).normalized()));
+ height((to - from).norm());
+
+ return *this;
+ }
+
+ Arrow& Arrow::direction(Eigen::Vector3f dir)
+ {
+ return orientation(Convert::directionToQuaternion(dir));
+ }
+
+ ArrowCircle& ArrowCircle::normal(Eigen::Vector3f dir)
+ {
+ Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
+ Eigen::Vector3f cross = naturalDir.cross(dir);
+ float angle = std::acos(naturalDir.dot(dir));
+ if (cross.squaredNorm() < 1.0e-12f)
+ {
+ // Directions are almost colinear ==> Do no rotation
+ cross = Eigen::Vector3f::UnitX();
+ angle = 0.0f;
+ }
+ Eigen::Vector3f axis = cross.normalized();
+ Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
+
+ return orientation(ori);
+ }
+
+ Polygon& Polygon::points(const std::vector<Eigen::Vector3f>& ps)
+ {
+ auto& points = data_->points;
+ points.clear();
+ points.reserve(ps.size());
+ for (auto& p : ps)
+ {
+ points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
+ }
+
+ return *this;
+ }
+
+ Polygon& Polygon::plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
+ {
+ const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
+ Eigen::Vector3f::UnitZ(), plane.normal());
+ return this->plane(plane.projection(at), ori, size);
+ }
+
+ Polygon& Polygon::plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
+ {
+ const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
+ const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
+
+ addPoint(center + x + y);
+ addPoint(center - x + y);
+ addPoint(center - x - y);
+ addPoint(center + x - y);
+
+ return *this;
+ }
+
+ Polygon& Polygon::circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation)
+ {
+ const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
+ ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
+
+ const float angle = 2 * M_PI / tessellation;
+ const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
+
+ Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
+ addPoint(simox::math::transform_position(pose, lastLocalPoint));
+ while (--tessellation)
+ {
+ const Eigen::Vector3f localPoint = rot * lastLocalPoint;
+ addPoint(simox::math::transform_position(pose, localPoint));
+ lastLocalPoint = localPoint;
+ }
+ return *this;
+ }
+
+
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 7b65e66ff7b273110366efa1da21ea7eff3412d5..9f3157c9fa39241bfb77f8251c5bbaedcfc665b2 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -14,11 +14,6 @@
#include <RobotAPI/interface/ArViz/Elements.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SimoxUtility/math/normal/normal_to_mat4.h>
-#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
-#include <SimoxUtility/math/pose/transform.h>
#include <SimoxUtility/shapes/OrientedBoxBase.h>
#include <Eigen/Geometry>
@@ -51,38 +46,6 @@ namespace armarx::viz
{
using data::ColoredPoint;
- struct Convert
- {
- static Eigen::Quaternionf directionToQuaternion(Eigen::Vector3f dir)
- {
- dir = dir.normalized();
- Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitY();
- Eigen::Vector3f cross = naturalDir.cross(dir);
- float dot = naturalDir.dot(dir);
- float angle = std::acos(dot);
- if (cross.squaredNorm() < 1.0e-12)
- {
- // Directions are almost colinear ==> Do no rotation
- cross = Eigen::Vector3f::UnitX();
- if (dot < 0)
- {
- angle = M_PI;
- }
- else
- {
- angle = 0.0f;
- }
- }
- Eigen::Vector3f axis = cross.normalized();
- Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
-
- return ori;
- }
- };
-
-
- // Move the ice datatypes into their own namespace
-
class Box : public ElementOps<Box, data::ElementBox>
{
public:
@@ -102,38 +65,8 @@ namespace armarx::viz
return size(Eigen::Vector3f(s, s, s));
}
- Box& set(const simox::OrientedBoxBase<float>& b)
- {
- size(b.dimensions());
- return pose(b.transformation_centered());
- }
- Box& set(const simox::OrientedBoxBase<double>& b)
- {
- size(b.dimensions<float>());
- return pose(b.transformation_centered<float>());
- }
-
- template<class T>
- Box(const std::string& name, const simox::OrientedBoxBase<T>& b)
- : ElementOps(name)
- {
- set(b);
- }
-
- template<class T>
- Box(
- const simox::OrientedBoxBase<T>& b,
- const std::string& name = std::to_string(std::time(0))
- )
- : Box(name, b)
- {}
-
- Box(std::string const& id)
- : ElementOps(id)
- {
- pose(Eigen::Matrix4f::Identity());
- }
-
+ Box& set(const simox::OrientedBoxBase<float>& b);
+ Box& set(const simox::OrientedBoxBase<double>& b);
};
@@ -156,14 +89,7 @@ namespace armarx::viz
return *this;
}
- Cylinder& fromTo(Eigen::Vector3f from, Eigen::Vector3f to)
- {
- position((to + from) / 2);
- orientation(Convert::directionToQuaternion((to - from).normalized()));
- height((to - from).norm());
-
- return *this;
- }
+ Cylinder& fromTo(Eigen::Vector3f from, Eigen::Vector3f to);
};
@@ -277,10 +203,7 @@ namespace armarx::viz
public:
using ElementOps::ElementOps;
- Arrow& direction(Eigen::Vector3f dir)
- {
- return orientation(Convert::directionToQuaternion(dir));
- }
+ Arrow& direction(Eigen::Vector3f dir);
Arrow& length(float l)
{
@@ -312,22 +235,7 @@ namespace armarx::viz
public:
using ElementOps::ElementOps;
- ArrowCircle& normal(Eigen::Vector3f dir)
- {
- Eigen::Vector3f naturalDir = Eigen::Vector3f::UnitZ();
- Eigen::Vector3f cross = naturalDir.cross(dir);
- float angle = std::acos(naturalDir.dot(dir));
- if (cross.squaredNorm() < 1.0e-12f)
- {
- // Directions are almost colinear ==> Do no rotation
- cross = Eigen::Vector3f::UnitX();
- angle = 0.0f;
- }
- Eigen::Vector3f axis = cross.normalized();
- Eigen::Quaternionf ori(Eigen::AngleAxisf(angle, axis));
-
- return orientation(ori);
- }
+ ArrowCircle& normal(Eigen::Vector3f dir);
ArrowCircle& radius(float r)
{
@@ -371,12 +279,6 @@ namespace armarx::viz
return *this;
}
- template<class...Ts>
- Polygon& lineColor(Ts&& ...ts)
- {
- return lineColor({std::forward<Ts>(ts)...});
- }
-
Polygon& lineColorGlasbeyLUT(std::size_t id, int alpha = 255)
{
return lineColor(Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha)));
@@ -389,18 +291,7 @@ namespace armarx::viz
return *this;
}
- Polygon& points(std::vector<Eigen::Vector3f> const& ps)
- {
- auto& points = data_->points;
- points.clear();
- points.reserve(ps.size());
- for (auto& p : ps)
- {
- points.push_back(armarx::Vector3f{p.x(), p.y(), p.z()});
- }
-
- return *this;
- }
+ Polygon& points(std::vector<Eigen::Vector3f> const& ps);
Polygon& addPoint(Eigen::Vector3f p)
{
@@ -415,12 +306,7 @@ namespace armarx::viz
* @param at Center of rectangle, is projected onto plane.
* @param size Extents of rectangle.
*/
- Polygon& plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size)
- {
- const Eigen::Quaternionf ori = Eigen::Quaternionf::FromTwoVectors(
- Eigen::Vector3f::UnitZ(), plane.normal());
- return this->plane(plane.projection(at), ori, size);
- }
+ Polygon& plane(Eigen::Hyperplane3f plane, Eigen::Vector3f at, Eigen::Vector2f size);
/**
* @brief Add points representing the XY-plane of the given coordinate system as rectangle.
@@ -428,37 +314,9 @@ namespace armarx::viz
* @param orientation The orientation of the coordinate system.
* @param size The XY-size of the rectangle.
*/
- Polygon& plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size)
- {
- const Eigen::Vector3f x = 0.5f * size.x() * (orientation * Eigen::Vector3f::UnitX());
- const Eigen::Vector3f y = 0.5f * size.y() * (orientation * Eigen::Vector3f::UnitY());
-
- addPoint(center + x + y);
- addPoint(center - x + y);
- addPoint(center - x - y);
- addPoint(center + x - y);
+ Polygon& plane(Eigen::Vector3f center, Eigen::Quaternionf orientation, Eigen::Vector2f size);
- return *this;
- }
-
- Polygon& circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation = 64)
- {
- const Eigen::Matrix4f pose = simox::math::normal_pos_to_mat4(normal, center);
- ARMARX_CHECK_GREATER_EQUAL(tessellation, 3);
-
- const float angle = 2 * M_PI / tessellation;
- const Eigen::Matrix3f rot = simox::math::rpy_to_mat3f(0, 0, angle);
-
- Eigen::Vector3f lastLocalPoint = Eigen::Vector3f::UnitX() * radius;
- addPoint(simox::math::transform_position(pose, lastLocalPoint));
- while (--tessellation)
- {
- const Eigen::Vector3f localPoint = rot * lastLocalPoint;
- addPoint(simox::math::transform_position(pose, localPoint));
- lastLocalPoint = localPoint;
- }
- return *this;
- }
+ Polygon& circle(Eigen::Vector3f center, Eigen::Vector3f normal, float radius, std::size_t tessellation = 64);
};
diff --git a/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt b/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
index 678b67dd3df935ebf6d7cb2bb530117f6430de9f..beb8a9d5e291f1c8faab89834417994baaeac0b9 100644
--- a/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
+++ b/source/RobotAPI/components/DebugDrawerToArViz/CMakeLists.txt
@@ -6,6 +6,7 @@ set(COMPONENT_LIBS
${PROJECT_NAME}Interfaces
RobotAPIComponentPlugins
+ ArViz
)
set(SOURCES
diff --git a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
index dcf81778c35b15b22b99fdcde688e18d7f5af801..8649d60af88061cfdb973a5fdbe94f7cbaed9c4b 100644
--- a/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
+++ b/source/RobotAPI/components/NaturalIKTest/CMakeLists.txt
@@ -6,6 +6,7 @@ set(COMPONENT_LIBS
# RobotAPICore # for DebugDrawerTopic
RobotAPIInterfaces
ArmarXGuiComponentPlugins
+ ArViz
natik
diffik
)
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
index fc4a18f313bf5320bc3307448b3dba645b8ffc41..8b4239814b0a3a7cdc6f1abd54a9da6f889fe122 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/CMakeLists.txt
@@ -6,5 +6,6 @@ armarx_add_component(
RobotAPICore
RobotAPIComponentPlugins
RobotUnit
+ ArViz
)
armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
index 524b650bf5a3767a92a457ab97b26ffcdd903a40..44687f4467d65481b8500e10871112863c892be2 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/CMakeLists.txt
@@ -6,5 +6,6 @@ armarx_add_component(
RobotAPICore
RobotAPIComponentPlugins
RobotUnit
+ ArViz
)
armarx_add_component_executable("main.cpp")
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index cce3efe6d385799d6496d39d0d0ca4362177403e..076fd258ef969776ab00016874a8813de38e3c67 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -8,6 +8,7 @@ armarx_add_library(
RobotStatechartHelpers # Contains RobotNameHelper
RobotAPI::armem
RobotAPI::ArmarXObjects
+ RobotAPI::ArViz
SOURCES box_to_grasp_candidates.cpp
grasp_candidate_drawer.cpp
GraspCandidateHelper.cpp