iKinChain object and iCub synchronisation

[marco-monforte]

Hey everyone!

I'm trying to have an iKinChain armChain object synched with what is going on on the robot (for the moment in simulation). Basically, I read the encoders' values of the torso and the right arm, and I assign them to armChain in order to compute the respective pose in the Cartesian Space.

For some reason, however, while I read the correct joint values from the chain, the resulting Cartesian coordinates do not match what I read from /icubSim/cartesianController/right_arm/state:o. I am aware of the difference between links and DOFs in a chain object and the fact that the latter works in radiant, while the encoders are read in degrees, but even converting them I cannot get my head around the issue.

I leave here a basic Gist code for you to try. It is basically an update module that periodically updates the chain joint values with what is read from the encoders and prints them, along with the resulting Cartesian pose. You can let it run while playing with the simulator and the yarpmotorgui.

Help me Robotology community, you are my only hope 🙏

[pattacini]

Did't go through all your code, but at first glance I would try out the following change to line 127:

-Vector full_chain = q0_torso;
+Vector full_chain = q0_torso;
+std::swap(full_chain[0], full_chain[2]);

Include #include <algorithm> to have std::swap available.

Essentially, the motor device streams out the torso joints in an order that does not correspond to the kinematic chain of iCub 2.x, which in turn does expect the pitch-roll-yaw sequence.

[pattacini]

Hi @marco-monforte
I had the chance to test your code and found the bug.

Try the following:

diff --git a/positionDirectTest.cpp b/positionDirectTest.cpp
index d51e21a..440b22e 100644
--- a/positionDirectTest.cpp
+++ b/positionDirectTest.cpp
@@ -1,7 +1,9 @@
 
 #include <fstream>
-#include <math.h>
+#include <cmath>
 #include <string>
+#include <algorithm>
+#include <memory>
 
 #include <yarp/os/all.h>
 #include <yarp/dev/all.h>
@@ -35,7 +37,7 @@ private:
     IPositionControl  *ipos_arm, *ipos_torso;
     IPositionDirect   *iposd_arm, *iposd_torso;
 
-    iKinChain armChain;
+    std::unique_ptr<iCubArm> libArm;
 
 public:
 
@@ -85,8 +87,7 @@ public:
         qf_torso.resize(joints_torso), qf_arm.resize(joints_arm);
 
         // create an iCubArm object
-        iCubArm libArm("right_v2");
-        armChain=*(libArm.asChain());
+        libArm = std::make_unique<iCubArm>("right_v2");
         setTorso(torso);
 
         goHomePose();
@@ -98,22 +99,22 @@ public:
     bool updateModule() {
 
         synchChain();
-        yInfo() << "Chain joint values = " << (CTRL_RAD2DEG*armChain.getAng()).toString();
-        x_arm = armChain.EndEffPose();
+        yInfo() << "Chain joint values = " << (CTRL_RAD2DEG*libArm->getAng()).toString();
+        x_arm = libArm->EndEffPose();
         yInfo() << "Chain end-effector pose = " << x_arm.toString();
         return true;
     }
 
     void setTorso(bool torso) {
         if (torso) {
-            armChain.releaseLink(0);
-            armChain.releaseLink(1);
-            armChain.releaseLink(2);
+            libArm->releaseLink(0);
+            libArm->releaseLink(1);
+            libArm->releaseLink(2);
             yInfo() << "Torso enabled!";
         } else {
-            armChain.blockLink(0);
-            armChain.blockLink(1);
-            armChain.blockLink(2);
+            libArm->blockLink(0);
+            libArm->blockLink(1);
+            libArm->blockLink(2);
             yInfo() << "Torso disabled!";
         }
         synchChain();
@@ -125,12 +126,13 @@ public:
 		ienc_arm->getEncoders(q0_arm.data());
         if (torso) {
             Vector full_chain = q0_torso;
+            std::swap(full_chain[0], full_chain[2]);
             for (int i = 0; i < 7; i++) {
-                full_chain.push_back(q0_arm[i]);         
+                full_chain.push_back(q0_arm[i]);
             }
-            armChain.setAng(CTRL_DEG2RAD*full_chain);
+            libArm->setAng(CTRL_DEG2RAD*full_chain);
         } else {
-            armChain.setAng(CTRL_DEG2RAD*q0_arm);
+            libArm->setAng(CTRL_DEG2RAD*q0_arm);
         }
     }

You could also download the related patch, unzip it, and apply it straight away:

>> git apply diff.patch

Basically, you were creating an object of type iCubArm locally to the configure() method to then copy out a iKinChain object via a call to the asChain(). This doesn't work as when configure() terminates the original object gets destroyed causing problems to the copied iKinChain that won't find anymore the links.

To get around this, I've simply resorted above to a smart pointer.

[marco-monforte]

Thanks a lot, @pattacini !

I just tested your solution and it works smoothly 😃

[pattacini]

Happy it helped out.