def _extract_collision_shapes_data(self):
"""
"""
cs = self.collision_shapes
sdist = zeros(len(cs))
svel = zeros(len(cs))
J = zeros((len(cs), self.ndof))
dJ = zeros((len(cs), self.ndof))
for i in arange(len(self.collision_shapes)):
sdist[i], Hgc0, Hgc1 = cs[i][1](cs[i][0])
sdist[i] -= self.options['avoidance margin']
f0, f1 = cs[i][0][0].frame, cs[i][0][1].frame
Hf0c0, Hf1c1 = dot(inv(f0.pose), Hgc0), dot(inv(f1.pose), Hgc1)
tf0_g_f0, tf1_g_f1 = f0.twist, f1.twist
Adc0f0, Adc1f1 = adjoint(inv(Hf0c0)), adjoint(inv(Hf1c1))
tc0_g_c0, tc1_g_c1 = dot(Adc0f0, tf0_g_f0), dot(Adc1f1, tf1_g_f1)
Jc0, Jc1 = dot(Adc0f0, f0.jacobian), dot(Adc1f1, f1.jacobian)
#as Tc_f_c = 0, no motion between the 2 frames because same body
dJc0, dJc1 = dot(Adc0f0, f0.jacobian), dot(Adc1f1, f1.jacobian)
svel[i] = tc1_g_c1[5] - tc0_g_c0[5]
dJ[i, :] = dJc1[5] - dJc0[5]
if f1.body in self.bodies:
J[i, :] += Jc1[5]
if f0.body in self.bodies:
J[i, :] -= Jc0[5]
return sdist, svel, J, dJ
def _extract_collision_shapes_data(self):
"""
"""
cs = self.collision_shapes
sdist = zeros(len(cs))
svel = zeros(len(cs))
J = zeros((len(cs), self.ndof))
dJ = zeros((len(cs), self.ndof))
for i in arange(len(self.collision_shapes)):
sdist[i], Hgc0, Hgc1 = cs[i][1](cs[i][0])
sdist[i] -= self.options['avoidance margin']
f0, f1 = cs[i][0][0].frame, cs[i][0][1].frame
Hf0c0, Hf1c1 = dot(inv(f0.pose), Hgc0), dot(inv(f1.pose), Hgc1)
tf0_g_f0, tf1_g_f1 = f0.twist, f1.twist
Adc0f0, Adc1f1 = adjoint(inv(Hf0c0)), adjoint(inv(Hf1c1))
tc0_g_c0, tc1_g_c1 = dot(Adc0f0, tf0_g_f0), dot(Adc1f1, tf1_g_f1)
Jc0, Jc1 = dot(Adc0f0, f0.jacobian), dot(Adc1f1, f1.jacobian)
#as Tc_f_c = 0, no motion between the 2 frames because same body
dJc0, dJc1 = dot(Adc0f0, f0.jacobian), dot(Adc1f1, f1.jacobian)
svel[i] = tc1_g_c1[5] - tc0_g_c0[5]
dJ[i, :] = dJc1[5] - dJc0[5]
if f1.body in self.bodies:
J[i, :] += Jc1[5]
if f0.body in self.bodies:
J[i, :] -= Jc0[5]
return sdist, svel, J, dJ
def add_body(name, mass, com_position, gyration_radius):
#mass matrix at com
mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1))))
H_fg = eye(4)
H_fg[0:3, 3] = com_position
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(Hg.adjoint(H_gf).T, dot(mass_g, Hg.adjoint(H_gf)))
bodies.append(Body(name=prefix+name, mass=mass_o))
def add_body(name, mass, com_position, gyration_radius):
#mass matrix at com
mass_g = mass * diag(hstack((gyration_radius**2, (1, 1, 1))))
H_fg = eye(4)
H_fg[0:3, 3] = com_position
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(Hg.adjoint(H_gf).T, dot(mass_g, Hg.adjoint(H_gf)))
bodies.append(Body(name=prefix + name, mass=mass_o))
def _update_Jc(self):
""" Extract the Jacobian and dJacobian matrix of contact points.
"""
self.Jc[:] = 0.
self.dJc[:] = 0.
i = 0
for c in self.constraints:
if c.is_enabled() and c.is_active() and self.is_enabled[c]:
frame0, frame1 = c._frames[0], c._frames[1]
if isinstance(c, PointContact):
self.dJc[3*i:(3*(i+1)), :] = frame1.djacobian[3:6] - \
frame0.djacobian[3:6]
if frame1.body in self.bodies:
self.Jc [3*i:(3*(i+1)), :] += frame1.jacobian[3:6]
if frame0.body in self.bodies:
self.Jc [3*i:(3*(i+1)), :] -= frame0.jacobian[3:6]
elif isinstance(c, BallAndSocketConstraint):
H1_0 = dot(inv(frame1.pose), frame0.pose)
Ad1_0 = adjoint(H1_0)
Ad0_1 = iadjoint(H1_0)
T0_g_0 = frame0.twist
T1_g_1 = frame1.twist
T1_g_0 = dot(Ad0_1, T1_g_1)
T0_1_0 = T0_g_0 - T1_g_0
J0 = dot(Ad1_0, frame0.jacobian)
J1 = frame1.jacobian
dJ0 = dot(Ad1_0, frame0.djacobian) + \
dot(dAdjoint(Ad1_0, T0_1_0), frame0.jacobian)
dJ1 = frame1.djacobian
self.Jc[3*i:(3*(i+1)), :] = (J1[3:6] - J0[3:6])
self.dJc[3*i:(3*(i+1)), :] = (dJ1[3:6] - dJ0[3:6])
i += 1
def _update_Jc(self):
""" Extract the Jacobian and dJacobian matrix of contact points.
"""
self.Jc[:] = 0.
self.dJc[:] = 0.
i = 0
for c in self.constraints:
if c.is_enabled() and c.is_active() and self.is_enabled[c]:
frame0, frame1 = c._frames[0], c._frames[1]
if isinstance(c, PointContact):
self.dJc[3*i:(3*(i+1)), :] = frame1.djacobian[3:6] - \
frame0.djacobian[3:6]
if frame1.body in self.bodies:
self.Jc[3 * i:(3 * (i + 1)), :] += frame1.jacobian[3:6]
if frame0.body in self.bodies:
self.Jc[3 * i:(3 * (i + 1)), :] -= frame0.jacobian[3:6]
elif isinstance(c, BallAndSocketConstraint):
H1_0 = dot(inv(frame1.pose), frame0.pose)
Ad1_0 = adjoint(H1_0)
Ad0_1 = iadjoint(H1_0)
T0_g_0 = frame0.twist
T1_g_1 = frame1.twist
T1_g_0 = dot(Ad0_1, T1_g_1)
T0_1_0 = T0_g_0 - T1_g_0
J0 = dot(Ad1_0, frame0.jacobian)
J1 = frame1.jacobian
dJ0 = dot(Ad1_0, frame0.djacobian) + \
dot(dAdjoint(Ad1_0, T0_1_0), frame0.jacobian)
dJ1 = frame1.djacobian
self.Jc[3 * i:(3 * (i + 1)), :] = (J1[3:6] - J0[3:6])
self.dJc[3 * i:(3 * (i + 1)), :] = (dJ1[3:6] - dJ0[3:6])
i += 1
def update(self, dt):
r"""
This method calls the collision solver and updates the constraint
status and the contact frames poses accordingly.
The two frames have the same orientation, with the contact normal along
the `z`-axis
"""
(sdist, H_gc0, H_gc1) = self._collision_solver(self._shapes)
H_b0g = Hg.inv(self._shapes[0].frame.body.pose)
H_b1g = Hg.inv(self._shapes[1].frame.body.pose)
self._frames[0].bpose = dot(H_b0g, H_gc0)
self._frames[1].bpose = dot(H_b1g, H_gc1)
H_c0c1 = dot(Hg.inv(H_gc0), H_gc1)
dsdist = (dot(Hg.adjoint(H_c0c1)[5, :], self._frames[1].twist) -
self._frames[0].twist[5])
self._is_active = (sdist + dsdist * dt < self._proximity)
self._sdist = sdist
self._force[:] = 0.
def update(self, dt):
r"""
This method calls the collision solver and updates the constraint
status and the contact frames poses accordingly.
The two frames have the same orientation, with the contact normal along
the `z`-axis
"""
(sdist, H_gc0, H_gc1) = self._collision_solver(self._shapes)
H_b0g = Hg.inv(self._shapes[0].frame.body.pose)
H_b1g = Hg.inv(self._shapes[1].frame.body.pose)
self._frames[0].bpose = dot(H_b0g, H_gc0)
self._frames[1].bpose = dot(H_b1g, H_gc1)
H_c0c1 = dot(Hg.inv(H_gc0), H_gc1)
dsdist = (dot(Hg.adjoint(H_c0c1)[5,:], self._frames[1].twist)
-self._frames[0].twist[5])
self._is_active = (sdist + dsdist*dt < self._proximity)
self._sdist = sdist
self._force[:] = 0.
def transport(M, H):
""" Transport (express) the mass matrix into another frame.
:param M: the mass matrix expressed in the original frame (say, `a`)
:type M: (6,6)-array
:param H: homogeneous matrix from the new frame (say `b`) to the
original one: `H_{ab}`
:type H: (4,4)-array
:return: the mass matrix expressed in the new frame (say, `b`)
:rtype: (6,6)-array
**Example:**
>>> M_a = diag((3., 2., 4., 1., 1., 1.))
>>> H_ab = Hg.transl(1., 3., 0.)
>>> M_b = transport(M_a, H_ab)
>>> allclose(M_b, [[ 12., -3., 0., 0., 0., -3.],
... [ -3., 3., 0., 0., 0., 1.],
... [ 0., 0., 14., 3., -1., 0.],
... [ 0., 0., 3., 1., 0., 0.],
... [ 0., 0., -1., 0., 1., 0.],
... [ -3., 1., 0., 0., 0., 1.]])
True
>>> ismassmatrix(M_b)
True
>>> from math import pi
>>> H_ab = Hg.rotx(pi/4)
>>> M_b = transport(M_a, H_ab)
>>> allclose(M_b, [[ 3., 0., 0., 0., 0., 0.],
... [ 0., 3., 1., 0., 0., 0.],
... [ 0., 1., 3., 0., 0., 0.],
... [ 0., 0., 0., 1., 0., 0.],
... [ 0., 0., 0., 0., 1., 0.],
... [ 0., 0., 0., 0., 0., 1.]])
True
>>> ismassmatrix(M_b)
True
"""
assert ismassmatrix(M)
assert Hg.ishomogeneousmatrix(H)
Ad = Hg.adjoint(H)
return dot(Ad.T, dot(M, Ad))
def transport(M, H):
""" Transport (express) the mass matrix into another frame.
:param M: the mass matrix expressed in the original frame (say, `a`)
:type M: (6,6)-array
:param H: homogeneous matrix from the new frame (say `b`) to the
original one: `H_{ab}`
:type H: (4,4)-array
:return: the mass matrix expressed in the new frame (say, `b`)
:rtype: (6,6)-array
**Example:**
>>> M_a = diag((3., 2., 4., 1., 1., 1.))
>>> H_ab = Hg.transl(1., 3., 0.)
>>> M_b = transport(M_a, H_ab)
>>> allclose(M_b, [[ 12., -3., 0., 0., 0., -3.],
... [ -3., 3., 0., 0., 0., 1.],
... [ 0., 0., 14., 3., -1., 0.],
... [ 0., 0., 3., 1., 0., 0.],
... [ 0., 0., -1., 0., 1., 0.],
... [ -3., 1., 0., 0., 0., 1.]])
True
>>> ismassmatrix(M_b)
True
>>> from math import pi
>>> H_ab = Hg.rotx(pi/4)
>>> M_b = transport(M_a, H_ab)
>>> allclose(M_b, [[ 3., 0., 0., 0., 0., 0.],
... [ 0., 3., 1., 0., 0., 0.],
... [ 0., 1., 3., 0., 0., 0.],
... [ 0., 0., 0., 1., 0., 0.],
... [ 0., 0., 0., 0., 1., 0.],
... [ 0., 0., 0., 0., 0., 1.]])
True
>>> ismassmatrix(M_b)
True
"""
assert ismassmatrix(M)
assert Hg.ishomogeneousmatrix(H)
Ad = Hg.adjoint(H)
return dot(Ad.T, dot(M, Ad))
def _update_matrices(self, rstate, dt):
"""
"""
H_0_f = self._frame.pose
HR_0_f = H_0_f.copy()
HR_0_f[0:3, 3] = 0
AdR_0_f = adjoint(HR_0_f)
T_f_0_f = self._frame.twist #WARNING: twist relative to its frame
T_f_0_0 = dot(AdR_0_f, T_f_0_f)
T_f_0_f_only_rot = T_f_0_f.copy()
T_f_0_f_only_rot[3:6] = 0.
dAdR_0_f = dAdjoint(AdR_0_f, T_f_0_f_only_rot)
J = dot(AdR_0_f, self._frame.jacobian)
dJ = dot(AdR_0_f, self._frame.djacobian) + \
dot(dAdR_0_f, self._frame.jacobian)
gpos = self._frame.pose
gvel = T_f_0_0
cmd = self._ctrl.update(gpos, gvel, rstate, dt)
self._J[:] = J[self._cdof, :]
self._dJ[:] = dJ[self._cdof, :]
self._dVdes[:] = cmd[self._cdof]
def iadjoint(self):
return Hg.adjoint(self.ipose)
def adjoint(self):
return Hg.adjoint(self.pose)
from arboris.homogeneousmatrix import transl
H_bc = transl(1, 1, 1)
else:
H_bc = eye(4)
half_extents = (.5, .5, .5)
mass = 1.
body = Body(name='box_body',
mass=massmatrix.transport(massmatrix.box(half_extents, mass),
H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0., 0., 0., 0., 0., 0.])
else:
twist_c = array([1, 1, 1, 0, 0, 0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, half_extents))
weightc = WeightController()
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
from arboris.visu_osg import Drawer
timeline = arange(0, 1, 5e-3)
simulate(w, timeline, [obs, Drawer(w)])
time = timeline[:-1]
obs.plot_error()
def iadjoint(self):
return Hg.adjoint(self.ipose)
def adjoint(self):
return Hg.adjoint(self.pose)
def update_dynamic(self, pose, jac, djac, twist):
r""" Compute the body ``pose, jac, djac, twist`` and 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.svg
:width: 300px
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_{cp} \; \twist[p]_{p/g} + \twist[c]_{c/p} \\
&= \Ad_{cp} \; \twist[p]_{p/g} + \Ad_{cn} \; \twist[n]_{n/r} \\
&= \Ad_{cp} \; \J[p]_{p/g} \; \GVel
+ \Ad_{cn} \; \J[n]_{n/r} \; \GVel_j \\
&= \J[c]_{c/g} \; \GVel
where `\twist[n]_{n/r}` is given 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_{cp} \; \J[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & \Ad_{cn} \; \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_{cp} \; \J[p]_{p/g} \; \GVel
+ \Ad_{cp} \; \dJ[p]_{p/g} \; \GVel
+ \Ad_{cp} \; \J[p]_g \; \dGVel
+ \Ad_{cn} \; \dJ[n]_{n/r} \; \GVel_j
+ \Ad_{cn} \; \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_{cp} \; \J[p]_{p/g} \; \GVel
+ \Ad_{cp} \; \dJ[p]_{p/g} \; \GVel
+ \Ad_{cn} \; \dJ[n]_{n/r} \; \GVel_j \\
\dJ[c]_{c/g}
&= \dAd_{cp} \; \J[p]_{p/g} + \Ad_{cp} \; \dJ[p]_{p/g} +
\begin{bmatrix}
0 & \cdots & 0 & (\Ad_{cn} \; \dJ[n]_{n/r}) & 0 & \cdots & 0
\end{bmatrix}
with
.. math::
\dAd_{cp} &= \Ad_{cn} \; \dAd_{nr} \; \Ad_{rp}
and where `\dAd_{nr}` 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[5, 5] <= 1e-10: #TODO: avoid hardcoded value
rx = zeros((3, 3))
else:
rx = self.mass[0:3, 3:6] / self.mass[5, 5]
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)
def jacobian(self):
H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
return (dot(Hg.adjoint(H_01)[2:6, :], self._frames[1].jacobian) -
self._frames[0].jacobian[2:6, :])
print("H_o_b * H_b_o\n", np.dot(H_o_b, H_b_o))
isHM =Hg.ishomogeneousmatrix(H_o_b) # check validity of homogeneous matrix
print("is homogeneous matrix?", isHM)
p_b = np.array([.4,.5,.6]) # point p expressed in frame {b}
p_o = Hg.pdot(H_o_b, p_b) # point p expressed in frame {o}
v_b = np.array([.4,.5,.6]) # idem for vector, here affine part is not
v_o = Hg.vdot(H_o_b, v_b) # taken into account
Ad_o_b = Hg.adjoint(H_o_b) # to obtain the adjoint related to displacement
Ad_b_o = Hg.iadjoint(H_o_b) # to obtain the adjoint of the inverse
##### About adjoint matrix #####################################################
import arboris.adjointmatrix as Adm
isAd = Adm.isadjointmatrix(Ad_b_o) # check validity of adjoint matrix
print("is adjoint matrix?", isAd)
Ad_b_o = Adm.inv(Ad_o_b) # get inverse of adjoint matrix
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)
def _human36(world, height=1.741, mass=73, name=''):
"""
TODO: HuMAnS' doc about inertia is erroneous (the real math is in the IOMatrix proc in DynamicData.maple)
"""
assert isinstance(world, World)
w = world
L = anatomical_lengths(height)
bodies = {}
humansbodyid_to_humansbodyname_map = {}
for b in _humans_bodies(height, mass):
#mass matrix at com
mass_g = b['Mass'] * diag(
hstack((b['GyrationRadius']**2, (1,1,1))))
H_fg = eye(4)
H_fg[0:3,3] = b['CenterOfMass']
H_gf = Hg.inv(H_fg)
#mass matrix at body's frame origin:
mass_o = dot(adjoint(H_gf).T, dot(mass_g, adjoint(H_gf)))
bodies[b['HumansName']] = Body(
name=b['HumansName'],
mass=mass_o)
humansbodyid_to_humansbodyname_map[b['HumansId']] = b['HumansName']
rf = SubFrame(w.ground,
Hg.transl(0, L['yfootL']+L['ytibiaL']+L['yfemurL'], 0))
w.add_link(rf, FreeJoint(), bodies['LPT'])
rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, L['zhip']/2.))
j = RzRyRxJoint()
w.add_link(rf, j, bodies['ThighR'])
rf = SubFrame(bodies['ThighR'], Hg.transl(0, -L['yfemurR'], 0))
w.add_link(rf, RzJoint(), bodies['ShankR'])
rf = SubFrame(bodies['ShankR'], Hg.transl(0, -L['ytibiaR'], 0))
w.add_link(rf, RzRxJoint(), bodies['FootR'])
rf = SubFrame(bodies['LPT'], Hg.transl(0, 0, -L['zhip']/2.))
w.add_link(rf, RzRyRxJoint(), bodies['ThighL'])
rf = SubFrame(bodies['ThighL'], Hg.transl(0, -L['yfemurL'], 0))
w.add_link(rf, RzJoint(), bodies['ShankL'])
rf = SubFrame(bodies['ShankL'], Hg.transl(0, -L['ytibiaL'], 0))
w.add_link(rf, RzRxJoint(), bodies['FootL'])
rf = SubFrame(bodies['LPT'], Hg.transl(-L['xvT10'], L['yvT10'], 0))
w.add_link(rf, RzRyRxJoint(), bodies['UPT'])
rf = SubFrame(bodies['UPT'],
Hg.transl(L['xsternoclavR'], L['ysternoclavR'], L['zsternoclavR']))
j = RyRxJoint()
w.add_link(rf, j, bodies['ScapulaR'])
rf = SubFrame(bodies['ScapulaR'],
Hg.transl(-L['xshoulderR'], L['yshoulderR'], L['zshoulderR']))
w.add_link(rf, RzRyRxJoint(), bodies['ArmR'])
rf = SubFrame(bodies['ArmR'], Hg.transl(0, -L['yhumerusR'], 0))
w.add_link(rf, RzRyJoint(), bodies['ForearmR'])
rf = SubFrame(bodies['ForearmR'], Hg.transl(0, -L['yforearmR'], 0))
w.add_link(rf, RzRxJoint(), bodies['HandR'])
rf = SubFrame(bodies['UPT'], Hg.transl(
L['xsternoclavL'], L['ysternoclavL'], -L['zsternoclavL']))
w.add_link(rf, RyRxJoint(), bodies['ScapulaL'])
rf = SubFrame(bodies['ScapulaL'],
Hg.transl(-L['xshoulderL'], L['yshoulderL'], -L['zshoulderL']))
w.add_link(rf, RzRyRxJoint(), bodies['ArmL'])
rf = SubFrame(bodies['ArmL'], Hg.transl(0, -L['yhumerusL'], 0))
w.add_link(rf, RzRyJoint(), bodies['ForearmL'])
rf = SubFrame(bodies['ForearmL'], Hg.transl(0, -L['yforearmL'], 0))
w.add_link(rf, RzRxJoint(), bodies['HandL'])
rf = SubFrame(bodies['UPT'], Hg.transl(L['xvT10'], L['yvC7'], 0))
w.add_link(rf, RzRyRxJoint(), bodies['Head'])
# add tags
tags = {}
for t in _humans_tags(height):
bodyname = humansbodyid_to_humansbodyname_map[t['HumansBodyId']]
tag = SubFrame(
bodies[bodyname],
Hg.transl(t['Position'][0], t['Position'][1], t['Position'][2]),
t['HumansName'])
tags[t['HumansName']] = tag
w.register(tag)
# Add point shapes to the feet
for k in ('Right foot toe tip', 'Right foot heel',
'Right foot phalange 5', 'Right foot Phalange 1',
'Left foot toe tip','Left foot heel',
'Left foot phalange 5','Left foot phalange 1'):
shape = Point(tags[k], name=k)
w.register(shape)
w.init()
return (bodies, tags)
from arboris.homogeneousmatrix import transl
H_bc = transl(1,1,1)
else:
H_bc = eye(4)
lengths = (1.,1.,1.)
mass = 1.
body = Body(
name='box_body',
mass=massmatrix.transport(massmatrix.box(lengths, mass), H_bc))
subframe = SubFrame(body, H_bc, name="box_com")
if True:
twist_c = array([0.,0.,0.,0.,0.,0.])
else:
twist_c = array([1,1,1,0,0,0.])
twist_b = dot(homogeneousmatrix.adjoint(H_bc), twist_c)
freejoint = FreeJoint(gpos=homogeneousmatrix.inv(H_bc), gvel=twist_b)
w.add_link(w.ground, freejoint, body)
w.register(Box(subframe, lengths))
weightc = WeightController(w)
w.register(weightc)
obs = TrajLog(w.getframes()['box_com'], w)
w.observers.append(obs)
from arboris.visu_osg import Drawer
w.observers.append(Drawer(w))
timeline = arange(0,1,5e-3)
simulate(w, timeline)
def jacobian(self):
H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
return (dot(Hg.adjoint(H_01)[2:6,:], self._frames[1].jacobian)
-self._frames[0].jacobian[2:6,:])
