Lab 1 RVIZ and PYTHON with Simple Robot Manipulator Model

Transcription

1 Lab 1 RVIZ and PYTHON with Simple Robot Manipulator Model In this Lab we will learn how to use the RVIZ Robot Simulator and convert Quaternions to/from axis angle representation. We will use the Python programming language to make our manual calculations and confirm ROS is working as expected. Python is an interpreted language and supported by the Robotic Operation System (ROS). ROS Python and tf Lab Setup There are four sections to this lab: a) Setup and RVIZ features b) Converting from Quaternion to Axis Angle Representation c) Frames of Reference d) Robot Arm Movement Sequence Setup will guide you through startup of RVIZ, Robot State Publisher and terminals. Features of the RVIZ display will be explored. We will convert Quaternions to Axis Angle representation to get useful information from the Quaternion. We will also change the Frames of Reference in the chain of links to take different measurements. Finally we will explore Sequences of moving arm joints to set the robot to different positions in 3D space. L1.1 Setup and RVIZ Features The initial screen after the login and password have been entered is shown. Note to start a terminal the next step double left click on the circled terminal icon on the lower left hand side of the screen.

2 Commands typed on the terminal window shown below can be edited with the left and right arrows. The up and down arrows in the terminal window are used to recall previous commands. The roslaunch command is used to start RVIZ, the Robot Publisher node using the specified URDF model. This command will tie up the terminal window. Right clicking on the terminal will give a option for a new terminal window. Type the command as shown in the above screen shot. If you are having trouble reading the text on the screen shot type in the following: roslaunch urdf_tutorial display.launch model:=urdf/armbojects.urdf gui:=true CopyRight 2015 by William Lehman All Rights Reserved. Page 2

3 The initial RVIZ window displays the Robot model and panel of options for the display. The State Publisher is also displayed by left clicking on the lower left of the screen where the icon is circled. We have noticed that RVIZ sometimes comes up with a blank display for the robot. If this happens simple control c out of the command in the RVIZ terminal and up arrow enter to run the command again. Moving the sliders sets the robots joints. The visual part of the link can be turned off by clicking on the enable for the link shown in the screen below. The three joints between the Base Link and Link1, Link1 and Link2, and finally between Link2 and Link3 form a kinematic chain. The joint between the Base Link and Link1 rotate the arm around the Base. The other two joints move the associated Links up and down. The Fixed Frame in the above screen shot above is set to the Base Link. Using the Base Link as the frame of reference gives a Kinematic Chain shown in the following diagram. Joint between Base Link and Link1 Joint between Link1 and Link2 Joint between Link2 and Link3 CopyRight 2015 by William Lehman All Rights Reserved. Page 3

4 With the link graphics disabled, the axis can be seen to rotate with the associated slider. Does the axis system when it rotates follow the right hand rule? The values of the Quaternions with each of the assoicated links can also be displayed with RVIZ. CopyRight 2015 by William Lehman All Rights Reserved. Page 4

5 We can also display the relationships of the links to one another under the tf panel in RVIZ. Relative position is with respect to the link origin. Relative orientation is rotation with respect to the link s origin. The Position is the position with respect to the link selected in the Fixed Frame field which is the Base Link by default. The Orientation is the rotation of the link with respect to the Fixed Frame field axis system. Parts or all visual elements can also be turned off to display the Frame axis system as shown in the next screen shot. CopyRight 2015 by William Lehman All Rights Reserved. Page 5

6 CopyRight 2015 by William Lehman All Rights Reserved. Page 6

7 L1.2 Converting from Quaternion to Axis Angle Representation Perform conversion from Quaternion for a one of the links using the Orientation field. Make sure to set the sliders to the position given in the next screen shot. To see the terminal window, click on the terminal icon on the left hand icon panel to bring the terminal window to the foreground. Python can then be brought up by typing python at the terminal prompt. We will be working with Axis angle representation in this section of the lab. Axis angle is somewhat intuitive and similar to quaternions but in 3D. A rotation about the x, y and z axis can also be represented by a unit vector and a single rotation about that unit vector. The following diagram shows a unit vector and a single rotation about the unit vector. A unit vector has the property x 2 + y 2 + z 2 = 1. CopyRight 2015 by William Lehman All Rights Reserved. Page 7

8 Axis angle can be converted to/from Euler angles to and from Quaternions. Equations Eq. 20 through Eq. 30 convert the axis angle vector to a Quaternion. Eq. 1 q1 = sin ( α 2 ) cos( β x) Eq. 2 q2 = sin ( α 2 ) cos( β y) Eq. 3 q3 = sin ( α 2 ) cos( β z) Eq. 4 q4 = cos ( α ) 2 The Quaternion is normalized so equation Eq. 5 is applies. Eq. 5 q1 2 + q2 2 + q3 2 + q4 2 = 1 Equations [6] through [9] convert a Quaternion to axis angle vector. Eq. 6 = 2 cos 1 (q4) Eq. 7 x = Eq. 8 y = q1 1 q4 2 q2 1 q4 2 Eq. 9 z = q3 1 q4 2 CopyRight 2015 by William Lehman All Rights Reserved. Page 8

9 The direction cosine angles can be found with the following equations: Eq. 10 x = cos 1 x Eq. 11 y = cos 1 y Eq. 12 z = cos 1 z EXAMPLE Given the Quaternion: q1 = , q2 = , q3 = , q4 = Note the Quaternion is normalized: 1 = q1 2 + q2 2 + q3 2 + q4 2 = a = = 2 cos 1 (q4) = = x = , y = , z = Note that x 2 + y 2 + z 2 = 1 and is thus normalized. x = cos 1 x = 46.5 y = cos 1 y = 77.4 z = cos 1 z = You can use the following screen shots of the terminal window and the above equations as a guide. The comments in the screen shot showing the terminal window performs the above calculations for quaternions in the links also shown in the screen shot. The lines in the terminal screen shots starting with a # character need not be typed, since they are comments to explain the calculations. CopyRight 2015 by William Lehman All Rights Reserved. Page 9

10 Ensure the sliders are still at settings seen in above screen shot and type the following: If you are having trouble seeing the text on the above screen shot, type in the following: python import rospy

11 import tf alpha = 2.0 * math.acos(1) print alpha alpha = 2.0 * math.acos( ) alphad = * alpha / math.pi print alphad alpha = 2.0 * math.acos( ) print alpha, * alpha / math.pi The equations for Quaternion to Axis Angle and Axis Angle to Quaternion were also given in the pre-lab materials. Use python to perform similar calculations for the following screen shot. Remember that you can use the up, down, left and right arrows to save typing in the terminal window. If you are having trouble seeing the text in the terminal window type the following: q1 = q2 = 0 q3 = 0

12 q4 = x = q1 / math.sqrt(1.0 q4 * q4) y = 0.0 z = 0.0 betax = x / (x * x + y * y + z * z) betay = 0.0 betaz = 0.0 print betax Convert the Quaternion in the following table to Axis Angle. Find the Angle in both Radians and Degrees. Remember that to convert Radians to degrees multiple the Angle in Radians by Degrees and then divide by PI.

13 Orientation Q1/x Q2/y Q3/z Q4/Angle Quaternion Link1 (Q1-Q4) Axis/Angle (x,y,z,angle) L1.3 Frames of Reference Do a control c in the terminal window for RVIS and use the up arrow to display the last command again. Edit with left and right arrow keys so we load the urdf/arm.urdf this time. This will display the robot arm without a object to the side of it and follow the directions below. We may not be interested in knowing the position relative to the Robots base link. In the following screen shot Link 2 is used as the reference point. The Grid Reference Frame was set to the Base Link, since we want the Grid along the floor. I Note in the next screen shot where Link2 as the reference point, Link2 now has a position of zero. The Base Link is -1 along the z axis and Link3 is 1 along the z axis.

14 Using Link2 as the Fixed Frame (Frame of Reference) gives us a Kinematic Tree shown in the following diagram. Note that every link is connected through a joint, so we can also express the tree as parent links and child links. Joint between Base Link and Link1 Joint between Link2 and Link3 Joint between Link1 and Link2 When we look at the position of the links they are in relation to the Link2 as the origin of Link2. Move the sliders and note how the position of Base, Link1, Link2 and Link3 change in relation to one another. How is this similar and different to the previous Kinematic Chain?

15 The following screen shots show the Link positions in relation to Link2 as the origin of the axis system. Link2 is the origin of the axis system in the next screen shot but we have set joints to non-zero values.

16 Draw a diagram for the Kinematic Chain when the Fixed Field is set to Link3. How do the values change for position and orientation when the Fixed Frame is changed to link3? L1.4 Robot Arm Movement Sequence RVIS display below shows the position for link3 as , , The Joint State Publisher window shows the joint rotations that resulted in that position. List different sequences the robot arm could have been moved to archive this position given an initial state of all zero joint angles and the Fixed Frame set to the Base Link. Try to set robot arm to position 0, 0, 0 or as close as you can with the Fixed Frame set to Object1. Control c out of RVIZ program in terminal window and re-run with urdf/arm6joints.urdf. Hint is to use your mouse to see different views of the robot and object t and change use the Fixed Frame field with different values. Make sure to set the Grid to the Base Link. You can also use the mouse to increase size of sliders for accuracy. Sequences should be limited to 3 or 4 joint rotation commands with others set to zero. Use the following table to list the sequence of movement: Only one joint may be commanded to a new angle for each command in the sequence. Sequence # Command 1 Joint 1 Base Link1 2 Angle (RAD) Command 2 Joint Link4 Link5 Angle (RAD) Command 3 Joint 1.05 Link1 Link2 Angle (RAD) Command 4 Joint Link3 Link4 Angle (RAD) ,

17 Screen shot of sequence 1 follows.