Lab 04 - Sampling Based MP

Lab 04 Goals

The goal of lab 04 is to implement a new type of controller, one that both incorportes the dynamic model of the system (as discussed in lecture), and also attempts to track a desired end-effector state error (one that hasn't been discussed in lecture) as opposed to a joint state.

To motivate this controller design, we will use a "catch" task in which a falling ball will need to be caught by the end-effector cup. This controller will try to match the position and velocity of the ball with the end-effector's position and velocity so as to "catch" the ball.

Your wonderful TA has coded up a 4DOF arm within a Mujoco simulator. He has provided two controllers, the second of which is to be completed by you:

1. Joint-Space Controller - when  error between current and the ik results are high. 

2. Task-Space Controller - when the error is low enough.

As part of the task space control design, students must accomplish the following goals:

1. Calculate a Jacobian that relates task space to joint space.

2. Implement a Task space torque controller

3. Test the controller on a robot ball catching problem.

4. Test the controller on another initial state

Key Deliverables

To be submitted on brightspace:

1. Live demo to the TA of the ball catching robot simulation

2. Submission of a video of the ball catching robot simulation from two initial robot states.

3. Copy of the notebook (files -> download -> download ipynb)

0. Clone the code

Open this link for the Colab notebook. Note that you must make a copy of the notebook to save your changes!

Follow the instructions in the "Setup the environment" section and restart the notebook before starting the lab.

1. Task Space Errors

In most of our lectures and labs, we have observed errors as being the difference between a desired joint angle and the current value of the joint angle. However, often we are interested in directly driving our end effector to a desired 3D position and 3D orientation.

For this lab we want the end effector "cup" to match the state of a falling ball. See video below.

2. Jacobian

In this section of work, you will code up the function that calculates the Jacobian matrix which relates end effector state to joint angles. A Jacobian matrix is one that relates two vectors of variables using partial derivatives. It is often used as a method for approximating non-linear functions as linear. Consider the nonlinear function y = g(x). We can approximate the relationship in how y changes linearly with changes in x:

Now, if x and y are vectors, the partial derivative in this relationship becames a matrix, which we call the Jacobian:

Check the "understanding the kinematics" part of the notebook for the coordinate definition and the formulas of forward kinematics. Derive the equations of the task space Jacobian (4x4 matrix) and implement it in  StudentCatchController.task_jacobian. After that, run the test block in the "Jacobian Test" block until you see the "...passed" message

3. Task Space Control

Lets add the following controller.

Step 1: In StudentCatchController._compute_task_space_tau, you can add the following equation to calculate your control signal:

In this equation, h() is called the nonlinear_effects (including gravity and Coriolis force) in the code and is provided as an input argument to the function.

The desired joint acceleration vector, q_dd_des, is calculated in the function _solve_for_qdd., which accepts the Jacobian and the desired task-space acceleration as arguments.

4. SuBmissions

Refer to Key Deliverables