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)
Exemple #2
0
 def djacobian(self):
     # we assume self._bpose is constant
     return dot(Hg.iadjoint(self._bpose), self._body._djacobian)
Exemple #3
0
 def twist(self):
     return dot(Hg.iadjoint(self._bpose), self._body._twist)
Exemple #4
0
 def jacobian(self):
     return dot(Hg.iadjoint(self._bpose), self._body._jacobian)
Exemple #5
0
    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)