Is there any BUG in robots.set_flMe()?

[allbluelai]

In tutorial-franka-coppeliasim-ibvs-apriltag.cpp, I run the code:

if ( 1 )
    {
      robot.setRobotState( vpRobot::STATE_POSITION_CONTROL );
      vpColVector q;
      robot.getPosition( vpRobot::JOINT_STATE, q );
      std::cout << "Initial joint position: " << q.t() << std::endl;
      vpHomogeneousMatrix fMfl(
          vpTranslationVector( 0.3261, 0.0932, 0.6310 ),
          vpRotationMatrix( vpRxyzVector( vpMath::rad( 168.17 ), vpMath::rad( 25.55 ), vpMath::rad( 39.13 ) ) ) );
      vpHomogeneousMatrix fMe(
          vpTranslationVector( 0.3707, 0.0740, 0.5396 ),
          vpRotationMatrix( vpRxyzVector( vpMath::rad( 168.17 ), vpMath::rad( 25.55 ), vpMath::rad( -5.87 ) ) ) );
      vpHomogeneousMatrix flMe = fMfl.inverse() * fMe;
      robot.set_flMe( flMe );
      vpMatrix fJe1( 6, 7 );
      robot.get_fJe( fJe1 );
      std::cout << "fJe1 ：\n" << fJe1 << std::endl;

      robot.set_flMe( flMe );
      vpMatrix fJe2( 6, 7 );
      robot.get_fJe( fJe2 );
      std::cout << "fJe2 ：\n" << fJe2 << std::endl;
      robot.coppeliasimStopSimulation();
      exit( 0 );
      q[0] += vpMath::rad( 10 ); // Add 10 deg axis 1
      std::cout << "Move to joint position: " << q.t() << std::endl;
      robot.setPosition( vpRobot::JOINT_STATE, q );
    }

the output of the terminal:

...
fJe1 ：
-0.05467073158  0.1084435441  -0.06656774707  0.1563449403  -0.08512165615  0.2266723672  -6.380964504e-05
0.4152706188  0.03947027947  0.3703218312  0.05690490497  0.2338695687  0.1516690187  -1.289307891e-05
0  -0.4089251852  0.06410448092  0.5740346342  -0.09065734369  0.1785741546  -2.846647752e-05
0  -0.3420205819  -0.664462926  0.3420204838  0.9396924968  0.3213941366  0.4313169748
0  0.9396924612  -0.241845078  -0.9396924968  0.3420204838  -0.8830221533  -0.1850333388
1  6.123233996e-17  0.7071067657  -1.043081929e-07  1.589324944e-08  0.3420200078  -0.8830222709
fJe2 ：
-0.03548340805  0.02259416643  -0.03090553266  0.2421943192  -0.1163683104  0.31390685  -0.0001021800582
0.4599599919  0.008223615989  0.3412173109  0.08815155484  0.3197189504  0.1963159302  -6.991089272e-05
0  -0.4443569927  0.087661651  0.6094664452  -0.1239722087  0.2118691679  -3.526088605e-05
0  -0.3420205819  -0.664462926  0.3420204838  0.9396924968  0.3213941366  0.4313169748
0  0.9396924612  -0.241845078  -0.9396924968  0.3420204838  -0.8830221533  -0.1850333388
1  6.123233996e-17  0.7071067657  -1.043081929e-07  1.589324944e-08  0.3420200078  -0.8830222709
Segmentation fault (core dumped)

I read fMfl and fMe from CoppeliaSim. I got fJe1≠fJe2. There seems to be a BUG with robots.set_flMe().

[aaoliva]

Dear Allbluelai,

It is not so clear to me what you are attempting to do with this code but, since the change of the end-effector pose in flange frame with the real robot has to be done using the web interface, this method was meant for being called once at the beginning of the program during configuration. BTW, the class vpROSRobotFrankaCoppeliasim implements some callbacks, like callback_flMe, which can automatically retrieve such parameters directly from coppeliasim (see tutorials).

I then run your code and the issue is that calling several times the method robot.set_flMe( flMe ) it will add the new transformation on top of the previous one. A quick fix you can do is to modify the method as follows:

void vpRobotFrankaSim::set_flMe( const vpHomogeneousMatrix &flMe ) { std::lock_guard< std::mutex > lock( m_mutex );

ifdef VISP_HAVE_OROCOS_KDL

KDL::Frame frame8Mfl_kdl; frame8Mfl_kdl = m_chain_kdl.segments[7].getFrameToTip(); vpHomogeneousMatrix f8Mfl, f8Me; for ( unsigned int i = 0; i < 3; i++ ) { for ( unsigned int j = 0; j < 3; j++ ) { f8Mfl[i][j] = frame8Mfl_kdl.M.data[3 i + j]; } f8Mfl[i][3] = frame8Mfl_kdl.p.data[i]; } f8Me = f8Mfl m_flMe.inverse() * flMe;

KDL::Rotation f8Re( f8Me[0][0], f8Me[0][1], f8Me[0][2], f8Me[1][0], f8Me[1][1], f8Me[0][2], f8Me[2][0], f8Me[2][1], f8Me[2][2] ); KDL::Vector f8te( f8Me[0][3], f8Me[1][3], f8Me[2][3] ); KDL::Frame f8Me_kdl( f8Re, f8te );

m_chain_kdl.segments[7].setFrameToTip( f8Me_kdl ); m_flMe = flMe;

endif

}

In this way, the kinematic chain will be always updated with the new transformation from the flange, even if a transformation was already added.

We will add this modification in the next update.

Best,

[allbluelai]

Dear @aaoliva ,

Thank you for your kind reply. I set robot.set_flMe() to verify whether the Jacobian matrix of the feature points is correct. I want to get the velocity of feature points which is attached to end effector. This means that the homogeneous matrix between the feature point frame and the end effector frame is constant. ${}^{E}Q$ is the coordinate of the feature point expressed in the end effecor frame. {f} is the robot base frame. {E} is the end effector frame and EORG is its origin. THere are, I guess, two ways to get the velocity of feature points expressed in the robot base frame {f}. One is get the Twist transformation matrix between Re(the feature point frame) and Rf(the robot base frame). --> see fVe. As shown in the above formula.

Another is set robot.set_flMe( flMe2 ). Here flMe2 is the homogeneous matrix between the feature point frame and the flange frame. Then I can get get_fJe() which means that I can get the feature point Jacobian expressed in the robot fixed frame