debugging ApproachHandle

source/armarx/manipulation/skills/ApproachHandle.cpp

@@ -6,6 +6,7 @@
 #include <Eigen/Geometry>
 
 #include <IceUtil/Time.h>
+#include <SimoxUtility/color/Color.h>
 
 #include <ArmarXCore/observers/variant/DatafieldRef.h>
 #include <ArmarXCore/util/CPPUtility/trace.h>
@@ -160,12 +161,12 @@ namespace armarx::manipulation::skills
         layer.add(armarx::viz::Sphere("tcp")
                       .position(global_P_tcp)
                       .radius(parameters.sphereVisuRadius)
-                      .color(255, 0, 0, parameters.visuAlpha));
+                      .color(simox::Color::red(parameters.visuAlpha)));
 
         layer.add(armarx::viz::Sphere("tcp_at_handle")
                       .position(global_P_tcp_at_handle)
                       .radius(parameters.sphereVisuRadius)
-                      .color(0, 255, 0, parameters.visuAlpha));
+                      .color(simox::Color::green(parameters.visuAlpha)));
 
         layer.add(armarx::viz::Arrow("movement_direction")
                       .fromTo(global_P_tcp, global_P_tcp + approachVector(params))

source/armarx/manipulation/skills/EstablishDoorContact.cpp

@@ -114,6 +114,10 @@ namespace armarx::manipulation::skills
         ARMARX_INFO << "Planning interaction. Node set is: `" << in.params.objectNodeSet << "`";
         interactionInfo = doorHelper.planInteraction(in.params.objectNodeSet);
 
+        object->setJointValues(dishwasher.config.jointMap);
+        object->setGlobalPose(dishwasher.config.globalPose.matrix());
+        object->setName(objectID.str()); // this is optional, name not used directly
+
         ARMARX_TRACE;
         ARMARX_INFO << "drawing";
         draw(in.params);
@@ -178,14 +182,19 @@ namespace armarx::manipulation::skills
         const Eigen::Vector3f global_P_tcp = tcp->getGlobalPosition();
 
         auto layer = ctx.arviz.layer("skills.establish_door_contact");
+
+        layer.add(viz::Pose("global_T_handle").pose(interactionInfo.rns.handle->getGlobalPose().matrix()));
+        layer.add(viz::Pose("global_T_door_initial_contact").pose(global_T_door_initial_contact.matrix()));
+        layer.add(viz::Pose(interactionInfo.rns.handleSurfaceProjection->getName()).pose(interactionInfo.rns.handleSurfaceProjection->getGlobalPose()));
+
         layer.add(armarx::viz::Sphere("handle")
                       .position(global_P_door_initial_contact)
                       .radius(parameters.sphereVisuRadius)
-                      .color(0, 0, 255, parameters.visuAlpha));
+                      .color(simox::Color::blue(parameters.visuAlpha)));
         layer.add(armarx::viz::Sphere("tcp")
                       .position(global_P_tcp)
                       .radius(parameters.sphereVisuRadius)
-                      .color(255, 0, 0, parameters.visuAlpha));
+                      .color(simox::Color::red(parameters.visuAlpha)));
         layer.add(armarx::viz::Arrow("direction_" + tcp->getName() + "_contact")
                       .fromTo(global_P_tcp, global_P_door_initial_contact)
                       .width(parameters.arrowVisuWidth));