UCSD MAE 204 FINAL Project

Depending on your programming experience, 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.

You can find an installation guide for every software and package you need in this project here.

V-REP tutorials are here for your reference.

Introduction

In your capstone project, you will write several codes that

1. plans a trajectory for the end-effector of the uArm robotic arm

2. converts the end-effector trajectory into the joint space

3. performs feedback control to drive the robot arm joints 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 arm and the state of the sucker (open or closed) as a function of time. This specification of the position-controlled uArm will then be "played" on the V-REP simulator to see if your trajectory succeeds in solving the task. Finally, you will implement the controller in the physical uArm to demonstrate your final project and receive credit.

Make sure you have a working V-REP installation. This project uses scene UArm_pro_milestone1.ttt and UArm_pro_milestone2.ttt that can be downloaded here. You should download it and test it with its sample csv file, to see what a solution looks like. Leave the block's initial and goal configurations as the default.

Unlike previous projects, where we used V-REP to simply animate the robot's motion, in this project V-REP will use a physics simulator to simulate the interaction of the uArm with the block. 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, ODE and Newton, but Newton is the default for all scene that we will use in this project.

The time between each successive configuration in your csv file is 0.01 seconds (10 milliseconds). This is an important bit of information, since, unlike the previous visualization scenes which simply animated a csv file with no particular notion of time, the notion of time is critical in a dynamic simulation.

A typical line of your csv file would be something like this:

[ 0.75959, 0.47352, 0.058167, 0.80405, 0 ]

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

[ J1, J2, J3, J4, pump state ]

where J1 to J4 are the arm joint angles and pump state is a on/off signal that control suction cup

Quick Summary

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 uArm
the reference initial configuration of the uArm (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 uArm to successfully pick up the block and put it down at the desired location - a csv data file containing the 5-vector joint angles [ J1, J2, J3, J4, suction pump state ]
a plot showing the error twist that would take the end-effector to the desired end-effector configuration in unit time) as a function of time.

Your solution must employ automated planning and control techniques from MAE 204. It cannot 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.

uArm Kinematics

Despite the four-bar-linkages, the uArm may be approximated as a 4DoF robot with open link chain kinematics. There is an auxiliary joint that follows the other joints’ movements and keep the end-effector always horizontal. At the beginning of this project, for simplicity we assume no joint limits on the five joints of the robot arm.

We will come back to this issue later. The end-effector frame {e} is rigidly attached to the last link and is centric in the suction cup.

uArm kinematics using screw axis representation

We can also use screw axis representation to solve the uArm kinematics. In this case, we approximated uArm as a 5R robot with open link chain kinematics, notice that there is an auxiliary joint, which is passive to keep the end-effector horizontal to the ground based on other joints’ configurations, as shown in the figure below. Nevertheless, the screw axis representation is similar to normal open link robots.

Practice for students

Joint Limits

It is recommended, that you choose limits on the joint angles outlined below:

Milestone and Details

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. Also, please refer to the Northeastern University Modern Robotics function code library. It contains useful functions that you can use in this capstone project. github.com/NxRLab/ModernRobotics

Milestone 1: Forward Kinematics and Reference Trajectory Generation

For this milestone you will write a function TrajectoryGenerator.m to generate the reference trajectory, which is relative to space frame {s}, for the end-effector frame {e}. This trajectory consists of 8 concatenated trajectory segments,

A trajectory to move the suction cup from its initial configuration to a "standoff" configuration, a few cm above the cube.
A trajectory to move the suction cup down to the initial configuration of the object.
Enable the suction cup.
A trajectory to move the suction cup back up to the "standoff" configuration.
A trajectory to move the suction cup to a "standoff" configuration above the final configuration.
A trajectory to move the suction cup to the final configuration of the object.
Disable the suction cup.
A trajectory to move the suction cup back to the "standoff" configuration.

Each trajectory segment begins and ends at rest (i.e zero velocity).

The end-effector body frame is defined at the position of the bottom-center of the suction cup (see image). The default initial end-effector configuration T_{se, initial} is different from the home position that we define in the previous section (because that is actually beyond the joint limits). Instead, it is defined by given the joint angles:

You should use forward kinematics you've derived, first find the home position matrix and screw axis then use FKinBody() or FKinSpace() functions, to calculate T_{se, initial}.

Furthermore, for enabling or disabling your suction cup (i.e. third and seventh segment of trajectory), it is recommended that to reserve 0.1 seconds for the process of action.

The cube body frame, which is the subscript {c} of the transformation matrix T, is defined at the center of the cube. You can change cube configuration in Vrep (refer to instruction), but the default initial configuration of the cube is :

The default final configuration of the cube is:

The dimension of the cube is 0.05x0.05x0.05. You should be able to find T_{ce, grab}based on the given and following information.

Inputs:

The initial configuration of the end-effector in the reference trajectory: T_{se, initial}.
The cube's initial configuration: T_{sc, initial}.
The cube's desired final configuration: T_{sc, final}.
The end-effector's configuration relative to the cube when pump is turned on: T_{ce, grab}.
The end-effector's standoff configuration above the cube, before and after turning on the pump, relative to the cube: T_{ce, standoff}. This specifies the configuration of the end-effector {e} relative to the cube frame {c} before lowering to the suction configuration T_{ce, grab}, for example.
The number of trajectory reference configurations per 0.01 seconds: k. The value 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 k = 10 (10 reference configurations, and 10 feedback servo cycles, per 0.01 seconds). It is fine to choose k = 1 if you'd like to keep things simple.

Outputs:

A representation of the N configurations of the end-effector along the entire concatenated eight-segment reference trajectory. Each of these N reference points represents a transformation matrix Tse of the end-effector frame {e} relative to {s} at an instant in time, plus the suction cup pump state (0 or 1). For example, if your entire eight-segment trajectory takes 30 seconds, for example, you will have approximately N = 30k/0.01 sequential reference configurations (perhaps one or a few more, depending on how you treat boundary conditions), each separated by 0.01 / k seconds in time. These reference configurations will be used by your controller. Your representation of the reference configurations could be anything you want. If you use an N x 13 matrix, for example, each of the N rows would represent 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 T_se at that instant of time, i.e., r₁₁,r₁₂,r₁₃,r₂₁,r₂₂,r₂₃,r₃₁,r₃₂,r₃₃,p_x,p_y,p_z , pump state

and the 13th entry is the pump state: 0 = disabled, 1 = enabled. As mentioned before, enabling and disabling the suction cup takes roughly 0.1 seconds (initiated when the suction cup state transitions from 0 to 1, or 1 to 0, in your csv file), so the trajectories involving enabling and disabling the suction cup should keep the {e} frame stationary while the suction cup completes its motion.

A csv file with the entire eight-segment reference trajectory. 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,

r₁₁, r₁₂, r₁₃, r₂₁, r₂₂, r₂₃, r₃₁, r₃₂, r₃₃, p_x, p_y, p_z, pump 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.m 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 (UArm_pro_milestone1.ttt) 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. This video shows an example of a typical output of your trajectory generator function, as animated by V-rep Scene UArm_pro_milestone2.ttt. Or, you can check this page out for tutorials.

Review material: This capstone project builds on material throughout the textbook "Modern Robotics: Mechanics, Planning, and Control." Click here for links to the preprint version of the textbook and the videos. Particularly relevant to this milestone are the following chapters and their associated videos:

Chapters 9.1 and 9.2: Point-to-Point Trajectories (Part 1 of 2) (5:40)

Chapters 9.1 and 9.2: Point-to-Point Trajectories (Part 2 of 2) (3:07)

Milestone 2: uArm Kinematics Simulator and CSV output

For this milestone, you will write a function to simulate the kinematics of the uArm, let's call it NextState.m, is specified by the following inputs and outputs:

Input:

A 5-vector representing the current configuration of the robot (5 variables for the arm joints).
A 5-vector of controls indicating the arm joint speeds .
A timestep Δt.
A positive real value indicating the maximum angular speed of the arm joints. The uArm has a joint speed limit of 40 degrees/second or 0.69 rad/s for all joints. In other words, the angular speed of the arm joints is limited to the range [-0.69 radians/s, 0.69 radians/s]. Any speed in the 5-vector of controls that is outside this range will be set to the nearest boundary of the range.

Output:

A 5-vector representing the configuration of the robot joints time Δt later for feedback control calculation: (theta1, theta2, theta3, thetaAux, theta4)
(optiomal) A 4-vector representing the configuration of the robot joint time Δt later for simulation and physical arm: (theta1, theta2, theta3, theta4)

Testing the NextState function:

To test your NextState function (assuming you write two outputs), you should embed it in a program that takes an initial configuration of the uArm and simulates constant controls for one second. For example, you can set Δt to 0.01 seconds and run a loop that calls NextState 100 times with constant controls . For each iteration in your program, it should write the resulted states into 2 matrices and than into 2 csv files respectively. In each csv file, each line in it records 5 and 4 values respectively (different output have different element, please refer to previous section) representing the robot's configuration after each integration step. Then you should load a csv file that combining second output for the first four columns and a suction cup on/off signal for fifth column into the V-REP scene UArm_pro_milestone2.ttt. Then 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).

As mentioned before, you can refer to this page to find out how you can load your csv file into V-REP.

Since the second output csv file will also apply to the physical arm in the final step, V-rep will inspect the data in your csv file to see whether your data exceed the joint limits or not. If the V-rep reads the data that exceed the joint limits, it will pause the simulation and specifies which angle in your csv file exceed joint limits at status bar. You can choose to stop simulation or continue simulation by clicking stop or play button. However, if you choose to continue simulation and the following positions are still exceed joint limits, V-rep will still pause simulation. The purpose of this procedure is to protect the physical arm as well as notice you that the physical arm may not able to perform the joint positions that specify in your csv file.

Example about robot writing:

Therefore, you should load the csv file (download here) into VREP and watch the simulation condition. If the simulation looks like the robot arm is writing something, you are ready to use physical arm. In this case, you have to load the csv file into python API to control physical arm to move. There is an instruction that you can follow.

If the motion is not what is described, then something is wrong with your implementation of odometry.

You should also try specifying a speed limit of for the joints (for simulation test), 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 for the joints, 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 . As a result, the arm should only rotate half the distance in these tests.

Milestone 3: Feedforward Plus Feedback 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 uArm robot. You will write the function FeedbackControl to calculate the kinematic task-space feedforward plus feedback control law.

Input:

The current actual end-effector configuration X (also written T_se).
The current end-effector reference configuration X_d (i.e., T_se_,d).
The end-effector reference configuration at the next timestep in the reference trajectory, X_d_,next (i.e., T_se_,d,next), at a time Δt later.
The PI gain matrices K_p and K_i.
The timestep Δt between reference trajectory configurations.

Output:

The commanded end-effector twist expressed in the end-effector frame {e}.
A 5-column csv file representing the controller-tuned trajectory configuration of the robot .

In this milestone, we primarily use control law written both as Equation (11.16) and (13.37) in the textbook:

Singularities: If the 6x5 Jacobian matrix J_e is singular (i.e., its rank drops below 6) or nearly singular, the pseudoinverse algorithm may generate a pseudoinverse with unreasonably large entries, which could lead to unacceptably large commanded joint speeds if we ask for even a very small component of a twist in a direction that the near-singularity nearly prevents. A better behavior for the robot would be to essentially ignore any requested twist components in directions that the near-singularity renders difficult to achieve. To achieve this in MATLAB and Mathematica, you may wish to explore the tolerance options for the pseudoinverse algorithm. The tolerance option allows you to specify how close to zero a singular value must be to be treated as zero. By treating small singular values (that are greater than the default tolerance) as zero, you will avoid having pseudoinverse matrices with unreasonably large entries. A MATLAB example is shown below:

>> M = [[2,0];[0,0.00001]]   % a matrix that's nearly singular

M =

   2.000000          0

          0   0.000010

>> s = svd(M)  % confirming that the singular values are 2 and 1e-5, i.e., the 2nd singular value is very small

s =

   2.000000

   0.000010

>> Mp = pinv(M)  % the default MATLAB pseudoinverse treats the 2nd singular value as nonzero; Mp has nonzero entries of 0.5 and 100,000

Mp =

   1.0e+05 *

   0.000005          0

          0   1.000000

>> Mptol = pinv(M,1e-4)  % tell pinv to treat singular values less than 0.0001 as zero; Mptol has no large entries

Mptol =

   0.500000          0

          0          0

In any case, you should always place "reasonable" limits on the maximum speeds for the robot joints and wheels, to mimic the limitations of a real robot and to prevent the simulated robot from moving wildly. Other methods have been proposed in the robotics literature to deal with the nearly-singular-Jacobian issue. Feel free to experiment with other methods if you wish.

Testing your FeedbackControl function:

Before moving on, it is a good idea to confirm that you are correctly calculating and the controls using the Jacobian pseudoinverse. Here are some test inputs you should try:

(X is calculated from the robot configuration given above)

K_p and K_i matrices are zero matrices (this will give you pure feed forward control)
Δt = 0.01

With these inputs, your program should give you

These results should make sense to you. The feedforward twist , from X_d to X_d_,next, should only have nonzero linear components, since the orientation of both frames is the same.

If your K_p matrix is the identity matrix instead of zero, then you should get

If you don't get these results, you should correct your program before moving on.

Bonus example:

Use the concepts of milestone 1, 2 and 3 to create a controller-tuned trajectory configration that picks and moves the cube from a point to another (you can either use default or choose your own initial and final configuration). In this task, try to use different control gains ( K_p and K_i ) and measure its response features (e.g. overshoot, settling time of the error twist) in simulation. Also run the same trajectory configration in the physical arm with different mass cubes and compare the simulation results and physical results.

Final step

Now that feedforward control is working, you are ready to complete your project.

Use the default initial and goal configurations for the cube in the provided V-REP scene, i.e., the initial cube configuration and the final cube configuration are specify in milestone 1. Let the initial configuration of the end-effector reference trajectory be the foreward kinematic result that you calculuate in milestone 1.

Choose a self-defined initial configuration of the Uarm so that the end-effector has at least 15 degrees of orientation error and 0.01 m of position error that is relative to the provided initial configration in milestone 1. Try executing the feedforward controller (K_p = K_i = 0) and play the resulting csv file through the V-REP capstone scene to see what happens.

Now add a positive-definite diagonal proportional gain matrix 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?

In this case, 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.

Eventually, you should run your csv file that you create in this step in the physical robot arm. For the physical arm, you will be given an instruction about how to control the physical arm in python. But make sure that the angles of your csv file do not exceed the joint limit by using the V-rep sence UArm_pro_milestone2.ttt to test your csv file.

Final Submission Package

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

A video of physical arm motion. You will be given a instruction to install and use python to control the physical robot arm. You will need to load the configration csv file that you create in final step into this python code and control the physical robot arm to move. Take a video about the robot arm motion.
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, or if you think an important point is neglected in the description of the project on this page, explain it. If you implemented singularity avoidance, joint limit avoidance, or any other enhancement over the basic project description given on this page, explain your method. You may also wish to include more results in the three results directories described below, showing the results when using your enhancements vs. when you don't use your enhancements, to highlight the value of your enhancements.
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. 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.
A directory called "results" with the results of your program. This directory should contain three directories: one titled "best," one titled "overshoot," and one titled "newTask." The directories "best" and "overshoot" both solve a pick-and-place task where the initial and final configurations of the cube are at the default locations in the capstone V-REP scene, i.e., the initial block configuration is at and the final block configuration is at . The directory "newTask" should have different initial and final block configurations, which you are free to choose yourself. In all cases, the initial configuration of the end-effector should have at least 30 degrees of orientation error and 0.1 m of position error from the first configuration on the reference trajectory. The directory "best" should contain 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 (in fact, it is more interesting if the convergence is not too fast, so the transient response is clearly visible), 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 "overshoot" will likely be feedforward-plus-PI or just PI. You can use any controller to solve the "newTask" task. In each of the three 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. For the "newTask" directory, indicate the initial and goal configurations for the cube.
2. The V-REP .csv file produced by your program when it is called with the input from the log file.
3. A video of your .csv file being animated by the V-REP scene.
4. The X_err data file produced by your program.
5. A plot of the six elements of X_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.
6. A log file showing your program being called with the input. In MATLAB, for example, this log file could be something like:

In this case, the file/function "runscript" contains the data for the program and actually invokes the program, and you should include the file "runscript". It is recommended that your program provide some simple feedback to the user, like "Generating animation csv file. Writing error plot data. Done." or similar, but it is not strictly necessary

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 MAE204 and Modern Robotics textbook in a fairly sophisticated piece of software.

Bonus Point

Now you have completed the MAE 204 final project, here is a challenge for you.

You will utilize uArm robot and the software you wrote to solve a maze puzzle. The CAD file for the maze can be downloaded here.

Given the initial configuration of the cube (XXX), you should use the robot to pick up the cube and navigate it through the maze to the goal configuration (XXX). While moving the cube inside the maze, try your best to avoid collision.

You can modify your milestone 1 code to generate the trajectory for this maze challenge.

TODO: maze CAD file, initial configuration and goal configuration

Report abuse