Set Point Task with Jacobian Pseudo Inverse

[amdshameer]

I am trying to do a set point test with left foot as base and right foot as end effector for HRP-4. I am able to reach the set point (x,y,z,R,P,Y), only when I use the joints in right foot(by setting left foot joint columns to zero). If I use the entire kinematic chain including the joints from left foot, I am not able to reach the set point. The legs make some very inappropriate motions. Here is a little pseudo code: hrp4_mb is a multi-body with left foot L_ANKLE_R_LINK as base body (With help from @gergondet @haudren ) hrp4_mbc is corresponding MultiBodyConfig

rbd::Jacobian jac_LF(hrp4_mb, "R_ANKLE_R");
const Eigen::MatrixXd& jac_matrix = jac_LF.jacobian(hrp4_mb, hrp4_mbc);
Eigen::MatrixXd J(6, hrp4_mb.nrDof());

jac_LF.fullJacobian(hrp4_mb, jac_matrix, J);
Eigen::Vector3d trans hrp4_mbc.bodyPosW[hrp4_mb.bodyIndexByName("R_ANKLE_R_LINK")].translation();
Eigen::Matrix3d rot hrp4_mbc.bodyPosW[hrp4_mb.bodyIndexByName("R_ANKLE_R_LINK")].rotation();

Eigen::VectorXd desired(6), current(6);
desired << R,P,Y,x,y,z,;
current << rot.eulerAngles(2,1,0),trans;

Eigen::Matrix6d GAIN_MATRIX;
velocityVector << GAIN_MATRIX * (desired - current)
Eigen::MatrixXd q_dot = pesudoInverse(J) * velocityVector;

while (timeStep < maxTimeStep)
{
    q = q + (0.05*q_dot);
    set new q to hrp4_mbc.q;
    rbd::forwardKinematics
    rbd::forwardVelocity
    get translation and rotation for right foot ("R_ANKLE_R_LINK")
    velocityVector_temp = GAIN_MATRIX *(desired - current_temp) // FK for rightfoot
    rbd::Jacobian
    get fullJacobian J_temp
    q = pesudoInverse(J_temp)*velocityVector_temp
    timeStep = timeStep + 0.05 // 0.05 to match VREP simulation
}

Can someone kindly point out the mistake I am doing here.

[haudren]

Why don't you have a look at either IK.cpp or at the tutorials ?

From here, I can see at least two problems:

• You are doing RPY differences. This is not the same thing as doing rotationError(rot0, rot1) (It is the log map of rot1^T rot0). Please use the latter.
• You are explicitly computing the pseudo-inverse (as far as I can tell from your pseudo-code). This is never a good idea, you should only ever compute X = J^# B via a call of the form solve(J, B).
• I hope that in your real code the gain matrix is correctly initialized.

[aescande]

You are doing RPY differences. This is not the same thing as doing rotationError(rot0, rot1) (It is the log map of rot1^T rot0). Please use the latter.

To be more complete, your Jacobian matrix need to be coherent with the error you are defining in that it relies the joint speed with the speed of your error. RBDyn's jacobian are coherent with rotationError. If you want to use RPY (which I don't encourage as you might face singularity issues if you error go over 90°), you need to adapt your jacobian to your error.

[amdshameer]

Thank you so much @haudren @aescande

I calculated the gain matrix with trail and error. With 0.3 gain factor and maximum time step of 20 seconds, I was able to reach the target set point.

I have changed it to sva::rotationError. But, it works only if I use the default multi-body(base_link). It doesn't work if the base of the multibody is changed to left foot or right foot. When left foot is used as base, right foot is moving in +x axis (expected) and left foot is moving in -x axis (not expected). Left foot is not staying on the ground. I suspect the issue is with floating base. And I am not using floating base columns(0 to 5) in the full jacobian.

I am using "HRP4NoHand" robot module

float HRP4_quat[4], HRP4_position[3];
simGetObjectQuaternion(simGetObjectHandle("base_link_visual"),-1, HRP4_quat);
simGetObjectPosition(simGetObjectHandle("base_link_visual"),-1, HRP4_position);
Eigen::Quaterniond waist(HRP4_quat[3],HRP4_quat[0],HRP4_quat[1],HRP4_quat[2]);
// not using default_attitude. So that robot's world pose will be updated if robot is translated/rotated
hrp4.mbc().q[0] = {waist.w(),waist.x(),waist.y(),waist.z(),HRP4_position[0],HRP4_position[1],HRP4_position[2]}; 
// setting other joint values using simGetJointPosition
rbd::forwardKinematics(hrp4.mb(), hrp4.mbc());
rbd::forwardVelocity(hrp4.mb(), hrp4.mbc());

//setting left foot as base
auto bodyP = hrp4.mbc().bodyPosW[hrp4.mb().bodyIndexByName("L_ANKLE_R_LINK")];
Eigen::Quaterniond q(bodyP.rotation().transpose());
const auto & t = bodyP.translation();

auto hrp4_mb = hrp4.mbg().makeMultiBody("L_ANKLE_R_LINK", false);
auto hrp4_mbc = rbd::MultiBodyConfig(hrp4_mb);
rbd::ConfigConverter cc2(hrp4.mb(), hrp4_mb);
cc2.convert(hrp4.mbc(), hrp4_mbc);
hrp4_mbc.q[0] = {q.w(), q.x(), q.y(), q.z(), t.x(), t.y(), t.z()};

rbd::forwardKinematics(hrp4_mb, hrp4_mbc);
rbd::forwardVelocity(hrp4_mb, hrp4_mbc);

//jacobian between left foot(base) and right foot
rbd::Jacobian jac_RF(hrp4_mb, "R_ANKLE_R_LINK");
Eigen::MatrixXd J(6, hrp4_mb.nrDof()), J_no_flyer(6,30);
jac_RF.fullJacobian(hrp4_mb, jac_RF.jacobian(hrp4_mb, hrp4_mbc), J);
J_no_flyer = J.block(0,6,6,30); // removing floating base columns (0 to 5)
Eigen::VectorXd q_dot = pseudoInverse(J_no_flyer) * veclocityVector;

while (timeStep < maxTimeStep)
{
  q = q + (0.05*q_dot);
   // do the same
}

velocity is vector is calculated by following sva::rotationError as it was suggested in the previous comments

[aescande]

I would think the problem is that your robot in VREP does not have the left foot as a base. If the left foot was the base in VREP, then it would not move, and you would only see the right foot moving in +x direction.

Note that this is issue does not seem related to problems in RBDyn, and should probably be discussed elsewhere.

[amdshameer]

@aescande I calculate the kinematics and jacobians after loading the HRP-4 model in the VREP scene. The version I am using is hrp4_v1.7

• In the VREP, the model starts from "base_link".

• rbd::Jacobian::jointsPath between left foot and right foot

• List of joints in the MultiBody with L_ANKLE_R_LINK as base

[aescande]

So, that's what I wrote. You are computing joint positions and sending the results to VREP. The joints are moved correctly, and since you are not moving the base, the base does not move. You are getting exactly what you are asking for. If this is not what you want, you need to rethink what you are asking. Either you make a new VREP model with the left foot as base, or you keep this one and you compute the position of its root according to the results of your IK so that the foot stay at a fix position.

[amdshameer]

@aescande I forgot to mention that I am running the simulation in kinematics mode(using a VREP external plugin). Could it be the reason? Since there is no dynamics involved, there is no information which says that the left foot should be on the ground

[haudren]

@amdshameer : Are you only sending the joint angles to that plugin, or are you sending transformations? In the latter case, it is weird. In the former, you need to also send the base (as in origin of the VREP model) transformation.

[amdshameer]

My mistake was I didn't set the free-flyer in a proper way. I didn't do a transpose for "base_link" rotation matrix. Thanks to @haudren for pointing it out