Lab 2A Finding Position and Interpolation with Quaternions In this Lab we will learn how to use the RVIZ Robot Simulator, Python Programming Interpreter and ROS tf library to study Quaternion math. There are three sections to this lab: a Basic Operations b Calculating Position c Interpolation of Quaternions and SLERP For all sections of the lab a-b type the follow command into a terminal window: roslaunch urdf_tutorial display.launch model:=urdf/joint6.urdf gui:=true The above command can also be copied and pasted into the terminal window to save some typing. L2A.1 Basic Operations In the previous labs conversion of quaternions, have converted quaternions to and from Euler angles as well as quaternions to and from axis angles. We will need some additional operations to compute position from quaternions, specifically normalizing a quaternion, finding the conjugate of a quaternion and the inverse of a quaternion. Finally, another example of quaternion multiplications will be presented. Normalizing/Inverse/Conjugate To normalize a quaternion we multiply q1 through q4 by the sum of the squares to produce values that range from 0 to 1.0. 1 * [q1, q2, q3,q4] q1 +q2 + q32 +q4 2 2 2 Quaternions in the RVIZ screen shots used in this lab are already normalized so: 1= q12 +q2 2+ q32 +q42 The conjugate of a normalized quaternion is the same as the inverse or: [q1, -q2, -q3, -q4] Page 1
We only need to negate each of the three vector components to get the conjugate of the normalized quaternion. Multiplying Page 2
In robot in the above screen shot is more complex than robots previously introduced. The Unified Robot Description Format (URDF) defined a robot with twists in the frame about the between link 3 and link 4. The joint definition in the URDF file for the robot is shown below: <joint name="up_down_movement" type="revolute"> <parent link="link3"/> <child link="link4"/> <axis xyz="1 0 0" /> <origin rpy="-1.5708 0-0.05258" xyz="0.02 0.0 0.0"/> <limit upper="0.95" lower ="-0.35" effort="1.0" velocity="1.0"/> </joint> The origin for link 4 has a roll pitch yaw of -1.5708 radians 0-0.05258. The roll of -1.5708 radians is about the x axis using the right hand rule. The yaw of -0.05258 radians is about the z axis. The roll converts from radians to -90. The yaw converts from radians to -3. Since the up_down_movement slider for the joint is set to zero the only rotation between link3 and link 4 with respect to link 3, is due to the twist in the frame. We can use the following code similar to what we saw in Lab 2: r = -1.5708 p = 0.0 y = -0.05258 q = tf.transformations.quaternion_from_euler(r, p, y, sxyz ) [-0.70686373 0.01858773-0.01858766 0.70686113] There are some roundoff errors in the above quaternion from link 4 orientation due to roundoff errors on the slider display. Page 3
Multiplication of quaternions represent rotations in 3D space. If we multiply the orientation of Link 3 by the relative orientation of Link4 we obtain the orientation of Link 4. rel_orientation4 = [-0.50146, 0.013186, -0.02274, 0.86478] orientation3 = [0, 0, 0.15837, 0.98738] orientation4 = tf.transformations.quaternion_multiply(orientation3, rel_orientation4) [-0.49721984-0.06639663 0.11450219 0.85746781] We can get the euler angles from the quaternion: print tf.transformations.euler_from_quaternion(orientation4) (-1.0509730495220082, -4.226410320329458e-07, 0.2655000992653402) Converting from radians we get a roll of -60, pitch of 0 and yaw of 15. Page 4
L2A.2 Calculating Position In ROS position between each frame of a kinematic chain is found using the tf Library. Using the six joint robot arm in the RVIZ screen shot below we can display what the position of each link for the robot. The tf Library calculates the position taking into account the quaternion values for each revolute joint in the robot. Since joint quaternions can be generated at different data rates tf extrapolates the joint values given the time received. In the example below the joint angles were constant so the extrapolation of joint values can be ignored and the position between frames can be calculated using following equation: n = q p q* Where n is the position of the next link in the Kinematic chain, p is the position of the previous link in the chain, q is the quaternion for the revolute joint, and q* is the quaternion conjugate of q. Note that the equation multiplies a position by a quaternion. The position is a 3 dimensional vector and a quaternion is composed of a 3 dimensional vector in imaginary space and an associated scaler value. To multiply the position by the quaternion we add a zero to the position to make it a 4 vector like the quaternion (x, y,z,0). The position is not normalized in the calculation. python import tf import math p3 = [0, 0.5, 0] Page 5
q = [0.76942, 0.25, 0.18164, 0.55902] qc = tf.transformations.quaternion_conjugate(q) qp = tf.transformations.quaternion_multiply(q,p) p4 = tf.transformations.quaternion_multiply(qp,qc) print p4 [ 0.27951245-0.38471117-0.15450534 0. ] pnext = p4 + p3 print pnext [ 0.45226245-0.62247117 2.24999466 0. ] Page 6
L2A.3 Interpolation of Quaternions and SLERP We can represent a rotation with two quaternions multiplying one another. But sometimes we want to find the intermediate orientations or the quaternions along the arc of the rotation. Spherical Interpolation is one method of finding intermediate orientations along the arc of rotation. SLERP is very useful for displaying a robots motion and computer graphics in general. We obtain from the above screen shot the orientation of the aircraft for yaw of [0,0,0,1]. The next screen shot below gives us a orientation for yaw of [0,0,0.70711,0.70711]. We did a 90 rotation to get from [0,0,0,1] to [0,0,0.70711,0.70711]. But we want to find the orientations or quaternions between these two orientations. Page 7
The screen shot below shows the orientation of the aircraft at 45 degrees for half way through the 90 rotation. Page 8
To find the orientation ½ way through the 90 rotation using slerp type in the following code in a terminal window: python import tf p1 = [0,0,0,1] p3 = [0,0, 0.70711, 0.70711] p2 = tf.transformations.quaternion_slerp(p1,p3,0.5) print p2 You should then see displayed on the terminal the value you see in the above screen shot for orientation of yaw with small roundoff errors. Find the orientation at 10 and 75 for the aircraft. Page 9