Equations of motion of a floating double pendulum

Consider a two-link inverted pendulum, where each link is a homogenous rod.

The center of mass of each link and the combined pendulum

Let $p_1$ and $p_2$ denote the position of CoM of link 1 and link 2, respectively. We get \begin{equation} p_1 = \begin{bmatrix} q_3 + \frac{l_1}{2}s_1\\ q_4 + \frac{l_1}{2}c_1 \end{bmatrix} \end{equation} \begin{equation} p_2 = \begin{bmatrix} q_3 + l_1s_1 + \frac{l_2}{2}s_{1+2}\\ q_4 + l_1c_1 + \frac{l_2}{2}c_{1+2} \end{bmatrix} \end{equation} The center of mass of the double pendulum becomes \begin{equation} p_c = \frac{m_1}{m_2+m_2}p_1 + \frac{m_2}{m_1 + m_2} p_2. \end{equation}

The velocities of the links

The velocity of the centers of mass: \begin{equation} \dot{p}_1 = \begin{bmatrix} \dot{q}_3 + \frac{l_1}{2}c_1\dot{q}_1\\ \dot{q}_4 + \frac{l_1}{2}s_1\dot{q}_1 \end{bmatrix} = \begin{bmatrix} \frac{l_1}{2}c_1 & 0 & 1 & 0\\ \frac{l_1}{2}s_1 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \dot{q}_4 \end{bmatrix} = V_1(q)\dot{q} \end{equation} \begin{equation} \dot{p}_2 = \begin{bmatrix} \dot{q}_3 - l_1c_1\dot{q}_1 - \frac{l_2}{2}c_{1+2}(\dot{q}_1 + \dot{q}_2)\\ \dot{q}_4 - l_1s_1\dot{q_1} - \frac{l_2}{2}s_{1+2}(\dot{q}_1 + \dot{q}_2) \end{bmatrix} = \begin{bmatrix} -l_1c_1 - \frac{l_2}{2}c_{1+2} & - \frac{l_2}{2}c_{1+2} & 1 & 0\\ -l1s_1- \frac{l_2}{2}s_{1+2} & - \frac{l_2}{2}s_{1+2} & 0 & 1\end{bmatrix} \begin{bmatrix} \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \dot{q}_4 \end{bmatrix} = V_2(q)\dot{q} \end{equation}

The rotational velocities: \begin{equation} \omega_1 = \dot{q}_1 = \begin{bmatrix} 1 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \dot{q}_4 \end{bmatrix} = \Omega_1\dot{q}. \end{equation} \begin{equation} \omega_2 = \dot{q}_1 + \dot{q}_2 = \begin{bmatrix} 1 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \dot{q}_4 \end{bmatrix} = \Omega_2\dot{q}. \end{equation}

The kinetic energy of the pendulum

Both links contribute to the kinetic energy. For each link, the kinetic energy consists of two parts: the translational velocity of the center of mass, and the rotational velocity of the link. Recall that the moment of inertia of a rod for rotations about its center of mass is $m\frac{l^2}{12}$. We get \begin{equation} T = \frac{m_1}{2} \dot{q}^T V_1(q)^T V_1(q) \dot{q} + \frac{m_2}{2} \dot{q}^T V_2(q)^T V_2(q) \dot{q} + \frac{m_1l_1^2}{24}\dot{q}^T\Omega_1^T\Omega_1\dot{q} + \frac{m_2l_2^2}{24}\dot{q}^T\Omega_2^T\Omega_2\dot{q} = \frac{1}{2} \dot{q}^T H(q) \dot{q}, \end{equation} where the mass matrix $H$ is \begin{equation} H(q) = m_1\left(V_1(q)^T V_1(q) + \frac{l_1^2}{12}\Omega_1^T\Omega_1\right) + m_2\left(V_2(q)^T V_2(q) + \frac{l_2^2}{12}\Omega_2^T\Omega_2\right) \end{equation}

The potential energy

The potential energy of the double-pendulum is \begin{equation} U(q) = m_1gy_1 + m_2gy_2 = m_1g(q_4 +\frac{l_1}{2}c_1) + m_2g(q_4 + l_1c_1 + \frac{l_2}{2}c_{1+2}) \end{equation}

The Lagrangian and the equations of motion

The lagrangian is defined as the difference between the kinetic and potential energy \begin{equation} L = T-U = \frac{1}{2} \dot{q}^T H(q) \dot{q} - U(q) \end{equation} From this the four equations of motion are obtained by the differentiation \begin{equation} \frac{d}{dt} \frac {\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = \tau_i, \end{equation}

The first term in the equations of motion is obtained as \begin{equation} \frac {\partial L}{\partial \dot{q}_i} = e_i^TH(q)\dot{q} = H_i(q)^T\dot{q}, \end{equation} where $e_i$ is the vector that picks element $i$, i.e. a unit vetor with a one in element $i$. Hence, since the mass matrix is symmetric, $H_i(q)$ is the $i$th column of $H(q)$.

Differentiating w.r.t time, we get \begin{equation} \frac{d}{dt} \frac{\partial L}{\partial \dot{q}_i} = H_i(q)^T\ddot{q} + \left(\frac{\partial H_i}{\partial q}\dot{q}\right)^T\dot{q} = H_i(q)^T\ddot{q} + \Gamma_i(q,\dot{q})^T \dot{q}, \end{equation} where, again, $H_i(q)$ is the $i$th column of the mass matrix $H(q)$, and $\Gamma_i(q,\dot{q})$ is the column vector \begin{equation} \Gamma_i(q, \dot{q}) = \frac{\partial H_i(q)}{\partial q} \dot{q}. \end{equation} The second term becomes \begin{equation} \frac{\partial L}{\partial q_i} = \frac{1}{2}\dot{q}^T \frac{\partial H(q)}{\partial q_i} \dot{q} - \frac{\partial U(q)}{\partial q_i}. \end{equation}

Combining the terms for the equation of motion for coordinate $q_i$ we get \begin{equation} H_i(q)^T\ddot{q} + \Gamma_i(q,\dot{q})^T \dot{q} - \frac{1}{2}\dot{q}^T \frac{\partial H(q)}{\partial q_i} \dot{q} + \frac{\partial u(q)}{\partial q_i} = \tau_i \end{equation} or \begin{equation} H_i(q)^T\ddot{q} + C_i(q,\dot{q})^T \dot{q} + G_i(q) = \tau_i, \end{equation} where $C_i(q,\dot{q})$ is the column vector \begin{equation} C_i(q,\dot{q}) = \Gamma_i(q,\dot{q}) - \frac{1}{2}\frac{\partial H(q)}{\partial q_i}\dot{q} = \frac{\partial H_i(q)}{\partial q} \dot{q} - \frac{1}{2}\frac{\partial H(q)}{\partial q_i}\dot{q} = \left(\frac{\partial H_i(q)}{\partial q} -\frac{1}{2}\frac{\partial H(q)}{\partial q_i}\right)\dot{q}, \end{equation} and \begin{equation} G_i(q) = \frac{\partial U(q)}{\partial q_i}. \end{equation} Defining the matrix \begin{equation} C(q,\dot{q}) = \begin{bmatrix} C_1(q,\dot{q})^T \\ C_2(q,\dot{q})^T\\ C_3(q,\dot{q})^T\\ C_4(q,\dot{q})^T \end{bmatrix} \end{equation} and the vector \begin{equation} G(q) = \begin{bmatrix} G_1(q) \\ G_2(q)\\ G_3(q)\\ G_4(q) \end{bmatrix} \end{equation} The equations of motion can be written on the general manipulator form \begin{equation} H(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = \tau \end{equation}

The generalized forces

We separate the joint torques ($\tau_1$ and $\tau_2$, which are actuated) from the external forces ($F_x$, $F_y$), so \begin{equation} \tau = \begin{bmatrix} \tau_1 \\ \tau_2 \\ F_x\\F_y \end{bmatrix} \end{equation}

Center of mass dynamics

The location of center of mass is \begin{equation} c = \begin{bmatrix} c_x\\c_z \end{bmatrix} = \frac{m_1}{m_1+m_2} p_1(q) + \frac{m_2}{m_1+m_2} p_2(q). \end{equation}

Newton's equations of motion tell us that the acceleration of the CoM equals the sum of external forces acting on the system, and the rate of change of angular momentum of the body w.r.t CoM equals the sum of torques of the external forces w.r.t the CoM.

Let $L$ denote the angular momentum of the body, let \begin{equation} F = \begin{bmatrix} F_a\\M_a \end{bmatrix} \end{equation} be the external force and torque applied at the base joint (feet), and \begin{equation} a(q) = \begin{bmatrix} a_x\\ a_z \end{bmatrix} = p_b(q) - c(q) \end{equation} be the vector from the CoM to the base joint where the force is applied. The torque due to the force $F_a$ w.r.t CoM is then \begin{equation} M_y = a_zF_x - a_xF_z \end{equation}

The equations of motion can then be written \begin{equation} \begin{bmatrix} D_1\\ D_2 \end{bmatrix} F = \begin{bmatrix} (m_1+m_2)\ddot{c} - (m_1+m_2)g\\ \dot{L} \end{bmatrix}, \end{equation} where \begin{equation} D_1 = \begin{bmatrix} I & 0 \end{bmatrix} \end{equation} \begin{equation} D_2 = \begin{bmatrix} a_z & -a_x & 1 \end{bmatrix}. \end{equation} The vector $g$ is the gravity vector which points in the negative $z$-direction, i.e. \begin{equation} g = \begin{bmatrix} 0\\-9.8 \end{bmatrix} \end{equation}

Introducing \begin{equation} K(q) = \begin{bmatrix} D_1\\ D_2(q) \end{bmatrix}, \end{equation} and \begin{equation} u = \begin{bmatrix} (m_1+m_2)\ddot{c} - (m_1+m_2)g\\ \dot{L} \end{bmatrix}, \end{equation} the equation of motion of the CoM can be written compactly \begin{equation} K(q)F = u. \end{equation}

Determining allowable external forces that give desired CoM acceleration

The right hand side of the equations of motion \begin{equation} u = \begin{bmatrix} (m_1+m_2)\ddot{c} - (m_1+m_2)g\\ \dot{L} \end{bmatrix}, \end{equation} consists of known quantities ($m_1$, $m_2$, $g$) and specified, desired quantities ($\ddot{c}$ and $\dot{L}$). On the left hand side, the matrix $K(q)$ depends on the current configuration $q$, since \begin{equation} K(q) = \begin{bmatrix} D_1\\ D_2(q) \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\0 & 1 & 0\\ a_z(q) & -a_x(q) & 1 \end{bmatrix}. \end{equation}

Constraints

Not all forces found as solutions to \begin{equation} K(q)F = u \end{equation} are allowed, since they must satisfy constraints on the support polygon (base of support) and friction. For the base of support, the constraint takes the form \begin{equation} -d_x^- \le -\frac{M_y}{F_z} \le d_x^+, \end{equation} OBS: In Stephens and Atkeson eq (10) there is no minus sign in front of $d_x^-$, meaning it must be a negative number. Here, we take it to be a distance, hence positive.

$d_x^-$ is the lower limit of the base of support, and $d_x^+$ is the upper limit. This constraint is equivalent to \begin{equation} \frac{M_y}{F_z} \le d_x^- \end{equation} \begin{equation} -\frac{M_y}{F_z} \le d_x^+, \end{equation} which can be written \begin{equation} M_y - F_z d_x^- \le 0 \end{equation} \begin{equation} -M_y - F_z d_x^+ \le 0 \end{equation}

The constraint of friction in 3D is nonlinear. It states that the relative magnitude of the horizontal to the vertical force must be less than a fricion coefficient $\mu$ specific for the interface between feet and ground. \begin{equation} \left| \frac{\sqrt{F_x^2 + F_y^2}}{F_z} \right | \le \mu \end{equation} In the 2D case the constraint is linear, and can be written \begin{equation} \left| \frac{F_x}{F_z} \right | \le \mu, \end{equation} or \begin{equation} \frac{F_x}{F_z} \le \mu \end{equation} \begin{equation} -\frac{F_x}{F_z} \le \mu \end{equation} which can be written \begin{equation} F_x - \mu F_z \le 0 \end{equation} \begin{equation} -F_x - \mu F_z \le 0. \end{equation} where it is assumed that $F_z$ is positive.

The constraints can be written as the linear inequality \begin{equation} BF \le 0, \end{equation} where \begin{equation} B = \begin{bmatrix} 0 & -d_x^- & 1\\ 0 & -dx^+ &-1\\ 1 & -\mu & 0\\ -1 & -\mu & 0 \end{bmatrix} \end{equation}

Optimization problem

Determining the required contact forces can be formulated as the constrainted optimization problem (Stephens and Atkeson eq (15)) \begin{equation} \hat{F} = \arg \min_F (KF - u)^T(KF-u) + F^TWF, \end{equation} \begin{equation} \text{s.t.} \quad BF \le 0, \end{equation} The matrix $W$ can be used to weight the force differently, for instance to penalize horizontal forces or torques at the ankle joint.

The criterion can be rewritten as \begin{equation} f(F) = (KF - u)^T(KF-u) + F^TWF = F^T(W + K^TK)F - 2u^T K F + u^Tu, \end{equation} the last term does not depend on $F$ and can be ignored in the optimization. This gives the quadratic programming problem (QP) \begin{equation} \text{minimize} \quad F^T(W + K^TK)F - 2u^T K F, \end{equation} \begin{equation} \text{s.t.} \quad BF \le 0. \end{equation}

Verify solution to required contact forces, using standard single pendulum model

If the body is static in a certain position, and we require it to be static, then the desired acceleration is zero $\ddot{c}= 0$ and the desired change in angular momentum is also zero, $\dot{L} = 0$.

The force and momentum at the contact point (foot) must then be such that they balance the force of gravity, while achieving no acceleration of the body. The horizontal force must be zero, and the reaction force must go through the center of mass. This means that the center of pressure is directly below the center of mass.

The result from the optimization is a vector $\hat{F}$ consisting of the horizontal and vertical forces, and the moment about the $y$ - - axis. \begin{equation} \hat{F} = \begin{bmatrix} \hat{F}_x\\ \hat{F}_z \\ \hat{M}_y \end{bmatrix} \end{equation}

Consider a person (or robot) standing facing in the $x$ - direction. If $\hat{M}_y>0$, we must have a center of pressure which is behind the ankle joint. The person is leaning backwards, and the distance must be \begin{equation} \hat{d}_x = \frac{\hat{M}_y}{\hat{F}_z}. \end{equation} This must be the same as the horizontal distance between the CoM and the base joint.

Let's check this below.

Jacobian of base joint

The position of the base joint (ankle joint) is simply \begin{equation} p_b = \begin{bmatrix} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix} q_1\\q_2\\q_3\\q_4\end{bmatrix} \end{equation} and the velocity \begin{equation} v_b = \begin{bmatrix} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix} \dot{q}_1\\\dot{q}_2\\\dot{q}_3\\\dot{q}_4\end{bmatrix} \end{equation} The feet are asumed to be parallell to the ground, so the orientation is constant \begin{equation} \omega_b = 0 \end{equation}

Thus, the Jacobian is the matrix $J$ that transforms generalized velocities into velocity of the base joint \begin{equation} \begin{bmatrix} v_b\\\omega_b \end{bmatrix} = J \dot{q} = \begin{bmatrix} 0 & I_2\\0 & 0\end{bmatrix} \dot{q}. \end{equation}

Constraint equation for the generalized coordinates

The equations of motion for the mechanism must be accompanied by a constraint equation obtained by differentiating the equation \begin{equation} \begin{bmatrix} v_b\\\omega_b \end{bmatrix} = J \dot{q} = \begin{bmatrix} 0 & I_2\\0 & 0\end{bmatrix} \dot{q}. \end{equation} This leads to \begin{equation} \begin{bmatrix} \dot{v}_b\\\dot{\omega}_b \end{bmatrix} = J \ddot{q} + \dot{J}\dot{q}. \end{equation} Specifically, in our case \begin{equation} \begin{bmatrix} \dot{v}_b\\\dot{\omega}_b \end{bmatrix} = \begin{bmatrix} 0 & I_2\\0 & 0\end{bmatrix} \ddot{q}. \end{equation} The constraint equation simply tells us that the acceleration in the last two generalized coordinates (the position of the base) must be zero, since we assume that the feet are firmly on the ground.

Dynamic balance force control

We have worked out most of the preliminary detials, and are now ready to derive the dynamics balance force control.

Augmented equations of motion

The equations of motion of the mechanism with the constraint equation is \begin{equation} H(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = S\tau + J^T(q)F \end{equation} \begin{equation} J\ddot{q} + \dot{J}\dot{q} = \ddot{P}_b, \end{equation} where $\ddot{P}_b$ is the translational and angular acceleration of the base (the feet). The matrix $S$ picks the elements of $\tau$ that are actuated. In our case, since only the joints are actuated, not the position of the feet, gives \begin{equation} S = \begin{bmatrix} I_2 & 0\\ 0 & 0 \end{bmatrix}. \end{equation} The transpose of the Jacobian $J^T(q)$ relates the external force acting at the feet to forces at the joints. In our case this becomes \begin{equation} J^T(q) = \begin{bmatrix} 0 & 0\\ I_2 & 0\end{bmatrix}, \end{equation} and is not dependent on the generalized coordinates $q$. The transposed Jacobian tells us that of the forces and moment acting on the feet \begin{equation} F = \begin{bmatrix} F_x\\F_z\\M_y \end{bmatrix}, \end{equation} none give (or require) any torques at the joints. Only the generalized coordinates corresponding to the position of the feet are affected, and only by the forces, not by $M_y$.

The equations of motion of the mechanism are augmented with the equations of motion of the center of mass. The unknowns in this equation are the generalized accelerations $\ddot{q}$, the generalized forces $\tau$ and the external force $F$. The state of the robot, i.e. $q$ and $\dot{q}$, is assumed to be known. This gives \begin{equation} \begin{bmatrix} H(q) & -S & -J^T\\ J(q) & 0 & 0\\ 0 & 0 & D_1\\ 0 & 0 & D_2 \end{bmatrix} \begin{bmatrix} \ddot{q} \\ \tau \\ F \end{bmatrix} = \begin{bmatrix} -N(q, \dot{q}) \\ \ddot{P}_{\text{des}} - \dot{J}\dot{q} \\ m\ddot{c}_{\text{des}} + F_g, \\ \dot{L}_{\text{des}} \end{bmatrix} \end{equation} where $N(q, \dot{q})=C(q,\dot{q})\dot{q} + G(q)$.

Now, if we solve separately for $F$ using the formulation as an QP in the previous section, we can simplify the equations to \begin{equation} \begin{bmatrix} H(q) & -S\\ J & 0 \end{bmatrix} \begin{bmatrix} \ddot{q} \\ \tau \end{bmatrix} = \begin{bmatrix} -N(q, \dot{q}) + J^T\hat{F}\\ -\dot{J}\dot{q} \end{bmatrix} \end{equation}

In the case of our floating double pendulum, this equation becomes \begin{equation} \begin{bmatrix} H(q) & -\begin{bmatrix} I_2 & 0\\ 0 & 0 \end{bmatrix}\\ \begin{bmatrix} 0 & I_2\\0 & 0\end{bmatrix} & 0 \end{bmatrix} \begin{bmatrix} \ddot{q} \\ \tau \end{bmatrix} = \begin{bmatrix} -N(q, \dot{q}) + \begin{bmatrix} 0 & 0\\ I_2 & 0\end{bmatrix}\hat{F}\\ 0 \end{bmatrix} \end{equation}

The system of equation can be written \begin{equation} Dz = f, \end{equation} where $D$ is $8 \times 8$. It can be solved, with regularization matrix $W$ with a pseudo-inverse \begin{equation} z = (D^TD + W)^{-1}D^Tf. \end{equation}

The solution is \begin{equation} z = \begin{bmatrix} \ddot{q} \\ \tau_{\text{dbfc}} \end{bmatrix}, \end{equation} where $\tau_{\text{dbfc}}$ are the generalized torques used for feedback control.