The purpose of this project is writing 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.
To achieve the goal of the project, I wrote four functions:
Input: The input of the function includes config, speed,
timestep, and max_speed.
config: 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).
speed: A 9-vector of controls indicating the wheel speeds and the arm joint speeds.
timestep: A timestep.
max_speed: A positive real value indicating the maximum angular speed of the arm joints and the wheels. Speeds outside the range are set to the nearest boundary.
Output: The return of the function is new_config.
new_config: A 12-vector representing the configuration of the robot.
The function is based on a simple first-order Euler step, updating arm joint angles, wheel angles, and chassis configuration using odometry.
Input: The function takes Tse_initial, Tsc_initial,
Tsc_goal, Tce_grasp, Tce_standoff, and k.
Tse_initial: Initial configuration of the end-effector in the reference trajectory.
Tsc_initial: Cube's initial configuration.
Tsc_goal: Cube's desired final configuration.
Tce_grasp: End-effector's configuration relative to the cube when grasping.
Tce_standoff: Standoff configuration above the cube.
k: Number of trajectory reference configurations per 0.01 seconds.
Output: The return of the function is N_final.
N_final: A representation of the N configurations of the end-effector along the concatenated eight-segment reference trajectory. Each configuration includes a transformation matrix and gripper state.
Input: The function takes X, Xd, Xd_next,
Kp, Ki, and delta_t.
X: Current actual end-effector configuration (Tse).
Xd: Current end-effector reference configuration (Tse,d).
Xd_next: Next timestep reference configuration (Tse,d,next).
Kp, Ki: PI gain matrices.
delta_t: Timestep between reference trajectory configurations.
Output:
V: Commanded end-effector twist expressed in the end-effector frame.
Xerr: Error twist.
Purpose: Helps the robot arm avoid singularities. Ensures joints 3 and 4 remain less than -0.3.
Input: joint_theta: Joint angles to check against limits.
Output: res: Indicates which joints exceed limits.