Added in two little spheres displaying the current tcp pose and the desired tcp pose.

source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp

@@ -54,7 +54,14 @@ armarx::RobotIKWidgetController::RobotIKWidgetController() : focusCameraOnRobot(
     robotIKComponentName = "";
     kinematicUnitComponentName = "";
 
-    dragger = NULL;
+    manipSeparator = NULL;
+    currentSeparator = NULL;
+
+    tcpManip = NULL;
+    tcpCurrent = NULL;
+    tcpCurrentTransform = NULL;
+    tcpDesired = NULL;
+    tcpManipTransform = NULL;
 }
 
 void armarx::RobotIKWidgetController::onInitComponent()
@@ -187,9 +194,22 @@ void armarx::RobotIKWidgetController::configured()
 
 void armarx::RobotIKWidgetController::solveIK()
 {
-    float x = dragger->center.getValue()[0] * 1000;
-    float y = dragger->center.getValue()[1] * 1000;
-    float z = dragger->center.getValue()[2] * 1000;
+    SoTransform* tempTransform = new SoTransform;
+    tempTransform->ref();
+
+    tempTransform->combineRight(tcpManip);
+    tempTransform->combineRight(tcpManipTransform);
+
+
+    SbRotation tempRotation = tempTransform->rotation.getValue();
+    SbVec3f tempTranslation = tempTransform->translation.getValue();
+
+    tempTransform->unref();
+
+    float x = tempTranslation[0] * 1000;
+    float y = tempTranslation[1] * 1000;
+    float z = tempTranslation[2] * 1000;
+
     Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
     mat (0,3) = x;
     mat (1,3) = y;
@@ -233,6 +253,14 @@ void armarx::RobotIKWidgetController::timerCB(void *data, SoSensor *sensor)
     try
     {
         armarx::RemoteRobot::synchronizeLocalClone(controller->robot,controller->robotStateComponentPrx);
+
+        if(controller->currentSeparator) {
+            Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
+            tcpMatrix(0, 3) /= 1000;
+            tcpMatrix(1, 3) /= 1000;
+            tcpMatrix(2, 3) /= 1000;
+            controller->tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(tcpMatrix));
+        }
         if(controller->focusCameraOnRobot) {
             controller->ui.robotViewer->getRobotViewer()->cameraViewAll();
         }
@@ -241,18 +269,49 @@ void armarx::RobotIKWidgetController::timerCB(void *data, SoSensor *sensor)
 
 void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
 {
-    std::cout << dragger << std::endl;
-
     Eigen::Matrix4f mat = robot->getRobotNodeSet(arg1.toStdString())->getTCP()->getGlobalPose();
-    ARMARX_INFO << VAROUT(mat);
-    if(dragger) {
-        this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(dragger);
+    mat(0, 3) /= 1000;
+    mat(1, 3) /= 1000;
+    mat(2, 3) /= 1000;
+
+    if(currentSeparator) {
+        this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(currentSeparator);
     }
-    dragger = new SoCenterballDragger();
-    this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(dragger, 0);
+    if(manipSeparator) {
+        this->ui.robotViewer->getRobotViewer()->getRootNode()->removeChild(manipSeparator);
+    }
+
+    manipSeparator = new SoSeparator;
+    currentSeparator = new SoSeparator;
+
+    tcpCurrent = new SoSphere;
+    tcpCurrent->radius = 0.03f;
+    tcpDesired = new SoSphere;
+    tcpDesired->radius = 0.03f;
+    tcpManip = new SoCenterballManip;
+    tcpCurrentTransform = new SoTransform;
+    tcpManipTransform = new SoTransform;
+
+    manipSeparator->addChild(tcpManipTransform);
+    manipSeparator->addChild(tcpManip);
+    manipSeparator->addChild(tcpDesired);
+
+    currentSeparator->addChild(tcpCurrentTransform);
+    currentSeparator->addChild(tcpCurrent);
+
+    this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(currentSeparator, 0);
+    this->ui.robotViewer->getRobotViewer()->getRootNode()->insertChild(manipSeparator, 0);
+
+    /*tcpCurrentTransform->translation.setValue(mat(0, 3) / 1000, mat(1, 3) / 1000, mat(2, 3) / 1000);
+    tcpCurrentTransform->rotation.setValue(SbRotation());*/
+
+    VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat);
+
+    tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat));
+    tcpManipTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat));
 
-    dragger->rotation.setValue(SbRotation(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat)));
-    dragger->center.setValue(mat(0, 3) / 1000, mat(1, 3) / 1000, mat(2, 3) / 1000);
+    /*tcpManip->translation.setValue(mat(0, 3) / 1000, mat(1, 3) / 1000, mat(2, 3) / 1000);
+    tcpManip->rotation.setValue(SbRotation(VirtualRobot::CoinVisualizationFactory::getSbMatrix(mat)));*/
 }
 
 Q_EXPORT_PLUGIN2(armarx_gui_RobotIKGuiPlugin, armarx::RobotIKGuiPlugin)

source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h

@@ -45,7 +45,8 @@
 
 //Inventor includes
 #include <Inventor/sensors/SoTimerSensor.h>
-#include <Inventor/draggers/SoCenterballDragger.h>
+#include <Inventor/manips/SoCenterballManip.h>
+#include <Inventor/nodes/SoSphere.h>
 
 namespace armarx
 {
@@ -110,7 +111,13 @@ namespace armarx
 
             VirtualRobot::RobotPtr robot;
 
-            SoCenterballDragger* dragger;
+            SoSeparator* manipSeparator;
+            SoSeparator* currentSeparator;
+            SoCenterballManip* tcpManip;
+            SoTransform* tcpManipTransform;
+            SoSphere* tcpDesired;
+            SoTransform* tcpCurrentTransform;
+            SoSphere* tcpCurrent;
 
             SoTimerSensor* timerSensor;
             static void timerCB(void *data, SoSensor *sensor);