Homework Assignment 4 600.445/645 Fall 2018 Instructions and Score Sheet (hand in with answers Name Email Other contact information (optional Signature (required I have followed the rules in completing this assignment Name Email Other contact information (optional Signature (required I have followed the rules in completing this assignment Please Indicate whether you are in 600.455 or 600.655 Remember to put your name(s on every sheet you hand in (if you hand in via Gracope this will be automatic. Note that the UPPER limit for the total score used to calculate your course gra will be 100. I have provided a quite a few extra credit points. The intent here is to give you some choice on possibly skipping several questions. Question Points (455 Points (655 Points Total 1 25 25 2 5 5 3 15 15 4 15 15 5 15 15 6 10 10 7 10 10 8 15 15 9 10 10 10 10 10 11 10 10 12 10 10 Total 150 150
1. Remember that this is a graded homework assignment. 2. You are to work alone or in a team of two people and are not to discuss the problems with anyone other than the TAs or the instructor. 3. Put your names and email address on each sheet and number the sheets. 4. You are encouraged to make free use of any published materials, the web, etc. in developing your answer but a you must give full and proper citations to any references consulted and b you may not consult, discuss, or otherwise communicate about this assignment with any human being except your lab partner, the course instructor, or the TAs. The one exception is that you should not refer to previous years homework. 5. Please refer to the course organizational notes for a fuller listing of all the rules. I am not reciting them all here, but they are still in effect. 6. Unless I say otherwise in class, it is due before the start of class on the due date posted on the web. 7. Sign and hand in the score sheet as the first sheet of your assignment 8. Remember to include a sealable 8 ½ by 11 inch self-addressed envelope if you want your assignment. 9. Attach the grade sheet as the first sheet and attach all sheets together. 10. You must include a self-addressed, seal-able 8 ½ x 11 inch envelope if you expect to the homework to be returned (per JHU s interpretation of FERPA.
Scenario 1 Calibration problem (Questions 1-4 F PE ( q 456 [R( q 456, p PE ] F BP ( q 123 [I, p BP ( q 123 ] F TC FEC Tracking system F T A Robot Controller Consider the robot calibration problem shown in Figure 1. The six degree-of-freedom robot consists of a parallel delta stage providing three cartesian degrees of freedom to a platform carrying an extended arm and three degree-of-freedom revolute wrist. The pose of the platform, relative to the base of the robot is nominally given by F BP ( q 123 = [I, p BP ( q 123 ], where q 123 = [q 1,q 2,q 3 ] T are the linear displacements of the parallel actuators. Similarly, the pose of the robot end-effector relative to the platform is given by F ( PE q = [R ( 456 q,, where PE 456 p PE ] q 4,5,6 = [q 4,q 5,q 6 ] T are actuator values for the revolute wrist. The nominal forward kinematics for the robot is: Figure 1: Calibration setup = F BP ( q 123 F PE ( q 123 = [R PE ( q 456, p PE + p BP ( q 456 ] where q=[ q T 123, T q 456 ] T For the purposes of this assignment, you can assume that the displacement is known exactly. However, there may be some uncertainty in the accuracy of the delta stage and revolute wrist. The total forward kinematics for the robot (including errors is thus: p PE where Δ ( q ( q = ΔF BP ( q 123 F BP ( q 123 F PE ( q 123 ΔF PE ( q 123 ΔF BP ( q 123 [I+ α BP ( q 123, BP ( q 123 ] ΔF PE ( q 456 [I+ α BE ( q 456, PE ( q 456 ]
Note that the origin of the robot s coordinate system is somewhat arbitrary, and we will take it at the pose of the robot end effector when q = 0. Thus, F BP ( 0 = [I, p PE ]. Due to its construction, the robot s kinematic errors are non-negligible, but change fairly smoothly over the range of the robot. You can assume that there is suitable software to solve both the forward kinematics problem (compute ( q from q and inverse kinematics problem (compute q to produce a ired. Also, you can assume that the robot controller software can command the robot to place the end effector at a commanded pose. However, the motion is not instantaneous; some time is required for the robot to move to the ired commanded set point and settle down into the commanded position. If multiple set points are commanded one after the other, the robot will move in a more-or-less continuous trajectory, but it may lag behind the last commanded position. The robot controller is also capable of commanding the robot joints to move at specified velocities q ", although the acceleration is not instantaneous. The robot controller runs at a fixed sample interval ΔT with the following approximate run-time loop: Step 0: Wait for the next ΔT interval; update an internal clock t robot t robot + ΔT. Step 1: Read joint values and any other sensor values needed for control Step 2: Compute and report the robot state to higher-level processes Step 3: Based on the commanded motion, compute a new commanded set of joint positions q or velocities "qcmd cmd. Step 4: Output and/or "qcmd to a lower level joint controller. Step 5: Go back to Step 0 Typical values for ΔT might be somewhere between 2 ms and 20 ms. For this exercise, you can assume 3 ms. A higher level process in the robot controller is capable of querying the most recent values of all robot state variables (,, "q q, etc. together with their corresponding clock time t robot q cmd. This higher level process is also capable of specifying robot position setpoints, virtual fixture constraints, and other motion commands. The calibration problem will require us to find a way of estimating Δ as a function of q. Alternatively, we can look for a way to estimate ΔF as a function of a nominal pose. BE * I.e., if the robot winds up at = Δ F when commanded to go to using the nominal BE kinematic equations, find. For this purpose, an optical tracking system is available, together with two tracking fiducial objects. One of these is placed on the base of the robot at some unknown position. The second is attached to the end effector of the robot at a precisely known pose relative to the robot. The tracking system is attached to a different computer than the F EC robot controller, although they can communicate through some sort of fast serial link. The tracking computer observes the tracking fiducials at a fixed sample rate of 30 Hz and saves the most recent values. At any given time, the robot control computer can request a frame of information from the tracking system, consisting of three values [t,f TA (t,f TC (t], where F TA (t is the pose relative to the tracker of the tracker fiducial body attached to the robot base and F TC (t is the pose relative to the tracker of the tracker fiducial attached to the end effector. The time stamp t is expressed in terms of the tracking system s internal clock, which may not agree with
the clock in the robot control computer. One consequence of this is that the pose values returned by the tracking system may not correspond exactly with the pose values computed by the robot controller if the robot is moving. The measurements of the tracking system may be assumed to have high accuracy with no systematic error and uniformly distributed random errors of α TX µ for angles and TX ν for all tracking fiducials, where µ and ν are small relative to the kinematic errors of the robot. Scenario 2 Virtual Fixture Problem Consider now the kinematically redundant endoscope manipulation robot shown in Figure 2. The robot is a serial link robot. For the purpose of this problem, we have broken the structure down into two sub-assemblies. The first consists of a set of joints that positions the elbow of the robot at a pose ( q E relative to the robot base. Further, you can assume that the robot controller software has code to compute the forward kinematics J BE Jacobeans α ( q E and J BE ( q E such that ( q E and left side Similarly, there is an additional kinematic chain extending from the elbow to the endoscope such that the coordinate system of the end of the endoscope relative to the elbow is given by F ET ( q T, where q T are the joint values. You can assume that the robot controller software has code to compute the forward kinematics FET ( qt and left side Jacobeans JET α ( q T and J ET ( q T such that F ET ( q T + Δ q T I+ skew(j α ET ( q T Δ q T,J ET ( q T Δ q T. if ( q ET T The nominal forward kinematics of the whole robot are thus given by F = ( q E if ET ( q T, where q = q T E, T T q. The end of the endoscope is T located at and the view axis of the endoscope passes through direction R z. ( q E + Δ q E I+ skew(j α BE ( q E Δ q E,J BE ( q E Δ q E if ( q BE E p pt out along a n b q E c ρ For the purpose of this this problem, the shaft of the endoscope is assumed to be inserted through a circular opening of radius ρ at position c relative to the base of the robot and lying in a plane parallel to the XY plane of the base coordinate system of the robot. There is also an overhead obstruction represented by a half space, so that only points x with ni x ni b are allowed to be occupied. ( q E F ET ( q T Figure 2: Redundant endoscope manipulation robot a 1 a 2
Questions (Calibration Scenario 1. Describe a kinematic calibration procedure for the cartesian parallel link portion of the robot. I.e., you should tell how you would perform a calibration to enable you to determine ΔF BP ( q 123 I+ sk ( α BP ( q 123, BP ( q 123 for over the cartesian working range of nom the robot. In this case, I am assuming that, given a nominal cartesian position, the controller will solve the inverse kinematic problem to compute a set of joint displacements q 123 = Kins 1 BP ( x nom BP and command the robot to go there. The actual position that the robot goes to will be F * BP = ΔF BP ( x nom BP F BP (Kins 1 BP ( x BP find a way to estimate ΔF BP ( x nom nom BP as a function of x BP. Your answer should include:. Your task is to a. A complete workflow for the procedure. This should include a clear step-by-step process for gathering the necessary data, including what measurement(s are to be taken at each step. Your answer should take account of the uncertain settling time for the robot if it is moved to a new position. b. A complete algorithmic cription of how you will produce functions for α BP ( q 123 and ω BP ( q 123. c. A complete workflow and algorithmic cription of how you will verify your calibration and assess the amount of residual error that the calibration is likely to produce. For this problem, your should plan to stop the robot at each point that you gather calibration data. Explain how you will assure yourself that you each data point used in your calculations is good. Your answer should include some discussion of how you will determine how much data to take, with the understanding that taking truly exhaustive data sets is extremely time consuming, but that taking too little will not produce a reliable calibration. Note that this may include some iteration between calibration and verification steps. cmd 2. How would you modify your calibration process to compute a ired x BP to command the robot so that the stage will wind up at a ired position. 3. Assuming that you have performed the calibration for ΔF BP ( q 123 correctly, explain how you would extend your answer for Question 1 to calibrate the robot for Δ. Again, give sufficient workflow detail so that your answer is clear. In giving your answer, recall that this robot has a special kinematic structure so that ΔF BP ( q 123 and ΔF PE ( q 456 are independent. Explain how you can use this fact to make your calibration process more efficient. 4. The calibration method suggested for Questions 1 and 2 assumes that you will stop the robot to gather each data point used in the calibration procedure. Clearly, it would be more efficient to gather data while the robot is moving. However, this would require a method to account for the offset and sample rate differences between the clock in the tracking system and the clock in the robot controller. How might a program running on x BP nom x BP
the robot controller determine this offset? Give a workflow and sufficient algorithmic detail so that it is clear how one might implement your answer. 5. Suppose that a pointer tool has been placed into tool holder of the robot, so that the position of the tool tip is located at i p tip. Suppose that the robot has placed the tip of this tool at the corners of a cube whose si are of length d. Let tio ( x represent the difference between the commanded position of the end effector and the actual position when the robot is commanded to place the tool tip at position x. Suppose further that we know that tip η, If the robot now moves the pointer to some location within the cube, estimate the worst case error of the tip of the tool relative to the cube s coordinate system. Questions (Virtual Fixture Scenario In the problems below, the robot is assumed to be at a pose level controller has determined a ired incremental motion F. Further a higher ΔF such that the new position of the robot would be F new = ΔF F, with ΔF I+ sk( α,. Further, the high level controller has determined weights and η expressing the strength of these ires. The kinematic control problem to be solved by the mid-level controller is thus Δ q = argmin Δ q η α Such that α ( q,δ q = J α Δ q (formulas to be determined in Question 6 ( q,δ q = J Δ q q min q+ Δ q q max (representing limits on joint travel Δ q min Δ q Δ q max (representing limits on joint motion in one time step In some cases, you may add more than one constraint or objective function. In answering Questions 7-12, I am looking for answers in terms of and. You can assume that the constraints and objective function terms given above are already in the η α α ( q,δ q α 2 + η ( q,δ q 2 Question 6 will ask you to produce formulas for the Jacobeans. Subsequent questions will ask you to add additional constraints and objective function terms to this problem in order to enforce various virtual fixtures modifying the motion. The constraints can either be linear equalities or inequalities or they can be quadratic inequalities of the form ( some linear expression 2 some limit or ( some linear expression 2 +(some other linear expression some limit The objective function terms will have the general form η xxx some linear expression 2 α
optimization problem to be solved. For each question, just give the new constraints and objective function terms. Clearly label these. E.g., New constraints: A α α b α 2 d α A b 2 d New Objective Terms: η 1 E α α f α + η 2 E e f Note that in some cases you will also need to add a few other free variables to your constraints and objective function terms. Define these carefully and clearly. 6. If F ( q+ Δ q I+ sk ( α ( q,δ q, ( q,δ q, produce formulas for and if α J, such that α ( q,δ q J α Δ q ( q,δ q J Δ q 7. Develop an additional virtual fixture constraint to ensure that the position of elbow of the robot does not violate the overhead constraint. I.e., ensure that ni p E ( q+ Δ q ni b 8. Modify this constraint and add a suitable objective function element to penalize motions that bring the elbow to within a distance δ of the constraining plane in Question 7. The penalty should increase quadratically as you get closer to the constraining plane. I.e., if the distance to the plane is d then the penalty should be η δ d δ 2. But there should be no penalty as long as δ d. Hint: Take a look at the lecture notes around pages 41-42 (http://www.cs.jhu.edu/cista/455/lectures/medical%20robots.pptx.pdf. 9. Add a constraint to require that the view of the endoscope lie along the line through a 1 a and a 2, Hint: If the constraint is satisfied, then the line segment through F a 1 and F 1 a 2 must intersect the z axis. 10. Add an objective function term with a quadratic penalty function η c d 2 where d is the distance that the endoscope axis passes from the circle center c. (Here, you will need to give a formula for computing d, among other things. 11. Add a constraint to require that the axis of the endoscope must stay within the constraining circle cribed in the scenario. 12. Modify your answer to Question 10 so that there is no penalty so long as the endoscope axis stays within a distance δ of the point c but incurs a penalty equal to η c (d δ 2 beyond that. J