Lab 03 - Tree Search MP

Lab 03 Goals

The goal of lab 03 is to implement Inverse Kinematics on the legs of the Pupper robot and use the IK to implment a walking gait. This requires the following subgoals:

1. Implement Inverse Kinematics

2. Implement A Trotting Gait

3. Simple Trot on the robot

Key Deliverables

To  be submitted on brightspace on the LECTURE section A:

1. IK plot

2. Trotting gait path for single foot plots

3. Live demo of Walking Pupper Robot

4. Copy of lab_3.py file

0. Pull the code

Make your own copy of the based code pulling from the course repo, where we have added lab 3. Link is here.

1. Inverse Kinematics

In this section of work, you will code up a solution to the inverse kinematics. You will leverage the forward kinematics already provided to you. Note this FK implementation used specific coordinate frames and zero angle positions. YOU CAN DO THIS SECTION OF WORK ON YOUR LAPTOP if you install scipy. Your robot may need to install scipy as well, (e.g. with command "pip install scipy").

Step 1:  In the lab_3_playground.py file, locate the two functions get_error_leg and  inverse_kinematics_single_leg. The second function should call the first function. These are the two functions you will need to edit to implement the IK.

Step 2: Within the function get_error_leg, calculate and return a scalar value representing the error between a desired position and a position outputed from the forward kinematics function leg_forward_kinematics. 

Step 3: Within the function inverse_kinematics_single_leg, calculate and return the joint angles that produce the target end effector position target_ee. Use the scipi minimization function demonstrated in lecture.

Step 4: Validate your results by plotting the joint angle outputs for the following series of front left end effector poses: 

[

    [0.11, -0.09, -0.14],

       [0.10, -0.09, -0.14],

       [0.09, -0.09, -0.14],

       [0.08, -0.09, -0.14],

       [0.07, -0.09, -0.14],

       [0.06, -0.09, -0.14],

       [0.05, -0.09, -0.14],

       [0.04, -0.09, -0.14],

       [0.03, -0.09, -0.14],

       [0.02, -0.09, -0.14],

       [0.01, -0.09, -0.14],

]

Note there is code at the bottom of the lab_3_playground.py file to assist you with plotting. Make sure your joint angle plots make sense to you. Double check them with your course assistant and explain why they make sense.

Step 5: Copy your IK code from lab_3_playground.py to the main file lab_3.py.

2. Trotting Gait Implementation

In this section, you will code a triangle shaped gait for each leg to follow.  YOU CAN DO THIS SECTION OF WORK ON YOUR LAPTOP, by modifying and running thelab_3_playground.py file.

Step 1:  Code up six 3D waypoint positions to that follow a triangle shape that each robot foot will track. The six positions are labeled touch_down_position, stand_position_1, stand_position_2, stand_position_3, liftoff_position,   mid_swing_position. The triangle should have 0 for all y values, and its bottom center value should be [0,0,-0.14].

For your initial triangle gate, make the triangle have a base width of 0.10m. Make the triangle's height 0.09m. Each of the four feet will track this triangle, but with a different offset from the robot's base frame 0.

Step 2: Code up each foot's waypoint sequence to form the trotting gait. Each foot may have a different initial way point in the sequence. For example, the right front and left back feet may start at the touch_down_position in the sequence, while the left front and right back feet may start at the stand_position_3 of the sequence. All six position way point positions should be listed at least once in each of the four ee_triangle_positions lists.

Step 3: Cache the robot feet paths. That is, when your code begins, it should create each robot foot's path, and cache it for later tracking. 

To be specific, you want to make a path of 50 trajectory points for each robot foot to track. Each point in the path should correspond to 12 joint angles, 3 for each leg. These joint angles, if tracked by joint motors in sequence, should move the robot feet along the triangular path. 

Since the points are stored as joint angles, you will first need to calculate them. Specifically, each of the 50 points along the path can be calculated as XYZ points by interpolating between the waypoints coded up in the previous section. Then, inverse kinematics can be used to convert the XYZ points to joint angles.

There is quite a bit of code written to handle this for you already, but the one missing function is interpolate_triangle. This function inputs a valute t between 0 and 1 that indicates the progress along the triangular foot path for the foot corresponding to function input leg_index. E.g. t = 0 would indicate the foot should be at the initial position of the trianglular path and t = 0.5 indicates being halfway through the triangular path.

You need to add code to the function interpolate_triangle to make it output the interpolated XYZ position along the path corresponding to value t.

When you want to generate (and print) your cached target joint positions, you can uncomment the generation and print lines at the bottom of the InverseKinematics class __init__ function.

Step 4: Plot out the desired path of the front left foot on an X vs Z plot. To accomplish this, leverage the plotting at the bottom of the lab_3_playground.py file. Make sure your path looks like a complete triangle.

Step 5: Copy your trotting gait code from lab_3_playground.py to the main file lab_3.py.

3. Trot Forward!

Lets make the pupper robot trot. Be sure you copied all relevant code from the playground file to the lab_3.py file.

Step 1: Keep the robot beached on the white support structure. Lauch the node graph defined in lab_3.launch.py. Run lab_3.py and make sure each leg tracks the desired triangle pattern.

Step 2: When everything looks good in Step 1, place your pupper robot on the ground and run lab_3.py file. Your robot should trot!!!

Step 3: Use the lab_3_playground.py file to design a "faster" gait. Plot it out, and discuss with the Course Assistant or Professor. Try implementing it on hardware.

4. ROTATE (Time Permitting)

Make a second gate, that rotates your pupper instead of translating forward. See if you can make your pupper walk a square path.

5. PUSH YOUR CODE!

To save your code on GitHub, lets push it.