Lab 3 - Safety Dance (Fall 2021)

Mini-lecture - Forward Kinematics

3D illustration of motor angles, directions of positive rotation, and relevant geometry.

../_images/kinematics.002.png

Problem statement.

../_images/kinematics.003.png

Coordinate frame, link lengths, and leg origin illustration.

../_images/kinematics.004.png

Derivation of x coordinate of foot and L.

../_images/kinematics.005.png

Derivation of y and z coordinate of foot.

Lab Instructions

Step 0. Get the starter code

  1. Get the starter code https://github.com/stanfordroboticsclub/independent-study-lab2

  2. Make sure to follow the installation instructions for the repo.

Step 1. Prepare hardware

  1. Set the controller for the base actuactor of the robot arm to 1 (1 blink).

  2. Set the controller for the shoulder actuactor of the robot arm to 2 (2 blinks).

  3. Set the controller for the elbow actuactor of the robot arm to 3 (3 blinks).

  4. Make sure all the motor controllers are plugged into the CAN 2 bus (the set of connectors near the Teensy).

Step 1. Implement and test a forward kinematics function

  1. Determine if you have a right or left robot leg (there’s a L or R on the lower link).

  2. Update line 15 of src/main.cpp based on the what side leg you have.

  3. Complete the forward_kinematics function inside of src/kinematics.h. You should return a BLA::Matrix<3> of the cartesian coordinates of the end-effector.

  4. Upload code.

  5. Press s to start. The starter code will first test your kinematics code and then run the main loop.

Step 2. View cartesian coordinates of end effector

  1. Start the robot from the zero position. See picture below

  2. Print out the cartesian coordinates of the end effector using your forward kinematics function

../_images/horizontal-config.png

A left robot arm in the starting position for lab 2 and its coordinate system.

Step 3. Make a safety box

  1. Pick a “safety” box – a virtual box in cartesian coordinates that the robot can operate safely in. For example, -0.1<x<0.1 and -.1<y>0.1 and 0<z<-0.2.

  2. Print a warning whenever the robot leaves the safety box.

Step 4. Do the safety dance

  1. Make a function to vibrate the motors (high frequency, low amplitude torque command sinusoid)

  2. If you program a torque sinusoid, a safe range for the amplitude is around 500 - 4000mA. Any lower is barely perceptible.

  3. Run the function whenever the robot end effector leaves the safety box.

[gif of completed project]