Mobile Manipulator in Simulator

Project Description

In this project, I wrote a complete software for a mobile manipulation pick-and-place task with a youBot. The task is accomplished by first planning a trajectory for the end-effector, performing odometry calculation for the omnidirectional mobile base, commanding the robot with a feedforward-plus-PI controller. The software is also divided into submodules (classes) according to the task for better organizing and testing. Additionally, I implemented the joint limit to avoid self-collision and singularity. The final outcome is ran by CoppeliaSim for demonstration.


Approach

The entire project includes three main components: Kinematics Simulator, Reference Trajectory Generation, and Feedforward + PID Controller

youBot Kinematics Simulator

The state of the youBot consists of x,y, orientation, five arm joint angles, and four wheel angles

$$ X = \begin{bmatrix} x & y & \theta_b & \theta_1 & \theta_2 & \theta_3 & \theta_4 & \theta_5 & \phi_1 & \phi_2 & \phi_3 & \phi_4 \end{bmatrix} $$

The control input consists of five arm joint speeds and four wheel speeds. Each element has its own limit range to match the real world behavior.

$$ U = \begin{bmatrix} \dot{\theta_1} & \dot{\theta_2} & \dot{\theta_3} & \dot{\theta_4} & \dot{\theta_5} & \dot{u_1} & \dot{u_2} & \dot{u_3} & \dot{u_4} \end{bmatrix} $$

For simplicity, this project uses first-order Euler step for updating the state. It can be replaced by Runge-Kutta, etc.

$$ \theta_{t+1} = \theta_{t} + \dot{\theta} \times \delta t \newline \phi_{t+1} = \phi_{t} + \dot{\phi} \times \delta t \newline \newline V_b = F\delta_{\theta} \newline \newline T_{bb’} = e^{[V_b]} \to \delta{q_b} = (\delta{\phi_b},\delta{x_b},\delta{y_b}) \newline \newline q_{t+1} = q_{t} + \delta{q} $$

Reference Trajectory Generation

The trajectory generation focuses on having the robot joints pass through a series of via points at specific time. The method used in this project is polinomial interpolation.

For each joint X, there are k via points with j = k-1 segments. The joint trajectory for segment j is:

$$ \beta(t_j + \delta{t}) = a_{j0} + a_{j1}\delta{t} + a_{j2}\delta{t^2} + a_{j3}\delta{t^3} $$

with four constraints:

$$ \beta{(t_j)} = \beta_{j} \newline \dot{\beta{(t_j)}} = \dot{\beta_{j}} \newline \beta(t_j + \delta{t}) = \beta_{j+1} \newline \dot{\beta}(t_j + \delta{t}) = \dot{\beta}_{j+1} $$

Solve for all the a in the equation to obtain the polinomial interpolation.

Feedforward + PID Controller

The commanded twist is calculated with the following equation:

The error twist is:

$$ X_{err} = log(X^{-1}X_{d}) $$

The feedforward reference twist is:

$$ V_d = \frac{1}{\delta{t}}log({X_d}^{-1} X_{d,next}) $$


Steps

  1. Initialize youBot kinematics
    a. Omni-directional mobile base
    b. 5-DoF Arm
  2. Generate trajectory for the end-effector of the arm
  3. Start simulation:
    a. Calculate current state
    b. Calculate control force for both joints and wheels using feedforward plus PID
    c. Check joint limit to avoid self-collision and singularity
    d. Update odometry using Euler Method
    e. Repeat
  4. Plot

Results

Here is an example demo with the error plot


Source Code

Unfortunately, the source code cannot be shared due to the copyright of this project. If you are interested, please feel free to contact me.


Reference

All images and equations comes from Modern Robotics: Mechanics, Planning, and Control (Kevin Lynch and Frank Park, Cambridge University Press 2017)

Updated: