def update(self, dt=None):
gforce = zeros(self._wndof)
for b in self._bodies():
#TODO: improve efficiency
from arboris.massmatrix import principalframe
H_bc = principalframe(b.mass)
R_gb = b.pose[0:3,0:3]
H_bc[0:3,0:3] = R_gb.T
#wrench_c = array((0,0,0,0,-9.81*b.mass[3,3],0))
#Ad_cb = homogeneousmatrix.iadjoint(H_bc)
#wrench_b = dot(Ad_cb.T, wrench_c)
#gforce += dot(b.jacobian.T, wrench_b )
# gravity acceleration expressed in body frame
g = dot(homogeneousmatrix.iadjoint(b.pose), self._gravity_dtwist)
gforce += dot(b.jacobian.T, dot(b.mass, g))
impedance = zeros( (self._wndof, self._wndof) )
return (gforce, impedance)
def djacobian(self):
# we assume self._bpose is constant
return dot(Hg.iadjoint(self._bpose), self._body._djacobian)
def twist(self):
return dot(Hg.iadjoint(self._bpose), self._body._twist)
def jacobian(self):
return dot(Hg.iadjoint(self._bpose), self._body._jacobian)
def update_dynamic(self, pose, jac, djac, twist):
r"""Sets the body ``pose, jac, djac, twist`` and computes its children ones.
This method (1) sets the body dynamical model (pose, jacobian,
hessian and twist) to the values given as argument, (2) computes
the dynamical model of the children bodies and (3) call the
equivalent method on them.
As a result, the dynamical model of all the bodies is computed
recursively.
:param pose: the body pose relative to the ground: `H_{gb}`
:type pose: 4x4 ndarray
:param jac: the body jacobian relative to the world (in body frame):
`\J[b]_{b/g}`
:type jac: 6x(ndof) ndarray
:param djac: the derivative of the body jacobian: `\dJ[b]_{b/g}`
:param twist: the body twist: `\twist[b]_{b/g}`
:type twist: 6 ndarray
**Algorithm:**
Let's define the following notations:
- `g`: the ground body,
- `p`: the parent body (which is the present :class:`arboris.Body`
instance)
- `c`: a child body,
- `j`: the joint between the bodies `p` and `c`,
- `r`: reference frame of the joint `j`, rigidly fixed to the parent
body
- `n`: new frame of the joint `j`, rigidly fixed to the child body
.. image:: img/body_model.png
One can notice that `H_{nc}` and `H_{pr}` are constant.
The child body pose can be computed as
.. math::
H_{gc} &= H_{gp} \; H_{pc} \\
&= H_{gp} \; (H_{pr} \; H_{rn} \; H_{nc})
where `H_{rn}` depends on the joint generalized configuration and is
given by its :attr:`~arboris.core.Joint.pose` attribute.
The chil body twist is given as
.. math::
\twist[c]_{c/g} &= \Ad[c]_p \; \twist[p]_{p/g} + \twist[c]_{c/p} \\
&= \Ad[c]_p \; \twist[p]_{p/g} + \Ad[c]_n \; \twist[n]_{n/r} \\
&= \Ad[c]_p \; \J[p]_{p/g} \; \GVel
+ \Ad[c]_n \; \J[n]_{n/r} \; \GVel_j \\
&= \J[c]_{c/g} \; \GVel
where `\twist[n]_{n/r}` isgiven by the joint
:attr:`~arboris.core.Joint.twist` attribute.
\GVel_j is the generalized velocity of the joint `j` and is
related to the world generalized velocity by trivial projection
.. math::
\GVel_j &=
\begin{bmatrix}
0 & \cdots &0 & I & 0 & \cdots & 0
\end{bmatrix} \; \GVel
therefore, the child body jacobian is
.. math::
\J[c]_{c/g} &= \Ad[c]_p \; \J[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & \Ad[c]_n \; \J[n]_{n/r} & 0 & \cdots & 0
\end{bmatrix} \\
where `\J[n]_{n/r}` is given by the joint
:attr:`~arboris.core.Joint.jacobian` attribute. Derivating the previous
expression leads to the child body acceleration:
.. math::
\dtwist[c]_{c/g} &= \dAd[c]_p \; \J[p]_{p/g} \; \GVel
+ \Ad[c]_p \; \dJ[p]_{p/g} \; \GVel
+ \Ad[c]_p \; \J[p]_g \; \dGVel
+ \Ad[c]_n \; \dJ[n]_{n/r} \; \GVel_j
+ \Ad[c]_n \; \J[n]_{m/r} \dGVel_j \\
&= \J[c]_{c/g} \; \dGVel + \dJ[c]_{c/g} \; \GVel
the expression of the child body hessian is then obtained by
identification:
.. math::
\dJ[c]_{c/g} \; \GVel
&= \dAd[c]_p \; \J[p]_{p/g} \; \GVel
+ \Ad[c]_p \; \dJ[p]_{p/g} \; \GVel
+ \Ad[c]_n \; \dJ[n]_{n/r} \; \GVel_j \\
\dJ[c]_{c/g}
&= \dAd[c]_p \; \J[p]_{p/g} + \Ad[c]_p \; \dJ[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & (\Ad[c]_n \; \dJ[n]_{n/r}) & 0 & \cdots & 0
\end{bmatrix}
with
.. math::
\dAd[c]_p &= \Ad[c]_n \; \dAd[n]_r \; \Ad[r]_p
and where `\dAd[n]_r` and `\dJ[n]_{n/r}` are respectively given by
the joint :attr:`~arboris.core.Joint.idadjoint` and
:attr:`~arboris.core.Joint.djacobian` attributes.
T_ab: velocity of {a} relative to {b} expressed in {a} (body twist)
"""
self._pose = pose
self._jacobian = jac
self._djacobian = djac
self._twist = twist
wx = array(
[[ 0,-self.twist[2], self.twist[1]],
[ self.twist[2], 0,-self.twist[0]],
[-self.twist[1], self.twist[0], 0]])
if self.mass[3,3]<=1e-10: #TODO: avoid hardcoded value
rx = zeros((3,3))
else:
rx = self.mass[0:3,3:6]/self.mass[3,3] #TODO: better solution?
self._nleffects = zeros((6,6))
self._nleffects[0:3,0:3] = wx
self._nleffects[3:6,3:6] = wx
self._nleffects[0:3,3:6] = dot(rx,wx) - dot(wx,rx)
self._nleffects = dot(self.nleffects, self.mass)
H_gp = pose
J_pg = jac
dJ_pg = djac
T_pg = twist
for j in self.childrenjoints:
H_cn = j._frame1.bpose
H_pr = j._frame0.bpose
H_rn = j.pose
H_pc = dot(H_pr, dot(H_rn, Hg.inv(H_cn)))
child_pose = dot(H_gp, H_pc)
Ad_cp = Hg.iadjoint(H_pc)
Ad_cn = Hg.adjoint(H_cn)
Ad_rp = Hg.adjoint(Hg.inv(H_pr))
dAd_nr = j.idadjoint
dAd_cp = dot(Ad_cn, dot(dAd_nr, Ad_rp))
T_nr = j.twist
J_nr = j.jacobian
dJ_nr = j.djacobian
child_twist = dot(Ad_cp, T_pg) + dot(Ad_cn, T_nr)
child_jac = dot(Ad_cp, J_pg)
child_jac[:,j.dof] += dot(Ad_cn, J_nr)
child_djac = dot(dAd_cp, J_pg) + dot(Ad_cp, dJ_pg)
child_djac[:,j.dof] += dot(Ad_cn, dJ_nr)
j._frame1.body.update_dynamic(child_pose, child_jac, child_djac,
child_twist)