Capstone Dev

This page describes the Capstone Project for the Coursera "Modern Robotics" Specialization. This project forms the sixth and final course: "Modern Robotics, Course 6: Capstone Project, Mobile Manipulation." This project draws on pieces of Courses 1 to 5.

A video summary of this project is given in this YouTube video.

Depending on your experience with programming, this project should take approximately 20 hours, broken down into three intermediate milestones and your final submission.

You should use the Modern Robotics code library to help you complete this project.

Introduction

In your capstone project, you will write software that plans a trajectory for the end-effector of the youBot mobile manipulator (a mobile base with four mecanum wheels and a 5R robot arm), performs odometry as the chassis moves, and performs feedback control to drive the youBot to pick up a block at a specified location, carry it to a desired location, and put it down.

The final output of your software will be a comma separated values (csv) text file that specifies the configurations of the chassis and the arm, the angles of the four wheels, and the state of the gripper (open or closed) as a function of time. This specification of the position-controlled youBot will then be "played" on the V-REP simulator to see if your trajectory succeeds in solving the task.

This project uses the CSV Mobile Manipulation youBot scene. You should download it and test it with the sample csv file, to see what a solution looks like.

Unlike previous projects, where we used V-REP to simply visualize the robot's motion, in this project V-REP will use a physics simulator to simulate the interaction of the youBot with the block. In other words, if the gripper closes on the block in the wrong position or orientation, the block may simply slide out of the grasp.

The V-REP scene drives the chassis configuration according to the sequence of chassis configurations in your csv file. For the robot arm, simulated high-gain controllers at the arm joints cause the arm to closely follow the configurations in your csv file.

The interaction between the robot and the block is governed by a physics simulator, often called a "physics engine," which approximately accounts for friction, mass, inertial, and other properties. V-REP has different physics engines which you can select, including Bullet and ODE.

The time between each successive configuration in your csv file is 0.01 seconds (10 milliseconds). A typical line of your csv file would be something like

-0.75959, -0.47352, 0.058167, 0.80405, -0.91639, -0.011436, 0.054333, 0.00535, 1.506, -1.3338, 1.5582, 1.6136, 0

i.e., thirteen values separated by commas, representing

chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, gripper state

where J1 to J5 are the arm joint angles and W1 to W4 are the four wheel angles.

Wheels 1 to 4 are numbered as shown in the image to the right. The ten angles (phi for the chassis, five arm joint angles, and four wheel angles) are in radians and the two chassis position coordinates (x,y) are in meters. A gripper state of 0 indicates that you want the gripper to be open, and a gripper state of 1 indicates that you want the gripper to be closed. In practice, the transition from open to closed (or from closed to open) takes up to one second, so any transition from 0 to 1, or 1 to 0, on successive lines in your csv file initiates an action (opening or closing) that will take some time to complete.

Your program will take as input:

• the initial resting configuration of the cube object (which has a known geometry), represented by a frame attached to the center of the object
• the desired final resting configuration of the cube object
• the actual initial configuration of the youBot
• the reference initial configuration of the youBot (which will generally be different from the actual initial configuration, to allow you to test feedback control)
• optionally: gains for your feedback controller (or these gains can be hard-coded in your program)

The output of your program will be:

• a csv file which, when "played" through the V-REP scene, should drive the youBot to successfully pick up the block and put it down at the desired location
• a data file containing the 6-vector end-effector error (the twist that takes the end-effector to the reference end-effector configuration in unit time) as a function of time

Your solution must employ automated planning and control techniques from the Coursera specialization. It should not simply be a manually coded trajectory of the robot. Your solution should automatically go from the input to the output, with no other human intervention. In other words, it should automatically produce a working csv file even if the input conditions are changed.

In your software, you should piece together a reference trajectory for the gripper of the robot, which the robot is then controlled to follow. A typical reference trajectory would consist of the following eight segments, as illustrated in the eight images to the right (click on any image to make it larger):

1. A trajectory to move the gripper from its initial configuration to a "standoff" configuration a few cm above the block.
2. A trajectory to move the gripper down to the grasp position.
3. Closing of the gripper.
4. A trajectory to move the gripper back up to the "standoff" configuration.
5. A trajectory to move the gripper to a "standoff" configuration above the final configuration.
6. A trajectory to move the gripper to the final configuration of the object.
7. Opening of the gripper.
8. A trajectory to move the gripper back to the "standoff" configuration.

Segments 3 and 7 each keep the end-effector fixed in space but, at the beginning of the segment, change the state of the gripper from 0 to 1 or 1 to 0, waiting one second for the gripper closing to complete. In other words, each of these segments would consist of 100 identical lines of the csv file (corresponding to one second), where the first line has a gripper state different from the previous line in the csv file, to initiate the opening or closing.

Segments 2, 4, 6, and 8 are simple up or down translations of the gripper of a fixed distance. Good trajectory segments would be cubic or quintic polynomials taking a reasonable amount of time (e.g., one second).

Trajectory segments 1 and 5 are longer motions requiring motion of the chassis. Segment 1 is calculated from the desired initial configuration of the gripper to the standoff configuration, and segment 5 is calculated from the first standoff configuration to the second standoff configuration. The gripper trajectories could correspond to constant screw motion paths or decoupled Cartesian straight-line motion plus rotational motion, time scaled by third-order polynomials (Chapter 9).

Once the entire gripper reference trajectory has been pieced together from the 8 segments, the actual trajectory of the youBot is obtained by using a Jacobian pseudoinverse position controller as described in Chapter 13.5. Starting from the actual initial robot configuration (which has some error from the beginning of reference segment 1), your controller drives the gripper to converge to the reference trajectory. Your feedback controller should eliminate initial error before the gripper attempts to grasp the block, to avoid failure.

You are welcome to adopt a different approach to solving the pick-and-place problem, provided your approach takes the same inputs specified above, produces an error data file and a csv file that successfully achieves the task, and is fully automatic (i.e., it works for different initial and final configurations of the block, different initial and reference configurations for the youBot, and requires no human intervention).

To simulate the effect of feedback control, you must write your own motion simulator. For each timestep, you take the initial configuration of the robot and the wheel and joint speeds calculated by your controller and numerically integrate the effect of these speeds over a timestep to get the new robot configuration. To calculate the new configuration of the chassis due to the wheel motions, you must implement an odometry step (Chapter 13.4).

Kinematics of the youBot

The images to the right illustrate the youBot. Click on them to make them bigger. The description below is consistent with Exercise 13.33 from the book, if you prefer to see the information there. All distances are in meters and all angles are in radians.

This figure illustrates the arm at its home configuration (all joint angles zero) and the frames {s}, {b}, {0}, and {e}. For the image on the right, joint axes 1 and 5 (not shown) point upward and joint axes 2, 3, and 4 are out of the screen. Click to make the image bigger.

A top view of the omnidirectional mobile base. The forward-backward distance between the wheels is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 2l = 0.47} meters and the side-to-side distance between wheels is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 2w = 0.3} meters. The radius of each wheel is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle r = 0.0475} meters. The forward driving and "free sliding" direction Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \gamma} of each wheel is indicated.

The gripper and the end-effector frame {e}, which has an origin midway between the fingers of the gripper. The minimum opening distance of the gripper is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{1,\text{min}} = 2} cm, the maximum opening distance is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{1,\text{max}} = 7} cm, the interior length of the fingers is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{2} = 3.5} cm, and the distance from the base of the fingers to the frame {e} is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{3} = 4.3} cm. The axis Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \hat{\text{x}}_{\text{e}}} is into the screen.

The object being manipulated is a cube, 5 cm x 5 cm x 5 cm. The cube's frame is at its center, and the axes are aligned with the edges of the cube.

The configuration of the frame {b} of the mobile base, relative to the frame {s} on the floor, is described by the 3-vector Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle q = (\phi,x,y)} or the Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle SE(3)} matrix

$T_{sb}(q)=\left[{\begin{array}{cccc}\cos \phi &-\sin \phi &0&x\\\sin \phi &\cos \phi &0&y\\0&0&1&0.0963\\0&0&0&1\end{array}}\right]$

where Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle z = 0.0963} meters is the height of the {b} frame above the floor. The forward-backward distance between the wheels is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 2l = 0.47} meters and the side-to-side distance between wheels is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle 2w = 0.3} meters. The radius of each wheel is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle r = 0.0475} meters. The forward driving and "free sliding" direction Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \gamma} of each wheel is indicated in the figures.

The fixed offset from the chassis frame {b} to the base frame of the arm {0} is

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{b0}= \left[\begin{array}{cccc} 1 & 0 & 0 & 0.1662 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0.0026 \\ 0 & 0 & 0 & 1 \end{array}\right].}

When the arm is at its home configuration (all joint angles zero, as shown in the figure), the end-effector frame {e} relative to the arm base frame {0} is

$M_{0e}=\left[{\begin{array}{cccc}1&0&0&0.033\\0&1&0&0\\0&0&1&0.6546\\0&0&0&1\end{array}}\right].$

When the arm is at its home configuration, the screw axes Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \mathcal{B}} for the five joints are expressed in the end-effector frame {e} as

1. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \omega_1 = (0,0,1), \; v_1 = (0,0.033,0)}
2. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \omega_2 = (0,-1,0), \; v_2 = (-0.5076,0,0)}
3. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \omega_3 = (0,-1,0), \; v_3 = (-0.3526,0,0)}
4. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \omega_4 = (0,-1,0), \; v_4 = (-0.2176,0,0)}
5. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \omega_5 = (0,0,1), \; v_5 = (0,0,0)}

In this project, for simplicity we assume no joint limits on the five joints of the robot arm. It is recommended, however, that you choose limits on the wheel and joint velocities. We will come back to this issue later.

The end-effector frame {e} is rigidly attached to the last link and is midway between the tips of the gripper fingers. The minimum opening distance of the gripper is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{1,\text{min}} = 2} cm, the maximum opening distance is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{1,\text{max}} = 7} cm, the interior length of the fingers is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{2} = 3.5} cm, and the distance from the base of the fingers to the frame {e} is Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle d_{3} = 4.3} cm. When the gripper closes, it closes until it reaches its minimum closing distance or encounters a force large enough to prevent further closing.

The object being manipulated is a cube, 5 cm x 5 cm x 5 cm. The cube's frame {c} is at the center of the cube, and the axes are aligned with the edges of the cube. The initial configuration of the cube is at (x, y, z) = (1, 0, 0.025 ) m in the space frame {s}, and the axes of {c} are aligned with {s}. The desired final configuration of the cube is at (x, y, z) = (0, 1, 0.025) m and the axes of {c} are rotated by Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle -\pi/2} about the Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \hat{\text{z}}_{\text{s}}} axis of {s}. A good choice for the "standoff" configuration before moving down to the cube and moving back up (the ends of trajectories 1, 4, 5, and 8) is to have the {e} frame a few cm above the {c} frame.

What You Will Do

Your solution to this project will be a fairly complex piece of software. To help you structure the project, and to allow you to test individual pieces of your solution, the project has three milestones before you finally complete the project. You do not turn in your solutions to these milestone subprojects; you only turn in your final project.

Milestone 1: youBot Kinematics Simulator and csv Output

You will write a simulator for the kinematics of the youBot. The main function in the simulator, let's call it NextState, is specified by the following inputs and outputs:

Input:

• A 12-vector representing the current configuration of the robot (3 variables for the chassis configuration, 5 variables for the arm configuration, and 4 variables for the wheel angles).
• A 9-vector of controls indicating the arm joint speeds Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \dot{\theta}} (5 variables) and the wheel speeds $u$ (4 variables).
• A timestep Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t} .
• A positive real value indicating the maximum angular speed of the arm joints and the wheels. For example, if this value is 12.3, the angular speed of the wheels and arm joints is limited to the range [-12.3 radians/s, 12.3 radians/s]. Any speed in the 9-vector of controls that is outside this range will be set to the nearest boundary of the range. If you don't want speed limits, just use a very large number. If you prefer, your function can accept separate speed limits for the wheels and arm joints.

Output: A 12-vector representing the configuration of the robot time Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t} later.

The function NextState is based on a simple first-order Euler step, i.e.,

• new arm joint angles = (old arm joint angles) + (joint speeds) * Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t}
• new wheel angles = (old wheel angles) + (wheel speeds) * Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t}
• new chassis configuration is obtained from odometry, as described in Chapter 13.4

To test your NextState function, you should embed it in a program that takes an initial configuration of the youBot and simulates constant controls for one second. For example, you can set Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t} to 0.01 seconds and run a loop that calls NextState 100 times with constant controls Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle (u,\dot{\theta})} . Your program should write a csv file, where each line has 13 values separated by commas (the 12-vector consisting of 3 chassis configuration variables, the 5 arm joint angles, and the 4 wheel angles, plus a "0" for "gripper open") representing the robot's configuration after each integration step. Then you should load the csv file into the CSV Mobile Manipulation youBot V-REP scene and watch the animation of the constant controls to see if your NextState function is working properly (and to check your ability to produce a csv file).

Sample controls to try: Simulate the following controls for 1 second and watch the results in the V-REP scene. The controls below are only for the wheels; you can choose the arm joint speeds as you wish.

1. $u=(10,10,10,10)$. The robot chassis should drive forward (in the Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle +\hat{\text{x}}_{\text{b}}} direction by 0.475 meters.
2. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle u = (-10,10,-10,10)} . The robot chassis should slide in the Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle +\hat{\text{y}}_{\text{b}}} direction by 0.475 meters.
3. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle u = (-10,10,10,-10)} . The robot chassis should spin counterclockwise in place by 1.234 radians.

If the chassis motion is not what is described, then something is wrong with your implementation of odometry. If you are uncertain that your wheel motions and chassis motions correspond to each other, you can check out the five basic mobile base motions shown in a .zip file in the CSV Animation youBot scene.

You should also check that your wheel angles and arm joint angles are being updated properly, but this should be easy.

You should also try specifying a speed limit of 5 for the joints and wheels, then try the same tests above. Since your commanded controls exceed the speed limit, your function should properly restrict the actual speeds executed by the wheels and joints to the range [-5, 5]. As a result, the chassis should only move half the distance in these tests.

Milestone 2: Reference Trajectory Generation

For this milestone you will write a function TrajectoryGenerator to generate the reference trajectory for the end-effector frame {e}. This trajectory consists of eight concatenated trajectory segments, as described above. Each trajectory segment begins and ends at rest.

Input:

• The initial configuration of the end-effector in the reference trajectory: Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{se,\text{initial}}} .
• The cube's initial configuration: Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{sc,\text{initial}}} .
• The cube's desired final configuration: $T_{sc,{\text{final}}}$.
• The end-effector's configuration relative to the cube when it is grasping the cube: Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{ce,\text{grasp}}} .
• The end-effector's standoff configuration above the cube, before and after grasping, relative to the cube: Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{ce,\text{standoff}}} . This specifies the configuration of the end-effector {e} relative to the cube frame {c} before lowering to the grasp configuration Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{ce,\text{grasp}}} , for example.
• The number of trajectory reference configurations per 0.01 seconds: Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle k} . The value Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle k} is an integer with a value of 1 or greater. Although your final animation will be based on snapshots separated by 0.01 seconds in time, the points of your reference trajectory (and your controller servo cycle) can be at a higher frequency. For example, if you want your controller to operate at 1000 Hz, you should choose Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle k=10} (10 reference configurations, and 10 feedback servo cycles, per 0.01 seconds). It is fine to choose Failed to parse (Conversion error. Server ("https://wikimedia.org/api/rest_") reported: "Cannot get mml. Server problem."): {\displaystyle k=1} if you'd like to keep things simple.

Outputs:

• An Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle N \times 13} matrix. Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle N} is the number of reference points in the entire concatenated eight-segment reference trajectory. Each of the Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle N} rows represents a configuration of the end-effector frame {e} relative to {s} at that instant in time. Twelve of the 13 entries of a matrix row are the top three rows of the transformation matrix Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{se}} at that instant of time, i.e., Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle r_{11}, r_{12}, r_{13}, r_{21}, r_{22}, r_{23}, r_{31}, r_{32}, r_{33}, p_x, p_y, p_z} from

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{se} = \left[\begin{array}{cccc} r_{11} & r_{12} & r_{13} & p_x \\ r_{21} & r_{22} & r_{23} & p_y \\ r_{31} & r_{32} & r_{33} & p_z \\ 0 & 0 & 0 & 1 \end{array}\right], }

and the 13th entry is the gripper state: 0 = open, 1 = closed. Keep in mind that opening and closing the gripper takes up to 1 second (initiated when the gripper state transitions from 0 to 1, or 1 to 0, in your csv file), so the trajectories involving opening and closing the gripper should keep the {e} frame stationary while the gripper completes its motion.

• A csv file with the entire eight-segment reference trajectory. (This csv file is only needed for visualizing the output of your trajectory generator; it is not needed for the final submitted project.) Each line of the csv file corresponds to one configuration $T_{se}$ of the end-effector, expressed as 13 variables separated by commas. The 13 variables are, in order,

r11, r12, r13, r21, r22, r23, r31, r32, r33, px, py, pz, gripper state

It is up to you to determine the duration of each trajectory segment, but it is recommended that each segment's duration be an integer multiple of 0.01 seconds. You could automatically choose the duration of each trajectory segment to be equal to the maximum of: the distance the origin of the {e} frame has to travel divided by some reasonable maximum linear velocity of the end-effector, and the angle the {e} frame must rotate divided by some reasonable maximum angular velocity of the end-effector. The duration of each trajectory should not be so short as to require unreasonable joint and wheel speeds.

Your TrajectoryGenerator function is likely to use either ScrewTrajectory or CartesianTrajectory, from the Modern Robotics code library, to generate the individual trajectory segments.

Testing your function: We have created a V-REP scene (COMING SOON) to help you test your TrajectoryGenerator function. This scene reads in your csv file and animates it, showing how the end-effector frame moves as a function of time. You should verify that your TrajectoryGenerator works as you expect before moving on with the project.

Milestone 3: Feedforward Control

Now that you are able to simulate the motion of the robot and generate a reference trajectory for the end-effector, you are ready to begin experimenting with feedback control of the mobile manipulator. You will write the function FeedbackControl to calculate the kinematic task-space feedforward plus feedback control law, written both as Equation (11.16) and (13.37) in the textbook:

Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \mathcal{V}(t) = [\text{Ad}_{X^{-1} X_d}] \mathcal{V}_d(t) + K_p X_{\text{err}}(t) + K_i \int_0^t X_{\text{err}}(\text{t}) d\text{t}. }

Input:

• The current end-effector reference configuration Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{d}} .
• The end-effector reference configuration at the next timestep in the reference trajectory, Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{d,\text{next}}} , at a time Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t} later.
• The current actual end-effector configuration Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X} (also written Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle T_{se}} ).
• The PI gain matrices $K_{p}$ and Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle K_i} .
• The timestep Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t} between reference trajectory configurations.

Output:

• The commanded end-effector twist Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \mathcal{V}} expressed in the end-effector frame {e}.

To calculate the control law FeedbackControl, we need the current actual end-effector configuration Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X(q,\theta)} , a function of the chassis configuration Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle q} and the arm configuration Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \theta} . The values Failed to parse (Conversion error. Server ("https://wikimedia.org/api/rest_") reported: "Cannot get mml. Server problem."): {\displaystyle (q,\theta )} come directly from the simulation results (Milestone 1). In other words, assume perfect sensors and odometry.

The error twist Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{\text{err}}} that takes Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X} to Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{d}} in unit time is extracted from Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle [X_{\text{err}}] = \log (X^{-1} X_{d})} . FeedbackControl also needs to maintain an estimate of the integral of the error, e.g., by adding Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{\text{err}} \Delta t} to a running total at each timestep. The feedforward reference twist Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \mathcal{V}_d} that takes $X_{d}$ to Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{d,\text{next}}} in time Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \Delta t} is extracted from Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle [\mathcal{V}_d] = (1/\Delta t) \log(X_{d}^{-1} X_{d,\text{next}})} .

The output of FeedbackControl is the commanded end-effector twist Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \mathcal{V}} expressed in the end-effector frame {e}. To turn this into commanded wheel and arm joint speeds Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle (u,\dot{\theta})} , we use the pseudoinverse of the mobile manipulator Jacobian Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle J_e(\theta)} ,

Failed to parse (Conversion error. Server ("https://wikimedia.org/api/rest_") reported: "Cannot get mml. Server problem."): {\displaystyle \left[{\begin{array}{c}u\\{\dot {\theta }}\end{array}}\right]=J_{e}^{\dagger }(\theta ){\mathcal {V}}.}

The full program: Now write your full program, according to the input specifications at the top of this page. Your program should first generate a reference trajectory using TrajectoryGenerator and set the initial robot configuration, a 13-vector as described earlier on this page:

chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4, gripper state

Now the program enters a loop that loops through the reference trajectory generated by TrajectoryGenerator. If the reference trajectory has Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle N} reference configurations, the loop runs Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle N-1} times. For example, the 10th time through the loop, the controller uses the 10th configuration of the reference trajectory as Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_d} and the 11th configuration as Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{d,\text{next}}} to help calculate the feedforward twist Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle \mathcal{V}_d} .

Each time through the loop, you

• calculate the control law using FeedbackControl and generate the wheel and joint controls using Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle J_e^\dagger(\theta)} ;
• send the controls, configuration, and timestep to NextState to calculate the new configuration;
• store every $k$th configuration for later animation (note that the reference trajectory has Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle k} reference configurations per 0.01 second step, as described in Milestone 2; you may choose Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle k=1} for simplicity); and
• store every Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle k} th Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{\text{err}}} 6-vector, so you can later plot the evolution of the error over time.

Once the program has completed all iterations of the loop, it should write out the csv file of configurations. If the total time of motion of the youBot is 15 seconds, your csv file should have 1500 lines (or 1501 lines), corresponding to 0.01 seconds between each configuration. Load the csv file into the CSV Mobile Manipulation youBot scene to see the results. Your program should also generate a file with the log of the Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{\text{err}}} 6-vector as a function of time, suitable for plotting by your favorite plotting software.

Testing feedforward control: You should make sure feedforward control works as you expect before testing feedback control. Choose an initial configuration of the robot that puts the end-effector exactly at the configuration at the beginning of the reference trajectory. Run your program with Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle K_p = K_i = 0} , i.e., feedforward control only. This should result in a csv file which, when played through the V-REP scene, drives the robot to pick up the block and put it down at the desired configuration. (Or at least it should come close to doing so!) If not, time to start debugging! Your end-effector reference trajectory must be correct, if you already tested Milestone 2. So now you have to figure out why the wheel and arm controls your feedforward controller and Jacobian pseudoinverse are generating do not drive the end-effector along the reference trajectory.

You should also try starting the end-effector with some initial error from the reference trajectory. See how the end-effector moves under these circumstances. Does it make sense to you?

Do not move on with the project until your feedforward control works as you expect. Otherwise the effects of PI feedback control will only further confuse the situation.

Final Step: Completing the Project and Your Submission

Now that feedforward control is working, you are ready to complete your project. Let the initial configuration of the end-effector reference trajectory be at

$T_{se}=\left[{\begin{array}{cccc}0&0&1&0\\0&1&0&0\\-1&0&0&0.5\\0&0&0&1\end{array}}\right]$

Choose an initial configuration of the youBot so that the end-effector has at least 30 degrees of orientation error and 0.2 m of position error. Try executing the feedforward controller (Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle K_p = K_i = 0} ) to see what happens.

Now add a positive-definite diagonal proportional gain matrix Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle K_p} while keeping the integral gains zero. You can keep the gains "small" initially so the behavior is not much different from the case of feedforward control only. As you increase the gains, can you see some corrective effect due to the proportional control?

Eventually you will have to design a controller so that essentially all initial error is driven to zero by the end of the first trajectory segment; otherwise, your grasp operation may fail.

Once you get good behavior with feedforward-plus-P control, try experimenting with other variants: P control only; PI control only; and feedforward-plus-PI control.

What to submit: You will submit a single .zip file of a directory with the following contents:

1. A file called README.txt or README.pdf. This file should briefly explain your software and your results. If you needed to follow a different approach to solve the problem than the one described above, explain why and explain your solution method. If you encountered anything surprising, or if there is something you still don't understand, explain it.
2. Your commented code in a directory called "code." Your code should be lightly commented, so it is clear to the reader what the code is doing. No need to go overboard, but keep in mind your reviewer may not be fluent in your programming language. Your code comments must include an example of how to use the code. Only turn in functions that you wrote or modified; you don't need to turn in other MR functions that your code uses. If your code is in MATLAB or Python, just turn in the text files with your functions. If your code is in Mathematica, turn in (a) your .nb notebook file and (b) a .pdf printout of your code, so a reviewer can read your code without having to have the Mathematica software.
3. A directory called "results" with the results of your program. This directory should contain two directories: one titled "best" and one titled "overshoot." The directory "best" should contain the results using a well-tuned controller, either feedforward-plus-P or feedforward-plus-PI. The convergence exhibited by the controller does not necessarily have to be fast, but the motion should be smooth, with no overshoot, and very little error by partway through trajectory segment 1. The directory "overshoot" should contain the results using a less-well-tuned controller, one that exhibits overshoot and a bit of oscillation. Nonetheless, the error should be eliminated before the end of trajectory segment 1. Your controller for these results will likely be feedforward-plus-PI or just PI. In each of the two directories, give:
   1. A very brief README.txt or README.pdf file that indicates the type of controller, the feedback gains, and any other useful information about the results.
   2. A log file showing the program being called with the input.
   3. The V-REP .csv file produced by the program.
   4. A video of your .csv file being animated by the V-REP scene.
   5. The Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{\text{err}}} data file produced by your program.
   6. A plot of the six elements of Failed to parse (MathML with SVG or PNG fallback (recommended for modern browsers and accessibility tools): Invalid response ("Math extension cannot connect to Restbase.") from server "https://wikimedia.org/api/rest_v1/":): {\displaystyle X_{\text{err}}} as a function of time, showing the convergence to zero. This plot should not require any special software (e.g., MS excel) to be viewable. In other words, you should save it as a .pdf or other freely-viewable format.

Project grading. Your project will be graded on the clarity and correctness of your README files and your code. Your "results" directories will be graded on their correctness, including the quality of your videos and whether your error plots show reasonable convergence to zero.

If you succeed in this project, congratulations! You have integrated concepts from all five previous Modern Robotics courses in a fairly sophisticated piece of software.

Alternative Solution Methods

You could imagine other approaches to solving the mobile manipulator pick-and-place problem, instead of just planning a trajectory for the end-effector and using feedback control to track it. For example, you could use an obstacle-avoiding motion planner to plan a reference trajectory for the entire robot, not just the end-effector. You could incorporate joint limits for the robot arm. You could use a weighted pseudoinverse, instead of the standard pseudoinverse, to indicate a preference to use the wheel or joint motions. You could actively avoid singularities of the arm. You could decide to keep the mobile base stationary during trajectory segments 2, 4, 6, and 8.

If you have other ideas on better ways to approach the mobile manipulation problem, feel free to mention them in the discussion prompt or your main README file.

For fun: See if you can plan and execute a trajectory for the robot arm that causes the gripper to throw the block to a desired landing point!