KUKA youBot Mobile Manipulator
Python | Kinematics | Screw Theory | Feedback Control | Trajectory Generation | Path Planning | CoppeliaSim
Project Overview
For this project, I wrote software that plans a trajectory for the end-effector of the KUKA youBot mobile manipulator (a mobile base with four mecanum wheels and a 5R robot arm), performs odometry as the chassis moves, and implements Feedforward + PI control to drive the youBot to pick up a block at a specified location, carry it to a desired location, and put it down. This project uses the CoppeliaSim physics simulator to simulate the interaction between the youBot and objects in the environment (in this case a simple cube). The full project description can be found here.
KUKA youBot Mobile Manipulator executing a Pick-and-Place action
The program takes 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 youBot (given by a 13-vector)
- the initial configuration
of the reference trajectory for the end-effector frame of the youBot
The output of the program is:
- a csv file which, when “played” through the CoppeliaSim scene, drives the youBot to successfully pick up the block and put it down at the desired location
- a data file containing the 6-vector end-effector error (the twist that would take the end-effector to the reference end-effector configuration in unit time) as a function of time
The configuration of the robot at time \(t\) is represented as a thirteen vector: $$q = \begin{bmatrix} \phi_{chassis} \\
x_{chassis} \\
y_{chassis} \\
J_1 \\
J_2 \\
J_3 \\
J_4 \\
J_5 \\
W_1 \\
W_2 \\
W_3 \\
W_4 \\
\text{gripper state} \end{bmatrix}$$
where \( \phi_{chassis},
x_{chassis},
y_{chassis}\) represent the chassis pose, \(J_1\) to \(J_5\) are the arm joint angles, \(W_1\) to \(W_4\) are the four wheel angles and gripper state {0 or 1} indicates whether the gripper is open or closed.
Using the program inputs, a reference trajectory for the pick-and-place task is generated in 8 distinct segments:
- A trajectory to move the gripper from its initial configuration to a “standoff” configuration a few cm above the block.
- A trajectory to move the gripper down to the grasp position.
- Closing of the gripper.
- A trajectory to move the gripper back up to the “standoff” configuration.
- A trajectory to move the gripper to a “standoff” configuration above the final configuration.
- A trajectory to move the gripper to the final configuration of the object.
- Opening of the gripper.
- A trajectory to move the gripper back to the “standoff” configuration.
The NextState() function in the codebase takes as input:
- a 13-vector representing the current configuration of the robot
- a 9-vector of controls indicating the wheel speeds \(u\)(4 variables) and the arm joint speeds \(\dot{\theta}\) (5 variables)
- a timestep \(\Delta t\)
and outputs a 13-vector representing the configuration of the robot time \(\Delta t\) later.
The FeedbackControl() function calculates the kinematic task-space feedforward plus feedback control signal which is defined as:
$$\mathcal{V}(t) = [Ad_{X^{-1}X_{d}}]\mathcal{V}_{d}(t) + K_{p}X_{err}(t) + K_i\int_{0}^{t}X_{err}(t)dt$$
where,
- \(X\) is the current actual end-effector configuration
- \(X_d\) is the current end-effector reference configuration
- \(X_{d,next}\) is the end-effector reference configuration at the next timestep in the reference trajectory
- \(K_p\) and \(K_i\) are the PI gain matrices
- \(\mathcal{V}(t) \) is the commanded end-effector twist expressed in the end-effector frame {e}.
The feedback control loop calculates the error between the actual end-effector configuration and the desired end-effector configuration and commands a twist to reduce this term to zero. The figures attached below show the 6-vector end-effector error (the twist that would take the end-effector to the reference end-effector configuration in unit time) as a function of time.
For a PI controller with feedback gains of \(K_p\) = 2.5 and \(K_i\) = 1.2, the error plot is:
For a PI controller with feedback gains of \(K_p\) = 2.5 and \(K_i\) = 0.001 the error plot is:
