# Equations of motion of a floating double pendulum

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

Let $p_1$ and $p_2$ denote the position of CoM of link 1 and link 2, respectively. We get $$p_1 = \begin{bmatrix} q_3 + \frac{l_1}{2}s_1\\ q_4 + \frac{l_1}{2}c_1 \end{bmatrix}$$ $$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}$$ The center of mass of the double pendulum becomes $$p_c = \frac{m_1}{m_2+m_2}p_1 + \frac{m_2}{m_1 + m_2} p_2.$$

The velocity of the centers of mass: $$\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}$$ $$\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}$$

The rotational velocities: $$\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}.$$ $$\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}.$$

## 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 $$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},$$ where the mass matrix $H$ is $$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)$$

## The potential energy

The potential energy of the double-pendulum is $$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})$$

## The Lagrangian and the equations of motion

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

The first term in the equations of motion is obtained as $$\frac {\partial L}{\partial \dot{q}_i} = e_i^TH(q)\dot{q} = H_i(q)^T\dot{q},$$ 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 $$\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},$$ 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 $$\Gamma_i(q, \dot{q}) = \frac{\partial H_i(q)}{\partial q} \dot{q}.$$ The second term becomes $$\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}.$$

Combining the terms for the equation of motion for coordinate $q_i$ we get $$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$$ or $$H_i(q)^T\ddot{q} + C_i(q,\dot{q})^T \dot{q} + G_i(q) = \tau_i,$$ where $C_i(q,\dot{q})$ is the column vector $$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},$$ and $$G_i(q) = \frac{\partial U(q)}{\partial q_i}.$$ Defining the matrix $$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}$$ and the vector $$G(q) = \begin{bmatrix} G_1(q) \\ G_2(q)\\ G_3(q)\\ G_4(q) \end{bmatrix}$$ The equations of motion can be written on the general manipulator form $$H(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = \tau$$

## 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 $$\tau = \begin{bmatrix} \tau_1 \\ \tau_2 \\ F_x\\F_y \end{bmatrix}$$



In [1]:

import numpy as np
import sympy as sy




In [2]:

# In the implementation, we will make the model more general, by allowing the moment of inertia about the
# respective center of mass to be specified. Also, the position of the CoM from the pivot of each link
# is given as a variable 0 < alpha_i <= 1. So, alpha_i=1 means that the CoM of the link is at the end of the link.

q1, q2, q3, q4, qd1, qd2, qd3, qd4 = sy.symbols('q1, q2, q3, q4, qd1, qd2, qd3, qd4', real=True)
qdd1, qdd2, qdd3, qdd4 = sy.symbols('qdd1, qdd2, qdd3, qdd4', real=True)
g, m1, m2, l1, l2, I1, I2, a1, a2 = sy.symbols('g, m1, m2, l1, l2, I1, I2, alpha1, alpha2', real=True, positive=True)
tau1, tau2, Fx, Fy = sy.symbols('tau1, tau2, Fx, Fy', real=True)

q = sy.Matrix([[q1],[q2],[q3], [q4]])
qd = sy.Matrix([[qd1],[qd2],[qd3], [qd4]])
qdd = sy.Matrix([[qdd1],[qdd2],[qdd3], [qdd4]])
tau = sy.Matrix([])
c1 = sy.cos(q1)
s1 = sy.sin(q1)
c12 = sy.cos(q1+q2)
s12 = sy.sin(q1+q2)

p1 = sy.Matrix([[q3 +a1*l1*s1],[q4+a1*l1*c1]])
p2 = sy.Matrix([[q3 +l1*s1 + a2*l2*s12],[q4+l1*c1 + a2*l2*c12]])
V1 = sy.Matrix([[a2*l1*c1, 0, 1, 0],[-a2*l1*s1, 0, 0, 1]])
pd1 = V1*qd
V2 = sy.Matrix([[l1*c1+a2*l2*c12, a2*l2*c12, 1, 0 ],[-l1*s1-a2*l2*s12, -a2*l2*s12, 0, 1]])
pd2 = V2*qd
Omega1 = sy.Matrix([[1, 0, 0 , 0]])
w1 = Omega1*qd
Omega2 = sy.Matrix([[1, 1, 0 , 0]])
w2 = Omega2*qd

H = m1*V1.T*V1 + m2*V2.T*V2 + I1*Omega1.T*Omega1 + I2*Omega2.T*Omega2

T = 0.5*qd.T*H*qd
U = sy.Matrix([m1*g*p1[1] + m2*g*p2[1]])

# The equations of motion on manipulator form
C = sy.zeros(4,4)
G = sy.zeros(4,1)

for i in range(4):
qi = q[i]
Hi = H[:,i]
Gammai = Hi.jacobian(q)*qd
ddtdLdqidot = Hi.T*qdd + Gammai.T*qd
dHdqi = H.diff(qi)
Di = 0.5*dHdqi*qd
Gi = U.diff(qi)
dLdqi = Di.T*qd - Gi
#lhs1 = ddtdLdq1dot - dLdq1
# Form the terms for the manipulator form
Ci = Gammai - Di
C[i,:] = Ci.T
G[i] = Gi

#dd

lhs = H*qdd + C*qd + G




In [3]:

## Verify calculations by comparing to equations of motion given explicitly in Tedrake 2009
Htest = H.subs(I1,0).subs(I2,0).subs(a1,1).subs(a2,1)
Ctest = C.subs(I1,0).subs(I2,0).subs(a1,1).subs(a2,1).subs(qd3,0).subs(qd4,0)
Gtest = G.subs(I1,0).subs(I2,0).subs(a1,1).subs(a2,1).subs(qd3,0).subs(qd4,0)

print "H"
sy.pretty_print(sy.trigsimp(Htest[0,0]))
sy.pretty_print(sy.trigsimp(Htest[1,0]))
sy.pretty_print(sy.trigsimp(Htest[1,1]))
# OK!

print "C"
sy.pretty_print(sy.trigsimp(Ctest[0,0])*qd[0] + sy.trigsimp(Ctest[0,1])*qd[1])
# OK!

print "G"
sy.pretty_print(sy.trigsimp(Gtest))
# OK!




H
2        2                             2
l₁ ⋅m₁ + l₁ ⋅m₂ + 2⋅l₁⋅l₂⋅m₂⋅cos(q₂) + l₂ ⋅m₂
l₂⋅m₂⋅(l₁⋅cos(q₂) + l₂)
2
l₂ ⋅m₂
C
2
-2⋅l₁⋅l₂⋅m₂⋅qd₁⋅qd₂⋅sin(q₂) - l₁⋅l₂⋅m₂⋅qd₂ ⋅sin(q₂)
G
⎡-g⋅(l₁⋅m₁⋅sin(q₁) + l₁⋅m₂⋅sin(q₁) + l₂⋅m₂⋅sin(q₁ + q₂))⎤
⎢                                                       ⎥
⎢                 -g⋅l₂⋅m₂⋅sin(q₁ + q₂)                 ⎥
⎢                                                       ⎥
⎢                           0                           ⎥
⎢                                                       ⎥
⎣                      g⋅m₁ + g⋅m₂                      ⎦



# Center of mass dynamics

The location of center of mass is $$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).$$

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 $$F = \begin{bmatrix} F_a\\M_a \end{bmatrix}$$ be the external force and torque applied at the base joint (feet), and $$a(q) = \begin{bmatrix} a_x\\ a_z \end{bmatrix} = p_b(q) - c(q)$$ 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 $$M_y = a_zF_x - a_xF_z$$

The equations of motion can then be written $$\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},$$ where $$D_1 = \begin{bmatrix} I & 0 \end{bmatrix}$$ $$D_2 = \begin{bmatrix} a_z & -a_x & 1 \end{bmatrix}.$$ The vector $g$ is the gravity vector which points in the negative $z$-direction, i.e. $$g = \begin{bmatrix} 0\\-9.8 \end{bmatrix}$$

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



In [4]:

Fa,Ma,L,Ld,cddx,cddz = sy.symbols('Fa, Ma, L, Ld, cddx, cddz', real=True)
m = m1+m2
pc = m1/m * p1 + m2/m * p2
pb = sy.Matrix([q3,q4]) # Column vector
pa = pb-pc
D1 = sy.Matrix([[1, 0, 0], [0, 1, 0]])
D2 = sy.Matrix([[pa[1], -pa[0], 1]])
gvec = g*sy.Matrix([0,-1])

# Abbreviation as in Stephens and Atkeson 2010: KF = u
K = D1.col_join(D2)
u = (m*sy.Matrix([cddx, cddz])-m*gvec).col_join(sy.Matrix([[Ld]]))



# Determining allowable external forces that give desired CoM acceleration

The right hand side of the equations of motion $$u = \begin{bmatrix} (m_1+m_2)\ddot{c} - (m_1+m_2)g\\ \dot{L} \end{bmatrix},$$ 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 $$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}.$$

## Constraints

Not all forces found as solutions to $$K(q)F = u$$ 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 $$-d_x^- \le -\frac{M_y}{F_z} \le d_x^+,$$ 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 $$\frac{M_y}{F_z} \le d_x^-$$ $$-\frac{M_y}{F_z} \le d_x^+,$$ which can be written $$M_y - F_z d_x^- \le 0$$ $$-M_y - F_z d_x^+ \le 0$$

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. $$\left| \frac{\sqrt{F_x^2 + F_y^2}}{F_z} \right | \le \mu$$ In the 2D case the constraint is linear, and can be written $$\left| \frac{F_x}{F_z} \right | \le \mu,$$ or $$\frac{F_x}{F_z} \le \mu$$ $$-\frac{F_x}{F_z} \le \mu$$ which can be written $$F_x - \mu F_z \le 0$$ $$-F_x - \mu F_z \le 0.$$ where it is assumed that $F_z$ is positive.

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

# Optimization problem

Determining the required contact forces can be formulated as the constrainted optimization problem (Stephens and Atkeson eq (15)) $$\hat{F} = \arg \min_F (KF - u)^T(KF-u) + F^TWF,$$ $$\text{s.t.} \quad BF \le 0,$$ 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 $$f(F) = (KF - u)^T(KF-u) + F^TWF = F^T(W + K^TK)F - 2u^T K F + u^Tu,$$ the last term does not depend on $F$ and can be ignored in the optimization. This gives the quadratic programming problem (QP) $$\text{minimize} \quad F^T(W + K^TK)F - 2u^T K F,$$ $$\text{s.t.} \quad BF \le 0.$$



In [5]:

import cvxopt as cvx

dxmin,dxplus,mu = sy.symbols('dxmin, dxplus, mu')
# The general formula for the QP when using cvxopt is
# minimize  0.5 x^T Q x + p x,
# subject to G x <= h,     B x z= 0 here
#            A x = b       not used here
# Determine matrices for the QP
lmbda = 0.001  # Used to make sure the weighting matrix W is not too big wrt K^TK
WW = sy.Matrix([[1, 0, 0],[0, 1, 0],[0,0,1]])*lmbda
QQ = 2*(WW + K.T*K)
pp = (-2*u.T*K).T
GG = sy.Matrix([[0, -dxmin, 1], [0, -dxplus, -1], [1, -mu, 0], [-1, -mu, 0]])
hh = sy.Matrix([0,0,0,0])

# Numerical example using cvxopt
#q1_ = np.pi/12
q1_ = 0
q2_ = 0
q3_ = 0.5
q4_ = 0.5
cdd = 1 # Magnitude of desired acceleration
cddx_ = -1.
cddz_ = 0.
Ld_ = 200.

g_ = 9.799
m1_ = 35.
m2_ = 40.
l1_ = 1.
l2_ = 1.
alpha1_ = 1.
alpha2_ = 1.
mu_ = 0.6
dxmin_ = 0.08
dxplus_ = 0.16

# Substitue numerical values
QQ_ = QQ.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)
QQ_ = QQ_.subs(a2, alpha2_)
pp_ = pp.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)
pp_ = pp_.subs(a2, alpha2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_)
GG_ = GG.subs(dxmin, dxmin_).subs(dxplus, dxplus_).subs(mu, mu_)
sy.pretty_print(QQ_)
sy.pretty_print(pp_)
sy.pretty_print(GG_)

# Convert to numpy
QQn = sy.matrix2numpy(QQ_)
ppn = sy.matrix2numpy(pp_)
GGn = sy.matrix2numpy(GG_)
hhn = sy.matrix2numpy(hh)

# And then to cvxopt matrices
print QQn.dtype
QQc = cvx.matrix(QQn.astype(np.float64))
ppc = cvx.matrix(ppn.astype(np.float64))
GGc = cvx.matrix(GGn.astype(np.float64))
hhc = cvx.matrix(hhn.astype(np.float64))

sol=cvx.solvers.qp(QQc, ppc, GGc, hhc)
# Try also unconstrained problem
solU = cvx.solvers.qp(QQc, ppc)
print "Constrained solution"
print sol['x']
print "Unconstrained solution"
print solU['x']




⎡6.70422222222222     0    -3.06666666666667⎤
⎢                                           ⎥
⎢        0          2.002          0        ⎥
⎢                                           ⎥
⎣-3.06666666666667    0          2.002      ⎦
⎡763.333333333333⎤
⎢                ⎥
⎢    -1469.85    ⎥
⎢                ⎥
⎣     -400.0     ⎦
⎡0   -0.08  1 ⎤
⎢             ⎥
⎢0   -0.16  -1⎥
⎢             ⎥
⎢1   -0.6   0 ⎥
⎢             ⎥
⎣-1  -0.6   0 ⎦
object
pcost       dcost       gap    pres   dres
0: -5.4194e+05 -6.1733e+05  8e+04  2e-14  3e-01
1: -5.8469e+05 -5.8861e+05  4e+03  1e-13  4e-03
2: -5.8490e+05 -5.8504e+05  1e+02  2e-13  1e-04
3: -5.8498e+05 -5.8498e+05  4e+00  8e-14  1e-06
4: -5.8498e+05 -5.8498e+05  4e-02  4e-14  1e-08
Optimal solution found.
Constrained solution
[-8.70e+01]
[ 7.35e+02]
[ 5.88e+01]

Unconstrained solution
[-7.51e+01]
[ 7.34e+02]
[ 8.48e+01]



### 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. $$\hat{F} = \begin{bmatrix} \hat{F}_x\\ \hat{F}_z \\ \hat{M}_y \end{bmatrix}$$

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 $$\hat{d}_x = \frac{\hat{M}_y}{\hat{F}_z}.$$ This must be the same as the horizontal distance between the CoM and the base joint.

Let's check this below.



In [6]:

import matplotlib.pyplot as plt
import double_pendulum as dpu




/home/kjartan/anaconda2/lib/python2.7/site-packages/pyface/ui/wx/init.py:25: wxPyDeprecationWarning: Using deprecated class PySimpleApp.
_app = wx.PySimpleApp()
/home/kjartan/anaconda2/lib/python2.7/site-packages/pyface/ui/wx/init.py:29: wxPyDeprecationWarning: Call to deprecated item 'InitAllImageHandlers'.
wx.InitAllImageHandlers()




In [13]:

# Set desired accelerations to zero
# Numerical example using cvxopt
#q1_ = np.pi/12
alpha1_ = 0.7
alpha2_ = 0.6
q1_ = -np.pi/12
q2_ = np.pi/4
q3_ = 0.5
q4_ = 0.5
# Desired acceleration
cddx_ = 2.
cddz_ = 0.
Ld_ = 0.

# Substitue numerical values
QQ_ = QQ.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)
QQ_ = QQ_.subs(a2, alpha2_)
pp_ = pp.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)
pp_ = pp_.subs(a2, alpha2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_)
GG_ = GG.subs(dxmin, dxmin_).subs(dxplus, dxplus_).subs(mu, mu_)

# Convert to numpy
QQn = sy.matrix2numpy(QQ_, dtype=np.float64)
ppn = sy.matrix2numpy(pp_, dtype=np.float64)
GGn = sy.matrix2numpy(GG_, dtype=np.float64)
hhn = sy.matrix2numpy(hh, dtype=np.float64)

# And then to cvxopt matrices
QQc = cvx.matrix(QQn)
ppc = cvx.matrix(ppn)
GGc = cvx.matrix(GGn)
hhc = cvx.matrix(hhn)

sol=cvx.solvers.qp(QQc, ppc, GGc, hhc)
# Try also unconstrained problem
solU = cvx.solvers.qp(QQc, ppc)
print "Constrained solution"
print sol['x']
print "Unconstrained solution"
print solU['x']

# Verify that My/Fz equals distance in x-dir between CoM and base joint
sy.pretty_print(pa)
pa_ = pa.subs(q1, q1_).subs(q2, q2_).subs(q3, q3_).subs(q4, q4_).subs(a1, alpha1_).subs(a2, alpha2_)
pa_ = pa_.subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_)
pa_n = sy.matrix2numpy(pa_, dtype=np.float64)

print "My/Fz - ax = %f" % (sol['x'][2]/sol['x'][1] - pa_n[0])

Fx = sol['x'][0]
Fz = sol['x'][1]
My = sol['x'][2]

#Fx = solU['x'][0]
#Fz = solU['x'][1]
#My = solU['x'][2]

dp = dpu.DoublePendulum()
dp.L1 = l1_
dp.L2 = l2_
dp.a1 = alpha1_
dp.a2 = alpha2_
dp.m1 = m1_
dp.m2 = m2_
animation = dpu.DoublePendulumAnimation(dp)
animation.init_dp(q1_, q2_, q3_, q4_, Fx, Fz, My)

plt.show()




pcost       dcost       gap    pres   dres
0: -5.0689e+05 -5.6534e+05  6e+04  4e-14  4e-01
1: -5.5086e+05 -5.5555e+05  5e+03  9e-14  3e-17
2: -5.5149e+05 -5.5155e+05  6e+01  9e-14  3e-17
3: -5.5150e+05 -5.5150e+05  6e-01  6e-14  5e-17
4: -5.5150e+05 -5.5150e+05  6e-03  2e-13  1e-16
Optimal solution found.
Constrained solution
[ 7.37e+01]
[ 7.35e+02]
[ 5.88e+01]

Unconstrained solution
[ 1.50e+02]
[ 7.34e+02]
[ 2.11e+02]

⎡  m₁⋅(α₁⋅l₁⋅sin(q₁) + q₃)   m₂⋅(α₂⋅l₂⋅sin(q₁ + q₂) + l₁⋅sin(q₁) + q₃)     ⎤
⎢- ─────────────────────── - ───────────────────────────────────────── + q₃⎥
⎢          m₁ + m₂                            m₁ + m₂                      ⎥
⎢                                                                          ⎥
⎢  m₁⋅(α₁⋅l₁⋅cos(q₁) + q₄)   m₂⋅(α₂⋅l₂⋅cos(q₁ + q₂) + l₁⋅cos(q₁) + q₄)     ⎥
⎢- ─────────────────────── - ───────────────────────────────────────── + q₄⎥
⎣          m₁ + m₂                            m₁ + m₂                      ⎦
My/Fz - ax = 0.017416




In [ ]:




In [8]:

# Check solution
Fhat = sy.Matrix(solU['x'])
K_ = K.subs(q1, q1_).subs(q2, q2_).subs(m1, m1_).subs(m2, m2_).subs(l1, l1_).subs(l2, l2_).subs(a1, alpha1_)
K_ = K_.subs(a2, alpha2_)
# Convert to numpy
Kn = sy.matrix2numpy(K_, dtype = np.float64)
print "det K = %f" % np.linalg.det(Kn)
print "uhat = "
uhat = K_*Fhat

sy.pretty_print(uhat)
un = sy.matrix2numpy(u.subs(m1, m1_).subs(m2,m2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_),
dtype = np.float64)
print "u_n ="
print un
Fn = np.linalg.solve(Kn, un)
print "F_n ="
print Fn

uhatn = np.dot(Kn, Fn)
print "uhat_n = "
print uhatn

print "K_ = "
sy.pretty_print(K_)
print "K'*K = "
sy.pretty_print(K_.T*K_)




det K = 1.000000
uhat =
⎡-0.116569624340393⎤
⎢                  ⎥
⎢ 734.17651532825  ⎥
⎢                  ⎥
⎣-0.102374946774901⎦
u_n =
[[   0.   ]
[ 734.925]
[   0.   ]]
F_n =
[[  5.19626687e-15]
[  7.34925000e+02]
[  1.02714797e+02]]
uhat_n =
[[  5.19626687e-15]
[  7.34925000e+02]
[  0.00000000e+00]]
K_ =
⎡       1                  0           0⎤
⎢                                       ⎥
⎢       0                  1           0⎥
⎢                                       ⎥
⎣-1.1397924750211  -0.139762284355361  1⎦
K'*K =
⎡2.29912688611473        0.1593         -1.1397924750211 ⎤
⎢                                                        ⎥
⎢     0.1593        1.01953349612823   -0.139762284355361⎥
⎢                                                        ⎥
⎣-1.1397924750211  -0.139762284355361          1         ⎦




In [9]:

sy.pretty_print(c)

sy.pretty_print(u)
sy.pretty_print(K)
sy.pretty_print(u.subs(m1, m1_).subs(m2,m2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_))




---------------------------------------------------------------------------
NameError                                 Traceback (most recent call last)
<ipython-input-9-c34e0a83ce98> in <module>()
----> 1 sy.pretty_print(c)
2
3 sy.pretty_print(u)
4 sy.pretty_print(K)
5 sy.pretty_print(u.subs(m1, m1_).subs(m2,m2_).subs(cddx, cddx_).subs(cddz, cddz_).subs(Ld, Ld_).subs(g, g_))

NameError: name 'c' is not defined



# Jacobian of base joint

The position of the base joint (ankle joint) is simply $$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}$$ and the velocity $$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}$$ The feet are asumed to be parallell to the ground, so the orientation is constant $$\omega_b = 0$$

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

# 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{bmatrix} v_b\\\omega_b \end{bmatrix} = J \dot{q} = \begin{bmatrix} 0 & I_2\\0 & 0\end{bmatrix} \dot{q}.$$ This leads to $$\begin{bmatrix} \dot{v}_b\\\dot{\omega}_b \end{bmatrix} = J \ddot{q} + \dot{J}\dot{q}.$$ Specifically, in our case $$\begin{bmatrix} \dot{v}_b\\\dot{\omega}_b \end{bmatrix} = \begin{bmatrix} 0 & I_2\\0 & 0\end{bmatrix} \ddot{q}.$$ 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 $$H(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) = S\tau + J^T(q)F$$ $$J\ddot{q} + \dot{J}\dot{q} = \ddot{P}_b,$$ 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 $$S = \begin{bmatrix} I_2 & 0\\ 0 & 0 \end{bmatrix}.$$ 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 $$J^T(q) = \begin{bmatrix} 0 & 0\\ I_2 & 0\end{bmatrix},$$ and is not dependent on the generalized coordinates $q$. The transposed Jacobian tells us that of the forces and moment acting on the feet $$F = \begin{bmatrix} F_x\\F_z\\M_y \end{bmatrix},$$ 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{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}$$ 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{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}$$

In the case of our floating double pendulum, this equation becomes $$\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}$$

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

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



In [ ]:

# Let's implement the equations and test a few things.
N = C*qd + G
J = sy.Matrix([[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
S = sy.Matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]])
Fxs, Fzs, Mys = sy.symbols('Fx, Fz, My')
Fhat = sy.Matrix([Fxs, Fzs, Mys])
Dr1 = H.row_join(-S)
Dr2 = J.row_join(sy.zeros(3,4))
D = Dr1.col_join(Dr2)
f1 = -N+J.T*Fhat
f = f1.col_join(sy.zeros(3,1))

# Substitue numerical values
qd1_ = 0.1
qd2_ = 0.2
qd3_ = 0
qd4_ = 0
I1_ = 1
I2_ = 2
numvals = {q1:q1_, q2:q2_, q3:q3_, q4:q4_, qd1:qd1_, qd2:qd2_, qd3:qd3_, qd4:qd4_,
m1:m1_, m2:m2_, l1:l1_, l2:l2_, a1:alpha1_, a2:alpha2_, I1:I1_, I2:I2_,
g:9.8, Fxs:10, Fzs:800, Mys:100}

D_ = D.subs(numvals)
f_ = f.subs(numvals)
sy.pretty_print(D_)
sy.pretty_print(f_)
print D_.rank()
Wn = 0.1*np.eye(8)
Dn = sy.matrix2numpy(D_, dtype=np.float64)
fn = sy.matrix2numpy(f_, dtype=np.float64)

z = np.linalg.solve(np.dot(Dn.T, Dn) + Wn, np.dot(Dn.T, fn))
print z




In [ ]:

np.linalg.solve?




In [ ]: