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))
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
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)
def bpose(self, bpose): assert Hg.ishomogeneousmatrix(bpose) self._bpose[:] = bpose