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
