====== Abstract ======
In this project, we design a mobile manipulator with four wheel differential
driven mobile base and 6 DoF hydraulic arm to be used in on-site building
construction tasks, and we evaluate the feasibility of the inverse dynamics
controller in the constraint consistent dynamics. Firstly, the Gazebo
simulation of the mobile base and the hydraulic arm is prepared. Then the
rigid body dynamics of the robot are modelled using simplified base actuation
dynamics. The rigid dynamics are projected to the constraint consistent
manifold. Finally, the inverse dynamic algorithms are implemented in the
configuration space and the task space, and their performance are evaluated
via experiments in the Gazebo simulations.
====== Modeling =====
===== Base Actuation =====
In order to avoid overly complicated friction model, we excluded the dynamics
of the wheel in our design and we used a simplified base model with two wheels
for base dynamics.
{{ :adrl:education:completed_projects:two_wheel.jpg?300| }}
After these simplifications, we need following assumptions
to characterise relation between base actuation torques and wheel torques.
* Wheels are always in contact with the ground
* Wheels apply force on the base only in x and z direction, the lateral
forces and moments are ignored.
* No motion in z direction of the base fixed frame
* The CoM of the robot does not change during the motion.
By using these assumptions, we can calculate the torque commands for the
right and the left wheels from torque around z-axis and force in x axis
via following equation,
\begin{equation*}
\begin{bmatrix}
\tau_{r} \\
\tau_{l}
\end{bmatrix}= \begin{bmatrix}
r/(2l) & r/2 \\
-r/(2l) & r/2
\end{bmatrix}\begin{bmatrix}
\tau_\phi \\
F_x
\end{bmatrix}
\label{wheel_torques}
\end{equation}
where $r$ is the radius of the wheels and $l$ is the distance between
the wheels and the CoM of the base. After calculating the wheel torques,
it is integrated over time to obtain angular velocity command as in
the below equation. Note that this integration assumes, that there
isn't any sliding friction at the ground contact.
\begin{equation*}
w^i(t+\Delta t) = (\tau-b_{w} w^i(t))\Delta t /I_{w}
\label{wheel_vel}
\end{equation}
$w^i$ is the angular velocities of $i^{th}$ wheel,and $I_w$ and $b_w$ are
the estimates of the inertia and the damping of the wheels. The estimation
of the damping of the wheel is not possible without an accurate model of
the friction dynamics, therefore it is also tuned with controller gains.
===== Rigid Body Dynamics =====
The mobile base is modelled as a floating body in the base fixed coordinate
frame. Thus, the configurations, which fully describe the state of the robot,
are composed of the pose of the base $\Bf x_B\in SE(3)$ and the joint angles
of the arm $q_A\in R^6$
\begin{equation*}
q = \begin{bmatrix}\Bf x_B \\ q_A \end{bmatrix}
\end{equation}
With $q$ as the state of the robot, below equation shows the rigid body dynamics
of the robot in the configuration space.
\begin{equation*}
M(q)\ddot{q}+H(q,\dot{q})=S^T\tau_a+J_c^T\tau_{c}
\end{equation}
The left handside terms $M\in R^{12\times12}$ and $H\in R^{12}$ are respectively
the inertia matrix, the sum of coriolis/centripetal and gravity forces. And the right
handside terms $S\in R^{8\times12}$ and $J_c\in R^{4\times12}$ are respectively
the selection matrix and the contact Jacobian. The actuation torque $\tau_a\in R^8$
is composed of the base actuation torque in z-axis, base actuation force in x-axis and
6 torque command for the joints of the arm. The contact
force $\tau_c\in R^{4}$ represents the unknown moments and forces applied to the
base by the ground through the wheels.
\begin{equation*}
\tau_a = \begin{bmatrix}
\tau_\phi & F_x & \tau_{arm}
\end{bmatrix}^T \in\real^8
\end{equation}
The $6\times6$ identity matrix and the pair of ones in selection matrix
respectively corresponds to the actuation of the arm joints and the base
actuation in yaw and x direction. On the other hand, the left pair of
ones in the contact Jabocian represents rotational constraints on the base
around x-axis and y-axis, and the right pair of ones represents constraints
on the linear motion in y and z direction which is parallel to assumptions
on the base actuation.
\begin{gather*}
S = \begin{pmatrix}
\begin{matrix}
0 & 0 & 1 & 0 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 & 0 \\
\end{matrix} & 0_{2\times6} \\
0_{6\times6} & I_{6\times6}
\end{pmatrix}
\label{eqn:Selection} \\
J_c = \begin{pmatrix}
\begin{matrix}
1 & 0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 0 & 1 \\
\end{matrix} & 0_{4\times6}
\end{pmatrix}
\label{eqn:Contact_Jac}
\end{gather}
===== Constraint Consistent Dynamics =====
The constraint consistent dynamics are obtained by applying QR
decomposition to the contact Jacobian.
\begin{equation*}
J_c^T=Q\begin{bmatrix}R\\0\end{bmatrix}^T
\end{equation}
$Q\in R^{12x12}$ is an orthogonal matrix (namely $Q^TQ=I$),
and $R\in R^{4x4}$ is an upper triangular matrix. Because of
the constant contact Jacobian, these matrices can be calculated
offline in our case. Then substituting $Q\times R$ into
rigid body dynamics yields two orthogonal dynamics described below.
\begin{gather*}
S_cQ^T[M(q)\ddot{q}+H(q,\dot{q})]=S_cQ^TS^T\tau+R\tau_c\\
S_uQ^T[M(q)\ddot{q}+H(q,\dot{q})]=S_uQ^TS^T\tau
\end{gather}
$S_u=[0_{8x4} , I_{8x8}]$ and $S_c=[I_{8x4} , 0_{8x8}]$ are selecting
the upper and the lower part of the rigid body dynamics. Second equation
doesn't depend on the unknown contact forces and represents the constraint
consistent dynamics on which we build our controllers.
====== Inverse Dynamics Controllers ======
We implemented inverse dynamic controllers, which generate torque signals
for the base and the arm joints to reach given desired positions and
velocities of the end-effector. Ultimately, we would like to have a task
space controller for the mobile manipulator. However, as a first step, we
implemented inverse dynamic controller in the configuration space under
the constraint consistent dynamics. Therewith end-effector position and
velocity can be controlled by inputing trajectories of the joint angles
and the base position.
===== In the Configuration Space =====
We implement acceleration based inverse dynamic controller. The acceleration
reference is set to summation of desired acceleration and PID terms for position
and velocity tracking.
\begin{equation*}
\ddot{q}_{ref} = \ddot{q}_{des} + K_p( q_{des}- q) + K_d(\dot{q}_{des}-\dot{q}) + K_i(\int(q_{des}-q)dt )
\end{equation}
where $K_p,K_d,K_i\in\real^{12\times12}$ are PID gains. It is important
to note that the state of the robot is in base fixed coordinate frame
and it is assumed that we have perfect information of the state of the
robot.
Then the constraint consistent control input is obtained by re-organizing rigid body
dynamics.
\begin{equation}
\tau = P [\hat{M}(q)\ddot{q}_{ref}+\hat{H}(q,\dot{q})]
\end{equation}
$P = (S_uQ^TS^T)^{-1} S_uQ^T$ and $\hat{M}$ and $\hat{H}$ are the
estimates of the $M$ and $H$ in real dynamics. The errors in different
elements of $q$ are affecting each other through the inertia matrix.
While selecting gain matrices, we choose the diagonal matrices
for sake of simplicity and diagonal terms corresponding to constrained
directions are set to zero.
===== In the Task Space =====
To characterize the relation between configuration space and task space,
we need to calculate $6\times12$ dimensional end-effector Jacobian matrix
with respect to world frame.
\begin{equation}
J_E = \begin{bmatrix}
\begin{matrix}
R_{WB} & 0 \\
R_{WB}[\Bf r_{BE}]^T_\times & R_{WB}
\end{matrix}
&
\begin{bmatrix}
R_{WB} & 0 \\
0 & R_{WB}
\end{bmatrix} J_{BE}
\end{bmatrix}
\end{equation}
where $R_{WB}$ is the $3\times3$ rotation matrix between world and base frame,
$\Bf r_BE$ is the position of the end-effector with respect to CoM of the base
and $J_{BE}$ is $6\times6$ Jacobian matrix describing the relative motion of
the end-effector with respect to the mobile base. Then the velocities of the
end-effector are related to the velocities of the base and the joints as in
the following equation,
\begin{equation}
\Wf \dot{x}_E =\begin{bmatrix} \Wf w_E \\\Wf v_E \end{bmatrix}
= J_E\begin{bmatrix} \Bf w_B \\\Bf v_B\\ \dot{q}_{A} \end{bmatrix} =J_E\dot{q}
\end{equation}
where $w_E,v_E,w_B,v_B$ are the angular and the linear velocities of the end-effector
and the base. The end effector velocities are expressed in world frame, whereas
the base velocities are expressed in the base frame.
The time derivative of the above equation give us the acceleration of the end-effector
in terms of the acceleration of the state, time derivative of the end-effector Jacobian
and velocities of the state.
\begin{equation}
\Wf \ddot{x}_E =J_E\ddot{q} +\dot{J}_E\dot{q}
\end{equation}
Re-organizing this equation yields the acceleration reference
in the configuration space as a function of accelaration reference in the task space.
\begin{equation*}
\ddot{q}_{ref} = J_E^\dag(\ddot{x}_{E,ref}-\dot{J}_E\dot{q})
\end{equation}
$J_E^\dag=M^{-1}J^T_E(J_EM^{-1}J^T_E)^{-1}$ is the inertia-weigthed psuedo-inverse
rather than Moorse's psuedo-inverse, so that the motion in the task space is reflected
to the configuration space inversely proportional to the inertias of the joints and base.
The acceleration reference in the task space is composed of desired acceleration plus PID
controller with state feedback \footnote{The difference in desired orientation and the
current orientation is calculated in the tangent space} as in the following equation.
\begin{equation*}
\ddot{x}_{E,ref} = \Wf\ddot{x}_{E,des} + PID(\Wf x_E,\Wf{x}_{E,des},\Wf\dot{x}_{E,des})
\end{equation}
An important part of the task space controller is the handling of redundancies. In the configuration
space, we have 12 DoF and in task space only 6, which means that there will be redundant configurations
throughout the motion and a null space controller is required resolve this problem. We used the
following null space controller.
\begin{equation*}
N = - K_{q,d}\dot{q}-K_{q,p}(q-q_{nom})
\end{equation}
The first term is damping term for the configuration space, the second terms is designed to push
joints to their nominal positions where they operate far from the joint limits and the singularities.
Moreover, the null space controller is applied to system in two different ways. The first approach
is with pre-multiplication of the inertia matrix. Namely, the null space controller is placed in
the acceleration reference of the configuration space. This way, the dynamic
couplings also affect null space controller. However, this leads to an extra difficulty while
tuning the null space gains.
\begin{gather*}
\tau = P[M(q)\ddot{q}_{ref}+H(q,\dot{q})]
\ddot{q}_{ref} = J_E^\dag(\ddot{x}_{E,ref}-\dot{J}_E\dot{q})+(I-J^\dag J) N
\end{gather}
The second approach is without pre-multiplication of the inertia matrix. Null space controller
is added next to the inverse dynamic controller and then the dynamics are projected to the
constraint consistent manifold.
\begin{gather*}
\tau = P[M(q)\ddot{q}_{ref}+H(q,\dot{q})+(I-J^\dag J) N]
\ddot{q}_{ref} = J_E^\dag(\ddot{x}_{E,ref}-\dot{J}_E\dot{q})
\end{gather}
Although, it is easier to tune gains for the controller without pre-multiplication than for the
controller with pre-multiplication, it is still a challenging task.
====== Evaluation ======
In order to evaluate performance of the inverse dynamic controller in configuration
space, we prepared two different experiments in the Gazebo simulation. In the first
experiment, step signals are applied to the arm joints, while the base is kept
stationary. Then joint positions and torques,base position errors and base actuation
torques are plotted.
{{:adrl:education:completed_projects:efe_tiryaki_first_task_arm.jpg?300|}}
{{:adrl:education:completed_projects:efe_tiryaki_first_task_base.jpg?300|}}
The results of the first experiment shows that arm joints reach desired positions
within one second and then, by the help of integral terms, they slowly compansate
the errors which arise from dynamic couplings and modeling errors. Meanwhile, base
agressively reacts to the motion of the arm in the first 2 seconds of the simulations.
The initial rotation of the base is due to the jerk of the arm against which
controller applies a sharp torque command to the base. On the other hand, the small
motion of the base in x-direction is due to the integration of the wheels torques.
It is mentioned in the base actuation chapter that integration of the wheels torques
a technique which is meant to be used in slowly varying signals. After the initial
displacement in x-direction,the base slowly recovers its position.
{{:adrl:education:completed_projects:efe_tiryaki_circular_path.jpg? 400|}}
In the second experiment, a circular trajectory is given to the controller as in figure.
The trajectory is composed of the time varying positions in XY plane and headings. And
to observe the effect of the arm motion to the base sinusoidal signals are sent to the
arm joints.
The joints are following the sinusoidal signal with a small delay, which is enough to
show feasibility of the controller. On the other side, the base rotational error oscilates
with the frequency of the arm joints sinusoidal signal, which means arm motion can
create enough force to overcome friction wheels.
{{:adrl:education:completed_projects:efe_tiryaki_circular_base.jpg?200|}}
{{:adrl:education:completed_projects:efe_tiryaki_circular_arm.jpg?200|}}
The base can follow the circular trajectory with
some accumulating error. The reason of this error can be seen in the plot of base position errors.
After the first 3 seconds, the mean error of the base is around $0.25\times10^{-2}rad$
and over time this small error create significant deviation from desired path in the lateral
direction. This behaviour is unavoidable for trajectory tracking. Nevertheless, a
trajectory can be engineered for better tracking by slowing the motion of the base or by
giving additional rotation for headings in each time step.
====== Conclusion ======
he simulation results suggest that by generating an appropriate trajectory for
the base and the joint configuration the inverse dynamic controller in the
configuration space can be used for controlling mobile manipulator. It is also
observed that the robot is significant slipping in y-direction, which is
contradicting with our non-slipping contact condition assumption. This problem
can be solved by modeling the frictions between wheels and the ground in the
future works.
Unfortunately, we couldn't obtain satisfactory performance from the inverse
dynamic controller in the task space. Our robot is fully actuated in the
configuration space with the constraints, however, the robot is redundant in
the task space. Our null space controller applies damping to arm joints and
pushes them to their nominal angular position. However, we only use damping
terms, since there isn't a desirable nominal pose for the base. The absence of
such nominal pose destabilises the null space controller and results in an
unstable base motion. To tackle this problem, in the future works a second null
space controller which handles internal forces of the robot can be added to
the inverse dynamics controller.
Finally, before implementing these algorithms on the real setup, we also need
a state estimator for the mobile manipulator. As it is mentioned before our
controllers receives base positions and velocities from the Gazebo simulator.
In real setup, the base position and velocities can be estimated by fusing
the wheels velocities, arm joint angles and accelerometer data.
To sum up, the inverse dynamic controller in the configuration space is shown
to be feasible in simulations, but, the task space controller should be
improved to be obtain satisfactory performance.