Ejemplo n.º 1
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)-shaped array
    :param H: homogeneous matrix from the new frame (say `b`) to the
              original one: `H_{ab}`
    :type H: (4,4)-shaped array
    :rtype: (6,6)-shaped 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)
    >>> M_b
    array([[ 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.]])
    
    """
    assert ismassmatrix(M)
    assert Hg.ishomogeneousmatrix(H)
    Ad = Hg.adjoint(H)
    return dot(Ad.T, dot(M, Ad))
Ejemplo n.º 2
0
    def __init__(self, body, bpose=None, name=None):
        """Create a frame rigidly fixed to a body. 
        
        >>> b = Body()
        >>> f = SubFrame(b, Hg.rotz(3.14/3.),'Brand New Frame')
        
        The ``body`` argument must be a member of the ``Body`` class:
        >>> f = SubFrame(None, Hg.rotz(3.14/3.))
        Traceback (most recent call last):
            ...
        ValueError: The ``body`` argument must be an instance of the ``Boby`` class

        The ``bpose`` argument must be an homogeneous matrix:
        >>> b = Body()
        >>> from numpy import ones
        >>> f = SubFrame(b, ones((4,4)))
        Traceback (most recent call last):
            ...
        AssertionError

        """
        if bpose is None:
            bpose = eye(4)
        
        NamedObject.__init__(self, name)
        assert Hg.ishomogeneousmatrix(bpose)
        self._bpose = bpose
        if not(isinstance(body, Body)):
            raise ValueError("The ``body`` argument must be an instance of the ``Boby`` class")
        else:
            self._body = body
Ejemplo n.º 3
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)
Ejemplo n.º 4
0
 def bpose(self, bpose):
     assert Hg.ishomogeneousmatrix(bpose)
     self._bpose[:] = bpose