diff --git a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
index 6002c8e9537044d6aebd9a1513c88b317085c607..5eb382bfd405d8c0c4888493038488c14b4cfb9d 100644
--- a/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
+++ b/GeometricPlanning/ArticulatedObjectGeometricPlanningHelper.cpp
@@ -21,11 +21,11 @@
#include "VirtualRobot/VirtualRobotException.h"
#include <GeometricPlanning/ParametricPath.h>
+#include <GeometricPlanning/assert/assert.h>
#include <GeometricPlanning/path_primitives/CircleSegment.h>
#include <GeometricPlanning/path_primitives/Line.h>
#include <GeometricPlanning/path_primitives/PathPrimitive.h>
#include <GeometricPlanning/types.h>
-#include <GeometricPlanning/assert/assert.h>
namespace simox::geometric_planning
@@ -41,11 +41,13 @@ namespace simox::geometric_planning
ArticulatedObjectGeometricPlanningHelper::getPathForNode(const std::string& nodeName) const
{
const auto node = articulatedObject->getRobotNode(nodeName);
+ REQUIRE(node != nullptr);
simox::geometric_planning::ArticulatedObjectGeometricPlanningHelper helper(
articulatedObject);
const auto parents = node->getAllParents();
+ REQUIRE(not parents.empty());
// VR_INFO << parents.size() << " parents for " << nodeName;
@@ -78,6 +80,7 @@ namespace simox::geometric_planning
const auto parentJoint = std::find_if(parents.begin(), parents.end(), isJoint);
const auto& joint = *parentJoint;
+ REQUIRE(joint != nullptr);
const auto parametricPath = helper.getPathForNode(node->getName(), joint->getName());
@@ -133,7 +136,7 @@ namespace simox::geometric_planning
// ARMARX_DEBUG << "Radius: " << radius;
REQUIRE_MESSAGE(joint->getJointLimitHigh() > joint->getJointLimitLow(),
- "Not implemented yet. Do so by flipping the z axis of the joint.");
+ "Not implemented yet. Do so by flipping the z axis of the joint.");
const ParameterRange parameterRange{
.min = 0, .max = joint->getJointLimitHigh() - joint->getJointLimitLow()};
@@ -162,8 +165,7 @@ namespace simox::geometric_planning
joint_plane__joint_plane_P_node_initial.x());
// ARMARX_DEBUG << VAROUT(yaw);
- const Pose joint_plane_T_joint_reference(
- Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()));
+ const Pose joint_plane_T_joint_reference(Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()));
subpart->setJointValue(joint->getName(), joint->getJointLimitLow());
@@ -172,7 +174,7 @@ namespace simox::geometric_planning
const Pose root_T_joint = root_T_global * global_T_joint;
const Pose root_T_joint_reference(root_T_joint * joint_T_joint_plane *
- joint_plane_T_joint_reference);
+ joint_plane_T_joint_reference);
const auto jointReferenceNode = std::make_shared<VirtualRobot::RobotNodeFixed>(
subpart, nodeJointReference, root_T_joint_reference.matrix());
@@ -180,7 +182,8 @@ namespace simox::geometric_planning
// TODO check if node already exists and unregister if needed
subpart->registerRobotNode(jointReferenceNode);
- CHECK(!jointReferenceNode->initialize(subpart->getRootNode()));
+ CHECK_MESSAGE(jointReferenceNode->initialize(subpart->getRootNode()),
+ "Failed to initialize node `" << jointReferenceNode->getName() << "`!");
// reset joint state
joint->setJointValue(initialJointValue);
diff --git a/GeometricPlanning/CMakeLists.txt b/GeometricPlanning/CMakeLists.txt
index 3e784bd4d695c0d9bbdacdb762bf51aafe5c55bf..371f3f52179e1b97ec1029cbb43d87dfef8f073b 100644
--- a/GeometricPlanning/CMakeLists.txt
+++ b/GeometricPlanning/CMakeLists.txt
@@ -31,6 +31,7 @@ SET(SOURCES
path_primitives/Circle.cpp
path_primitives/CircleSegment.cpp
path_primitives/Line.cpp
+ visualization.cpp
)
SET(HEADERS
@@ -43,6 +44,7 @@ SET(HEADERS
path_primitives/CircleSegment.h
path_primitives/Line.h
assert/assert.h
+ visualization.h
)
ADD_LIBRARY(GeometricPlanning SHARED) # sources will be added later
diff --git a/GeometricPlanning/visualization.cpp b/GeometricPlanning/visualization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dc943d41c6bb581b8936ec7eb34557b3742ee8a
--- /dev/null
+++ b/GeometricPlanning/visualization.cpp
@@ -0,0 +1,117 @@
+#include "visualization.h"
+
+#include <cmath>
+
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+
+#include <GeometricPlanning/ParametricPath.h>
+#include <GeometricPlanning/assert/assert.h>
+#include <GeometricPlanning/path_primitives/CircleSegment.h>
+#include <GeometricPlanning/path_primitives/Line.h>
+#include <Inventor/SbMatrix.h>
+#include <Inventor/SbRotation.h>
+#include <Inventor/SbVec3f.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoTransform.h>
+
+namespace simox::geometric_planning
+{
+ constexpr float defaultWidth = 5.f;
+ constexpr std::size_t numberOfCircleParts = 20;
+ inline const simox::Color DefaultColor = simox::Color::blue();
+
+ constexpr float
+ scaleColor(const auto colorVal)
+ {
+ return colorVal / 255.f;
+ }
+
+ namespace detail
+ {
+ SoNode*
+ visualizePathLine(const geometric_planning::ParametricPath& path)
+ {
+ const auto startPos = path.getPose(path.parameterRange().min);
+ const auto endPos = path.getPose(path.parameterRange().max);
+
+ return VirtualRobot::CoinVisualizationFactory::createCoinLine(
+ startPos.matrix(),
+ endPos.matrix(),
+ defaultWidth,
+ scaleColor(DefaultColor.r),
+ scaleColor(DefaultColor.g),
+ scaleColor(DefaultColor.b));
+ }
+
+
+ SoNode*
+ visualizeCircleSegment(const geometric_planning::ParametricPath& path)
+ {
+ const auto* circleSegment =
+ dynamic_cast<geometric_planning::CircleSegment*>(path.path.get());
+ REQUIRE(circleSegment != nullptr);
+
+ const float circleCompletion =
+ (circleSegment->parameterRange().max - circleSegment->parameterRange().min) /
+ (2 * M_PIf32);
+
+ // circle centered at origin
+ auto* circleNode = VirtualRobot::CoinVisualizationFactory::createCoinPartCircle(
+ circleSegment->getRadius(),
+ circleCompletion,
+ defaultWidth,
+ scaleColor(DefaultColor.r),
+ scaleColor(DefaultColor.g),
+ scaleColor(DefaultColor.b),
+ numberOfCircleParts);
+
+
+ const Pose global_T_path_origin(path.frame->getGlobalPose());
+
+ const SbVec3f translation(global_T_path_origin.translation().x(),
+ global_T_path_origin.translation().y(),
+ global_T_path_origin.translation().z());
+
+ const SbVec3f scale(1., 1., 1.);
+
+ const Eigen::AngleAxisf rot(global_T_path_origin.linear());
+ const SbRotation rotation(SbVec3f(rot.axis().x(), rot.axis().y(), rot.axis().z()),
+ rot.angle());
+
+ SbMatrix transf;
+ transf.setTransform(translation, rotation, scale);
+
+ auto* t = new SoMatrixTransform();
+ t->matrix.setValue(transf);
+
+ auto* separator = new SoSeparator();
+ separator->addChild(t);
+ separator->addChild(circleNode);
+ return separator;
+ }
+ } // namespace detail
+
+
+ SoNode*
+ visualize(const geometric_planning::ParametricPath& path)
+ {
+ if (dynamic_cast<geometric_planning::Line*>(path.path.get()) != nullptr)
+ {
+ return detail::visualizePathLine(path);
+ }
+
+ if (dynamic_cast<geometric_planning::CircleSegment*>(path.path.get()) != nullptr)
+ {
+ return detail::visualizeCircleSegment(path);
+ }
+
+ REQUIRE_MESSAGE(false, "Unknown parametric path");
+ return nullptr;
+ }
+
+} // namespace simox::geometric_planning
diff --git a/GeometricPlanning/visualization.h b/GeometricPlanning/visualization.h
new file mode 100644
index 0000000000000000000000000000000000000000..5dc38d42e89cf1619b13774f7468224da5290284
--- /dev/null
+++ b/GeometricPlanning/visualization.h
@@ -0,0 +1,18 @@
+
+#pragma once
+
+class SoNode;
+
+namespace simox::geometric_planning
+{
+ class ParametricPath;
+
+ SoNode* visualize(const ParametricPath& path);
+
+ namespace detail
+ {
+ SoNode* visualizePathLine(const ParametricPath& path);
+ SoNode* visualizeCircleSegment(const ParametricPath& path);
+ } // namespace detail
+
+} // namespace simox::geometric_planning