Пример #1
0
 def update(self):
     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)
     #self._frames[0].bpose = dot(self._shapes[0].frame.bpose, H_s0c0)
     #self._frames[1].bpose = dot(self._shapes[1].frame.bpose, H_s1c1)
     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) #TODO: use a set method?
     self._frames[1]._bpose = dot(H_b1g, H_gc1)
     self._is_active = (sdist < self._proximity)
     self._sdist = sdist
Пример #2
0
    def update(self):
        r"""
        Compute the predicted relative position error between the socket and
        ball centers, `p_{01}(t)` and save it in ``self._pos0``.

        .. math::

            H_{01}(t) 
            &= \left(H_{g0}(t)\right)^{-1} \; H_{g1}(t) \\
            H_{01}(t) 
            &=
            \begin{bmatrix}
            R_{01}(t) & p_{01}(t) \\
            0             & 1
            \end{bmatrix}

        """
        H_01 = dot(Hg.inv(self._frames[0].pose), self._frames[1].pose)
        self._pos0 = H_01[0:3,3]
Пример #3
0
    def update_geometric(self,pose):
        """
        - g: ground body
        - p: parent body
        - c: child body
        - r: reference frame of the joint, rigidly fixed to the parent body
        - n: new frame of the joint, rigidly fixed to the child body
        
        so H_nc and H_pr are constant.

        H_gc = H_gp * H_pc
             = H_gp * (H_pr * H_rn * H_nc)
        """
        self._pose = pose
        H_gp = pose
        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)
            j._frame1.body.update_geometric(child_pose)
Пример #4
0
def _box_sphere_collision(H_g0, lengths0, p_g1, radius1):
    """
    :param H_g0: pose of the box `H_{g1}`
    :type H_g0: (4,4) array
    :param lengths0: lengths of the box
    :type lengths0: (3,) array
    :param p_g1: center of the sphere
    :type p_g1: (3,) array 
    :param radius1: radius of the sphere
    :type radius1: float

    .. image:: img/box_sphere_collision.png

    **Tests:**
    
    >>> from numpy import array, eye
    >>> H_g0 = eye(4)
    >>> lengths0 = array([1., 2., 3.])
    >>> r1 = 0.1
    >>> p_g1 = array([0., 3., 1.])
    >>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0, p_g1, r1)
    >>> print(sdist)
    1.9
    >>> print(H_gc0)
    [[ 0.  1.  0.  0.]
     [-0.  0.  1.  1.]
     [ 1. -0.  0.  1.]
     [ 0.  0.  0.  1.]]
    >>> print(H_gc1)
    [[ 0.   1.   0.   0. ]
     [-0.   0.   1.   2.9]
     [ 1.  -0.   0.   1. ]
     [ 0.   0.   0.   1. ]]
    >>> p_g1 = array([0.55, 0., 0.])
    >>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0, p_g1, r1)
    >>> print(sdist)
    -0.05
    >>> print(H_gc0)
    [[-0.   0.   1.   0.5]
     [ 0.  -1.   0.   0. ]
     [ 1.   0.   0.   0. ]
     [ 0.   0.   0.   1. ]]
    >>> print(H_gc1)
    [[-0.    0.    1.    0.45]
     [ 0.   -1.    0.    0.  ]
     [ 1.    0.    0.    0.  ]
     [ 0.    0.    0.    1.  ]]
    >>> p_g1 = array([0.45, 0., 0.])
    >>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0, p_g1, r1)
    >>> print(sdist)
    -0.15
    >>> print(H_gc0)
    [[-0.   0.   1.   0.5]
     [ 0.  -1.   0.   0. ]
     [ 1.   0.   0.   0. ]
     [ 0.   0.   0.   1. ]]
    >>> print(H_gc1)
    [[-0.    0.    1.    0.35]
     [ 0.   -1.    0.    0.  ]
     [ 1.    0.    0.    0.  ]
     [ 0.    0.    0.    1.  ]]

    """
    assert Hg.ishomogeneousmatrix(H_g0)
    p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
    if (-lengths0[0]/2. <= p_01[0] and p_01[0] <= lengths0[0]/2.) and\
       (-lengths0[1]/2. <= p_01[1] and p_01[1] <= lengths0[1]/2.) and\
       (-lengths0[2]/2. <= p_01[2] and p_01[2] <= lengths0[2]/2.):
        # p_01 is inside the box, we need to find the nearest face
        i = argmin(hstack((lengths0 - p_01, lengths0 + p_01)))
        f_0 = p_01.copy()
        normal = zeros(3)
        if i < 3:
            f_0[i] = lengths0[i]/2.
            normal[i] = 1
        else:
            f_0[i-3] = -lengths0[i-3]/2.
            normal[i-3] = -1 #TODO check this line is correct
        f_g = Hg.pdot(H_g0, f_0)
        sdist = -norm(f_g - p_g1)-radius1
    else:
        # find the point x inside the box that is the nearest to 
        # the sphere center:
        f_0 = zeros(3)
        for i in range(3):
            f_0[i] = max(min(lengths0[i]/2., p_01[i]), -lengths0[i]/2.)
        f_g = Hg.pdot(H_g0, f_0)
        vec = p_g1 - f_g
        normal = vec/norm(vec)
        sdist = norm(vec) - radius1
    H_gc0 = _normal_to_frame(normal)
    H_gc1 = H_gc0.copy()
    H_gc0[0:3,3] = f_g
    H_gc1[0:3,3] = p_g1 - radius1*normal
    return (sdist, H_gc0, H_gc1)
Пример #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)
Пример #6
0
 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,:])