WIXLIB

LAB 3Forward Kinematics3.1ObjectivesThe purpose of this lab is to compare the theoretical solution to the forwardkinematics problem with a physical implementation on the Rhino robot. Inthis lab you will: parameterize the Rhino following the Denavit-Hartenberg (DH) convention use Robotica to compute the forward kinematic equations for theRhino write a cpp function that moves the Rhino to a configuration specifiedby the user.From now on, labwork is closely tied to each arm’s differences.Pick a robot and stick with it for the remaining labs.3.2References Chapter 3 of SH&V provides details of the DH convention and itsuse in parameterizing robots and computing the forward kinematicequations. A matlab version code for translating DH parameters to forward HTmatrix is available online, ”Denavit Hartenberg Parameters” by Mahmoud KhoshGoftar.11

12LAB 3. FORWARD /fileexchange/44585-denavithartenberg-parameters) The complete Robotica manual is available in pdf form on the coursewebsite. Additionally, a “crash course” on the use of Robotica andMathematica is provided in Appendix A of this lab manual.3.33.3.1TasksPhysical ImplementationThe user will provide five joint angles {θ1 , θ2 , θ3 , θ4 , θ5 }, all given in degrees.Angles θ1 , θ2 , θ3 will be given between 180 and 180 , angle θ4 will begiven between 90 and 270 , and angle θ5 is unconstrained. The goal is totranslate the desired joint angles into the corresponding encoder counts foreach of the five joint motors. We need to write five mathematical expressionsencB (θ1 , θ2 , θ3 , θ4 , θ5 ) ?encC (θ1 , θ2 , θ3 , θ4 , θ5 ) ?encD (θ1 , θ2 , θ3 , θ4 , θ5 ) ?encE (θ1 , θ2 , θ3 , θ4 , θ5 ) ?encF (θ1 , θ2 , θ3 , θ4 , θ5 ) ?and translate them into cpp code (note that we do not need an expressionfor encoder A, the encoder for the gripper motor, the ros message commandstarts with encoder B). Once the encoder values have been found, we willcommand the Rhino to move to the corresponding configuration.3.3.2Theoretical SolutionFind the forward kinematic equations for the Rhino robot. In particular,we are interested only in the position d05 of the gripper and will ignore theorientation R50 . We will use Robotica to find expressions for each of thethree components of d05 .3.3.3ComparisonFor any provided set of joint angles {θ1 , θ2 , θ3 , θ4 , θ5 }, we want to comparethe position of the gripper after your cpp function has run to the positionof the gripper predicted by the forward kinematic equations.

3.4. PROCEDURE13Figure 3.1: Wrist z axes do not intersect.3.4Procedure3.4.1Physical Implementation1. Download and extract lab3.zip from the course website. Inside thispackage are a number of files to help complete the lab, very similar tothe one provided for lab 2.2. Before we proceed, we must define coordinate frames so each of thejoint angles make sense. For the sake of the TA’s sanity when helpingstudents in the lab, DH frames have already been assigned in Figure 3.3(on the last page of this lab assignment). On the figure of the Rhino,clearly label the joint angles {θ1 , θ2 , θ3 , θ4 , θ5 }, being careful that thesense of rotation of each angle is correct.Notice that the z3 and z4 axes do not intersect at the wrist, as shownin Figure 3.1. The offset between z3 and z4 requires the DH frames atthe wrist to be assigned in an unexpected way, as shown in Figure 3.3.Consequently, the zero configuration for the wrist is not what we wouldexpect: when the wrist angle θ4 0 , the wrist will form a right anglewith the arm. Please study Figure 3.3 carefully.3. Use the rulers provided to measure all the link lengths of the Rhino.Try to make all measurements accurate to at least the nearest halfcentimeter. Label these lengths on Figure 3.3.steps4. Now measure the ratio encoderjoint angle for each joint. Use the teach pendant to sweep each joint through a 90 angle and record the startingand ending encoder values for the corresponding motor. Be careful

14LAB 3. FORWARD KINEMATICSFigure 3.2: With the Rhino in the hard home position encoder D is zerowhile joint angle θ2 is nonzero.that the sign of each ratio corresponds to the sense of rotation of eachjoint angle.

3.4. PROCEDURE15For example, in order to measure the shoulder ratio, consider followingthese steps: Adjust motor D until the upper arm link is vertical. Record thevalue of encoder D at this position. Adjust motor D until the upper arm link is horizontal. Recordthe value of encoder D at this position. Refer to the figure of the Rhino robot and determine whetherangle θ2 swept 90 or 90 . Compute the ratio ratioD/2 encD (1) encD (0).θ2 (1) θ2 (0)We are almost ready to write an expression for the motor D encoder,but one thing remains to be measured. Recall that all encoders are setto 0 when the Rhino is in the hard home configuration. However, inthe hard home position not all joint angles are zero, as illustrated inFigure 3.2. It is tempting to write encD (θ2 ) ratioD/2 θ2 but it is easyto see that this expression is incorrect. If we were to specify the jointangle θ2 0, the expression would return encD 0. Unfortunately,setting encoder D to zero will put the upper arm in its hard homeposition. Look back at the figure of the Rhino with the DH framesattached. When θ2 0 we want the upper arm to be horizontal. Wemust account for the angular offset at hardhome.

16LAB 3. FORWARD KINEMATICS5. Use the provided protractors to measure the joint angles when theRhino is in the hard home position. We will call these the joint offsetsand identify them as θi0 . Now we are prepared to write an expressionfor the motor D encoder in the following form:encD (θ2 ) ratioD/2 (θ2 θ20 ) ratioD/2 θ2 .Now, if we were to specify θ2 0, encoder D will be set to a valuethat will move the upper arm to the horizontal position, which agreeswith our choice of DH frames.6. Derive expressions for the remaining encoders. You will quickly noticethat this is complicated by the fact that the elbow and wrist motorsare located on the shoulder link. Due to transmission across the shoulder joint, the joint angle at the elbow is affected by the elbow motorand the shoulder motor. Similarly, the joint angle at the wrist is affected by the wrist, elbow, and shoulder motors. Consequently, theexpressions for the elbow and wrist encoders will be functions of morethan one joint angle.It is helpful to notice the trasmission gears mounted on the shoulderand elbow joints. These gears transmit the motion of the elbow andwrist motors to their respective joints. Notice that the diameter of thetransmission gears is the same. This implies that the change in theshoulder joint angle causes a change in the elbow and wrist joint anglesof equal magnitude. Mathematically, this means that θ3 is equal tothe change in the shoulder angle added or subtracted from the elbowangle. That is, θ3 ((θ3 θ30 ) (θ2 θ20 )).A similar expression holds for θ4 . It is up to you to determine the sign in these expressions.3.4.2Theoretical Solution1. Construct a table of DH parameters for the Rhino robot. Use the DHframes already established in Figure 3.3.2. Write a Robotica source file containing the Rhino DH parameters.Consult Appendix A of this lab manual for assistance with Robotica.

3.5. REPORT173. Write a Mathematica file that finds and properly displays the forwardkinematic equation for the Rhino. Consult Appendix A of this labmanual for assistance with Mathematica.4. OR Use matlab ”Denavit Hartenberg Parameters” to calculate theforward kinematics. Use simplify() in matlab as you see fit.3.4.3Comparison1. The TA will supply two sets of joint angles {θ1 , θ2 , θ3 , θ4 , θ5 } for thedemonstration. Record these values for later analysis.2. Run the executable file and move the Rhino to each of these configurations. Measure the x,y,z position vector of the center of the gripperfor each set of angles. Call these vectors r1 and r2 for the first andsecond sets of joint angles, respectively.3. In Mathematica OR Matlab, specify values for the joint variables thatcorrespond to both sets of angles used for the Rhino. Note the vectord05 (θ1 , θ2 , θ3 , θ4 , θ5 ) for each set of angles. Call these vectors d1 and d2 .(Note that Mathematica expects angles to be in radians, but you caneasily convert from radians to degrees by adding the word Degree aftera value in degrees. For example, 90 Degree is equivalent to π2 .)4. For each set of joint angles, Calculate the error between the two forward kinematic solutions. We will consider the error to be the magnitude of the distance between the measured center of the gripper andthe location predicted by the kinematic equation:error1 kr1 d1 k q(r1x d1x )2 (r1y d1y )2 (r1z d1z )2 .A similar expression holds for error2 . Because the forward kinematicscode will be used to complete labs 4 and 6, we want the error to be assmall as possible. Tweak your code until the error is minimized.3.5ReportAssemble the following items in the order shown.1. Coversheet containing your names, “Lab 3”, and the weekday and timeyour lab section meets (for example, “Tuesday, 3pm”).

18LAB 3. FORWARD KINEMATICS2. A figure of the Rhino robot with DH frames assigned, all joint variablesand link lengths shown, and a complete table of DH parameters.3. A clean derivation of the expressions for each encoder. Please sketchfigures where helpful and draw boxes around the final expressions.4. The forward kinematic equation for the tool position only of the Rhinorobot. Robotica and the matlab ”DH parameters” will generate theentire homogenous transformation between the base and tool frames"T50 (θ1 , θ2 , θ3 , θ4 , θ5 ) R50 (θ1 , θ2 , θ3 , θ4 , θ5 )000d05 (θ1 , θ2 , θ3 , θ4 , θ5 )1#but you only need to show the position of the tool frame with respectto the base framed05 (θ1 , θ2 , θ3 , θ4 , θ5 ) [vector expression].5. For each of the two sets of joint variables you tested, provide thefollowing: the set of values, {θ1 , θ2 , θ3 , θ4 , θ5 } the measured position of the tool frame, ri the predicted position of the tool frame, di the (scalar) error between the measured and expected positions,errori .6. A brief paragraph (2-4 sentences) explaining the sources of error andhow one might go about reducing the error.3.6DemoYour TA will require you to run your program twice, each time with adifferent set of joint variables.3.7GradingGrades are out of 3. Each successful demo will be graded pass/fail with apossible score of 1. The remaining point will come from the lab report.

3.7. GRADINGFigure 3.3: Rhino robot with DH frames assigned.19

20LAB 3. FORWARD KINEMATICS

LAB 4Inverse Kinematics4.1ObjectivesThe purpose of this lab is to derive and implement a solution to the inversekinematics problem for the Rhino robot, a five degree of freedom (DOF)arm without a spherical wrist. In this lab we will: derive the elbow-up inverse kinematic equations for the Rhino write a cpp function that moves the Rhino to a point in space specifiedby the user.4.2ReferenceChapter 3 of SH&V provides multiple examples of inverse kinematics solutions.4.34.3.1TasksSolution DerivationGiven a desired point in space (x, y, z) and orientation {θpitch , θroll }, writefive mathematical expressions that yield values for each of the joint variables.For the Rhino robot, there are (in general) four solutions to the inversekinematics problem. We will implement only one of the elbow-up solutions.21

22LAB 4. INVERSE KINEMATICS In the inverse kinematics problems we have examined in class (for 6DOF arms with spherical wrists), usually the first step is to solve forthe coordinates of the wrist center. Next we would solve the inverseposition problem for the first three joint variables.Unfortunately, the Rhino robots in our lab have only 5 DOF and nospherical wrists. Since all possible positions and orientations requirea manipulator with 6 DOF, our robots cannot achieve all possibleorientations in their workspaces. To make matters more complicated,the axes of the Rhino’s wrist do not intersect, as do the axes in thespherical wrist. So we do not have the tidy spherical wrist inversekinematics as we have studied in class. We will solve for the jointvariables in an order that may not be immediately obvious, but is aconsequence of the degrees of freedom and wrist construction of theRhino. We will solve the inverse kinematics problem in the following order:1. θ5 , which is dependent on the desired orientation only2. θ1 , which is dependent on the desired position only3. the wrist center point, which is dependent on the desired positionand orientation and the waist angle θ14. θ2 and θ3 , which are dependent on the wrist center point5. θ4 , which is dependent on the desired orientation and arm anglesθ2 and θ3 . The orientation problem is simplified in the following way: instead ofsupplying an arbitrary rotation matrix defining the desired orientation,the user will specify θpitch and θroll , the desired wrist pitch and rollangles. Angle θpitch will be measured with respect to the zw , the axisnormal to the surface of the table, as shown in Figure 4.1. The pitchangle will obey the following rules:1. 90 θpitch 270 2. θpitch 0 corresponds to the gripper pointed down3. θpitch 180 corresponds to the gripper pointed up4. 0 θpitch 180 corresponds to the gripper pointed away fromthe base5. θpitch 0 and θpitch 180 corresponds to the gripper pointedtoward the base.

4.4. PROCEDURE23Figure 4.1: θpitch is the angle of the gripper measured from the axis normalto the table.4.3.2ImplementationImplement the inverse kinematics solution by writing a cpp function to receive world frame coordinates (xw , yw , zw ), compute the desired joint variables {θ1 , θ2 , θ3 , θ4 , θ5 }, and command the Rhino to move to that configuration using the lab3.cpp function written for the previous lab.4.4Procedure1. Download and extract lab4.zip from the course website. Inside thispackage are a number of files to help complete the lab. You will alsoneed to copy functions implemented in lab3.cpp file which you wrotefor the previous lab to current lab4.cpp.2. Establish the world coordinate frame (frame w) centered at the cornerof the Rhino’s base shown in Figure 4.2. The xw and yw plane shouldcorrespond to the surface of the table, with the xw axis parallel tothe sides of the table and the yw axis parallel to the front and backedges of the table. Axis zw should be normal to the table surface,with up being the positive zw direction and the surface of the tablecorresponding to zw 0.

24LAB 4. INVERSE KINEMATICSFigure 4.2: Correct location and orientation of the world frame.We will solve the inverse kinematics problem in the base frame (frame0), so we will immediately convert the coordinates entered by the userto base frame coordinates. Write three equations relating coordinates(xw , yw , zw ) in the world frame to coordinates (x0 , y0 , z0 ) in the baseframe of the Rhino.x0 (xw , yw , zw ) y0 (xw , yw , zw ) z0 (xw , yw , zw ) Be careful to reference the location of frame 0 as your team defined itin lab 3, as we will be using the functions copied from lab3.cpp fileyou created.3. The wrist roll angle θroll has no bearing on our inverse kinematicssolution, therefore we can immediately write our first equation:θ5 θroll .(4.1)4. Given the desired position of the gripper (x0 , y0 , z0 ) (in the base frame),write an expression for the waist angle of the robot. It will be helpfulto project the arm onto the x0 y0 plane.θ1 (x0 , y0 , z0 ) (4.2)

4.4. PROCEDURE25Figure 4.3: Diagram of the wrist showing the relationship between the wristcenter point (xwrist , ywrist , zwrist ) and the desired tool position (x0 , y0 , z0 )(in the base frame). Notice that the wrist center and the tip of the tool arenot both on the line defined by θpitch .5. Given the desired position of the gripper (x, y, z)0 (in the base frame),desired pitch of the wrist θpitch , and the waist angle θ1 , solve for thecoordinates of the wrist center. Figure 4.3 illustrates the geometry atthe wrist that involves all five of these parameters.xwrist (x0 , y0 , z0 , θpitch , θ1 ) ywrist (x0 , y0 , z0 , θpitch , θ1 ) zwrist (x0 , y0 , z0 , θpitch , θ1 ) Remember to account for the offset between wrist axes z3 and z4 , asshown in Figure 4.3.6. Write expressions for θ2 and θ3 in terms of the wrist center positionθ2 (xwrist , ywrist , zwrist ) (4.3)θ3 (xwrist , ywrist , zwrist ) (4.4)as we have done in class and numerous times in homework.7. Only one joint variable remains to be defined, θ4 . Note: θ4 6 θpitch(see if y

Read "A Gentle Introduction to ROS", available online, Speci cally: Chapter 2: 2.4 Packages, 2.5 The Master, 2.6 Nodes, 2.7.2 Messages and message types. Chapter 3 Writing ROS programs. 2.3 References Consult Appendix B of this lab manual for details of ROS and cpp functions used to control the Rhino. "A Ge