Gazebo: Q&A Forum - RSS feedhttp://answers.gazebosim.org/questions/Open source question and answer forum for GazeboenCopyright Askbot, 2010-2011.Sun, 21 Apr 2019 09:23:17 -0500How to apply an extrinsic rotation to a link, programmatically?http://answers.gazebosim.org/question/22339/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](https://www.lfd.uci.edu/~gohlke/code/transformations.py.html) 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](/upfiles/15558744315746221.png)
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](/upfiles/15558746353024074.world).altvaliSun, 21 Apr 2019 09:23:17 -0500http://answers.gazebosim.org/question/22339/