Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

How to apply an extrinsic rotation to a link, programmatically?

I have models written in SDF that I want to rotate at specific extrinsic angles before starting a Gazebo simulation with them. I have the roll, pitch and yaw of the pose of the link, I have the extrinsic roll, pitch and yaw of the desired rotation, and I want to find out the formula for obtaining the roll, pitch and yaw of the link, after the rotation, so that I can update the pose values in the SDF file.

I couldn't find a source for the Euler angle convention that Gazebo uses, but I can tell that it is using intrinsic angles because when I rotate a link on the X axis by changing its roll pose number, it rotates around the extrinsic X axis, but if I then change the pitch pose number, it doesn't rotate around the extrinsic Y axis but its own, intrinsic, Y axis.

I'll greatly appreciate any help, thank you!

Edit: I've managed to find out about a widely used transformations library and I've managed to write the following code:

Re = euler_matrix(arr[3]-np.pi, arr[4], arr[5], 'rxyz')
Rx = rotation_matrix(r_arr[0], [1, 0, 0])
Ry = rotation_matrix(r_arr[1], [0, 1, 0])
Rz = rotation_matrix(r_arr[2], [0, 0, 1])
M  = concatenate_matrices(Re, Rx, Ry, Rz)
euler = euler_from_matrix(M, 'rxyz')
arr[3] = euler[0]
arr[4] = euler[1]
arr[5] = euler[2]

where r_arr contains the desired extrinsic rotations and arr[3:6] contains the current intrinsic rotations.

Here is the result: hand rotated 90 degrees on the Y axis

The hand has been rotated 90 degrees on the Y axis. The little finger didn't come out right, I think this is a numerical inaccuracy, the links of the little finger being the only ones with negative pitch from all the other fingers. If there is a better way, I'm open to suggestions.

I'm also attaching the hand SDF file.