the velocity transformation from Vc of the camera in the camera frame to the Ve of the effector in the fix frame

[lexidliny]

Hello!

These days i am trying to finish IBVS on a kinova Gen2 JOCO manipulator. I have looked through the vision servoing tutorials . With the tutorials, I have finished the intrinsic and extrinc calibration of the camera. Then, I used the servoFrankaIBVS.cpp file to compute the camera velocity by the apriltag given in the tutorial, and configure my own kinova manipulator. Considering the vc computed as metioned above is the velocity of camera in the camera frame, I need to transform vc to the velocity of the effector in the fix frame. Because the velocity control of kinova need to the velocity twist command. So I using the following code

// Send to the robot
      //robot.setVelocity(vpRobot::CAMERA_FRAME, v_c);

      vpHomogeneousMatrix fMc;
      vpHomogeneousMatrix fMe;

      vpRotationMatrix fRc;
      vpTranslationVector tran;
      vpRxyzVector euler;
      vpColVector v_s(6);

      (*MyGetCartesianCommand)(dataCommand);
            (*MyGetCartesianPosition)(dataPosition);
      euler[0] = dataPosition.Coordinates.ThetaX;
      euler[1] = dataPosition.Coordinates.ThetaY;
      euler[2] = dataPosition.Coordinates.ThetaZ;

      tran[0] = dataPosition.Coordinates.X;
      tran[1] = dataPosition.Coordinates.Y;
      tran[2] = dataPosition.Coordinates.Z;

      vpThetaUVector tu(euler);
      fMe.buildFrom(tran, tu);
      fMc = fMe * eMc;
      fRc = fMc.getRotationMatrix();

      vpVelocityTwistMatrix fVc;
      fVc.buildFrom(fRc);
      // vpVelocityTwistMatrix fVc(fMc);

      v_s = fVc * v_c;

      pointToSend.Position.CartesianPosition.X = v_s[0];
            pointToSend.Position.CartesianPosition.Y = v_s[1]; 
            pointToSend.Position.CartesianPosition.Z = v_s[2];
            pointToSend.Position.CartesianPosition.ThetaX = v_s[3];
            pointToSend.Position.CartesianPosition.ThetaY = v_s[4];
            pointToSend.Position.CartesianPosition.ThetaZ = v_s[5];

      MySendBasicTrajectory(pointToSend);

firstly I get the Cartesian coordinates of the effetors at every loop. Then I geted the matrix of fMe, where f is the fix frame of the robot at the base.

I calculated the fMc using fMe and eMc.

using the fVc buid from fRc to calculate the velocity twist of the carmea expressed in base from.

However, when i implemented this code the manipulator can move, but the directions wasn's right enough, and the destination of the effector was not the right place showned in the video.

now, I can't found the solution to solve the problem. Looking forward to your reply！

[fspindle]

I suggest that you use vpRobotKinova class that I introduced yesterday

[AnukritiSinghh]

Hi, did you try this on gazebo?

[fspindle]

Not on my side