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 jacobian(self):
try:
return dot(Hg.iadjoint(self._bpose), self._body.jacobian)
except TypeError:
raise TypeError(
"jacobian is not up to date, run world.update_dynamic() first."
)
def update(self, dt):
self._CoMPosition[:] = 0.
self._CoMJacobian[:] = 0.
self._CoMdJacobian[:] = 0.
for i in arange(len(self.bodies)):
H_0_com_i = dot(self.bodies[i].pose, self.H_body_com[i])
self._CoMPosition += self.mass[i] * H_0_com_i[0:3, 3]
if self.compute_Jacobians is True:
##### For jacobian
H_com_i_com2 = zeros((4, 4)) #com2 is aligned with ground
H_com_i_com2[0:3, 0:3] = H_0_com_i[0:3, 0:3].T
H_com_i_com2[3, 3] = 1.
Ad_com2_body = iadjoint(dot(self.H_body_com[i], H_com_i_com2))
self._CoMJacobian[:] += self.mass[i] * dot(
Ad_com2_body, self.bodies[i].jacobian)[3:6, :]
##### For dJacobian
T_com2_body = self.bodies[i].twist.copy()
T_com2_body[3:6] = 0.
dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body)
dJ_com2 = dot(Ad_com2_body, self.bodies[i].djacobian) + dot(
dAd_com2_body, self.bodies[i].jacobian)
self._CoMdJacobian[:] += self.mass[i] * dJ_com2[3:6, :]
self._CoMPosition /= self.total_mass
self._CoMJacobian /= self.total_mass
self._CoMdJacobian /= self.total_mass
def update(self, dt=None):
""" Compute gravity vector for current state. """
gforce = zeros(self._wndof)
for b in self._bodies:
g = dot(Hg.iadjoint(b.pose), self._gravity_dtwist)
gforce += dot(b.jacobian.T, dot(b.mass, g))
return (gforce, self._impedance)
def djacobian(self):
try:
# we assume self._bpose is constant
return dot(Hg.iadjoint(self._bpose), self._body.djacobian)
except TypeError:
raise TypeError(
"djacobian is not up to date, run world.update_dynamic() first."
)
def body_com_properties(body, compute_J=True):
""" Compute the Center of Mass properties of a body.
"""
H_body_com = principalframe(body.mass)
H_0_com = dot(body.pose, H_body_com)
P_0_com = H_0_com[0:3, 3]
if compute_J:
H_com_com2 = inv(H_0_com)
H_com_com2[0:3, 3] = 0.
Ad_com2_body = iadjoint(dot(H_body_com, H_com_com2))
J_com2 = dot(Ad_com2_body, body.jacobian)
T_com2_body = body.twist.copy()
T_com2_body[3:6] = 0.
dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body)
dJ_com2 = dot(Ad_com2_body, body.djacobian) + \
dot(dAd_com2_body, body.jacobian)
return P_0_com, J_com2, dJ_com2
else:
return P_0_com
def body_com_properties(body, compute_J=True):
""" Compute the Center of Mass properties of a body.
"""
H_body_com = principalframe(body.mass)
H_0_com = dot(body.pose, H_body_com)
P_0_com = H_0_com[0:3, 3]
if compute_J:
H_com_com2 = inv(H_0_com)
H_com_com2[0:3, 3] = 0.
Ad_com2_body = iadjoint(dot(H_body_com, H_com_com2))
J_com2 = dot(Ad_com2_body, body.jacobian)
T_com2_body = body.twist.copy()
T_com2_body[3:6] = 0.
dAd_com2_body = dAdjoint(Ad_com2_body, T_com2_body)
dJ_com2 = dot(Ad_com2_body, body.djacobian) + \
dot(dAd_com2_body, body.jacobian)
return P_0_com, J_com2, dJ_com2
else:
return P_0_com
def transport_mass_matrix(mass,H):
"""Transport (express) the mass matrix into another frame."""
Ad = Hg.iadjoint(H)
return np.dot(
Ad.transpose(),
np.dot(mass, Ad))
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 djacobian(self):
# we assume self._bpose is constant
return dot(Hg.iadjoint(self._bpose), self._body._djacobian)
def jacobian(self):
return dot(Hg.iadjoint(self._bpose), self._body._jacobian)
def twist(self):
try:
return dot(Hg.iadjoint(self._bpose), self._body.twist)
except TypeError:
raise TypeError(
"twist is not up to date, run world.update_dynamic() first.")
def jacobian(self):
try:
return dot(Hg.iadjoint(self._bpose), self._body.jacobian)
except TypeError:
raise TypeError("jacobian is not up to date, run world.update_dynamic() first.")
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)
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 twist(self):
return dot(Hg.iadjoint(self._bpose), self._body._twist)
def twist(self):
try:
return dot(Hg.iadjoint(self._bpose), self._body.twist)
except TypeError:
raise TypeError("twist is not up to date, run world.update_dynamic() first.")
def djacobian(self):
try:
# we assume self._bpose is constant
return dot(Hg.iadjoint(self._bpose), self._body.djacobian)
except TypeError:
raise TypeError("djacobian is not up to date, run world.update_dynamic() first.")
