Rotation Matrix VREP and RBDyn (SVA)

[amdshameer]

I am having troubles in creating rotation matrix from VREP in accordance with SVA. I am using HRP4 in VREP simulator. I suspect the trouble is due to the reason that: In SVA, sin components of the rotation matrix(Rx, Ry, Rz) has "-"(minus) sign with it.

Rotation matrix from:(SVA) hrp4.mbc().bodyPosW[hrp4.mb().bodyIndexByName("r_wrist")].rotation() ---> (1)

0.905646 0.0867599 0.415063 -0.0150835 0.984816 -0.172943 -0.423765 0.150365 0.893204

Rotation Matrix from VREP for R_WRIST_R: simGetObjectQuaternion(simGetObjectHandle("R_WRIST_R"), -1,quat); // x,y,z,w Eigen::Quaterniond eigen_quat(quat[3],quat[0],quat[1],quat[2]); // w,x,y,z eigen_quat.matrix(); // produces the below matrix ----> (2) 0.423763 -0.0151001 0.905647 -0.150361 0.984815 0.0867758 -0.893206 -0.172947 0.415058

(1) is almost ~same as (2), but few rows and columns in (2) are interchanged. I tried to perform rotations in different RPY order along different axis. But still couldn't make (1) = (2). Inverting (2) didn't help as well. Is there any way to handle this? I am not able to do tasks in VREP, as there is a mismatch between the VREP rotation and SVA rotation.

[aescande]

Not an explanation to what is happening, but a remark: you have (1) * (2) = P, where P is

 0 0 1
 0 1 0
-1 0 0

so that (1) = P (2)^T and (2) = (1)^T P

If this is consistent for other matrices, you can use this as a fix.

[haudren]

To complete @aescande suggestion, note that this is basically a -pi/2 rotation along the Y axis, i.e:

(1) * (2) = sva::RotY(-M_PI/2)

[amdshameer]

@aescande That fix worked and it is consistent for all the matrices. Thank you so much

@haudren Yes, (1) * (2) is equal to sva::RotY(-M_PI/2). But, I couldn't understand why is it equal to sva::RotY(-M_PI/2). Would it be possible to get a brief explanation for that?

[ahundt]

http://github.com/ahundt/grl has many of the conversions and apis you might want