def diff(val1, val2):
"""
"""
if ishomogeneousmatrix(asarray(val1)):
v1 = quatpos(val1)
else:
v1 = asarray(val1)
if ishomogeneousmatrix(asarray(val2)):
v2 = quatpos(val2)
else:
v2 = asarray(val2)
return v1 - v2
def diff(val1, val2):
"""
"""
if ishomogeneousmatrix(asarray(val1)):
v1 = quatpos(val1)
else:
v1 = asarray(val1)
if ishomogeneousmatrix(asarray(val2)):
v2 = quatpos(val2)
else:
v2 = asarray(val2)
return v1 - v2
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 _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
""" Get information on plane/sphere collision.
:param H_g0: pose of the center of the plane relative to the ground
:type H_g0: (4,4)-array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,)-array
:param p_g1: position of center of the sphere relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the plane and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on plane 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/plane_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
""" Get information on plane/sphere collision.
:param H_g0: pose of the center of the plane relative to the ground
:type H_g0: (4,4)-array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,)-array
:param p_g1: position of center of the sphere relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the plane and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on plane 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/plane_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
"""
:param H_g0: pose of the plane `H_{g0}`
:type H_g0: (4,4) array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3,3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3,3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
"""
:param H_g0: pose of the plane `H_{g0}`
:type H_g0: (4,4) array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def __init__(self, body, bpose=None, name=None):
""" Create a frame rigidly fixed to a body.
:param body: parent body which will support this subframe
:type body: class:`~arboris.core.Body`
:param bpose: the homogeneous matrix from the parent body to the subframe
:type bpose: (4,4)-array
:param name: the name of the subframe
:type name: string
**Example:**
>>> 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
"""
NamedObject.__init__(self, name)
Frame.__init__(self)
if bpose is None:
bpose = eye(4)
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 __init__(self, body, bpose=None, name=None):
""" Create a frame rigidly fixed to a body.
:param body: parent body which will support this subframe
:type body: class:`~arboris.core.Body`
:param bpose: the homogeneous matrix from the parent body to the subframe
:type bpose: (4,4)-array
:param name: the name of the subframe
:type name: string
**Example:**
>>> 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
"""
NamedObject.__init__(self, name)
Frame.__init__(self)
if bpose is None:
bpose = eye(4)
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 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)-array
:param H: homogeneous matrix from the new frame (say `b`) to the
original one: `H_{ab}`
:type H: (4,4)-array
:return: the mass matrix expressed in the new frame (say, `b`)
:rtype: (6,6)-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)
>>> allclose(M_b, [[ 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.]])
True
>>> ismassmatrix(M_b)
True
>>> from math import pi
>>> H_ab = Hg.rotx(pi/4)
>>> M_b = transport(M_a, H_ab)
>>> allclose(M_b, [[ 3., 0., 0., 0., 0., 0.],
... [ 0., 3., 1., 0., 0., 0.],
... [ 0., 1., 3., 0., 0., 0.],
... [ 0., 0., 0., 1., 0., 0.],
... [ 0., 0., 0., 0., 1., 0.],
... [ 0., 0., 0., 0., 0., 1.]])
True
>>> ismassmatrix(M_b)
True
"""
assert ismassmatrix(M)
assert Hg.ishomogeneousmatrix(H)
Ad = Hg.adjoint(H)
return dot(Ad.T, dot(M, Ad))
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)-array
:param H: homogeneous matrix from the new frame (say `b`) to the
original one: `H_{ab}`
:type H: (4,4)-array
:return: the mass matrix expressed in the new frame (say, `b`)
:rtype: (6,6)-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)
>>> allclose(M_b, [[ 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.]])
True
>>> ismassmatrix(M_b)
True
>>> from math import pi
>>> H_ab = Hg.rotx(pi/4)
>>> M_b = transport(M_a, H_ab)
>>> allclose(M_b, [[ 3., 0., 0., 0., 0., 0.],
... [ 0., 3., 1., 0., 0., 0.],
... [ 0., 1., 3., 0., 0., 0.],
... [ 0., 0., 0., 1., 0., 0.],
... [ 0., 0., 0., 0., 1., 0.],
... [ 0., 0., 0., 0., 0., 1.]])
True
>>> ismassmatrix(M_b)
True
"""
assert ismassmatrix(M)
assert Hg.ishomogeneousmatrix(H)
Ad = Hg.adjoint(H)
return dot(Ad.T, dot(M, Ad))
# also Hg.rotzy R = Rz * Ry
# Hg.rotzx R = Rz * Rx
# Hg.rotyx R = Ry * Rx
# Hg.rotz R = Rz
# Hg.roty R = Ry
# Hg.rotx R = Rx
H_o_b = np.dot(H_o_a, H_a_b) # H from {o} to {b} is H{o}->{a} * H{a}->{b}
H_b_o = Hg.inv(H_o_b) # obtain inverse, H from {b} to {o}
print("H_o_b * H_b_o\n", np.dot(H_o_b, H_b_o))
isHM =Hg.ishomogeneousmatrix(H_o_b) # check validity of homogeneous matrix
print("is homogeneous matrix?", isHM)
p_b = np.array([.4,.5,.6]) # point p expressed in frame {b}
p_o = Hg.pdot(H_o_b, p_b) # point p expressed in frame {o}
v_b = np.array([.4,.5,.6]) # idem for vector, here affine part is not
v_o = Hg.vdot(H_o_b, v_b) # taken into account
Ad_o_b = Hg.adjoint(H_o_b) # to obtain the adjoint related to displacement
Ad_b_o = Hg.iadjoint(H_o_b) # to obtain the adjoint of the inverse
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
""" Get information on box/sphere collision.
:param H_g0: pose of the center of the box relative to the ground
:type H_g0: (4,4)-array
:param half_extents0: half lengths of the box
:type half_extents0: (3,)-array
:param p_g1: position of the center of the sphere relative to the ground
:type p_g1: (3,) array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the box and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on box 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/box_sphere_collision.svg
:width: 300px
**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/2, 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/2, 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/2, 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 (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
near_face = zeros(6)
near_face[0:3] = half_extents0 - p_01
near_face[3:6] = half_extents0 + p_01
i = argmin(near_face)
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i-3] = -half_extents0[i-3]
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 arange(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec/norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(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 _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
"""
:param H_g0: pose of the box `H_{g0}`
:type H_g0: (4,4) array
:param half_extents0: half lengths of the box
:type half_extents0: (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/2, 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/2, 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/2, 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 (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
i = argmin(hstack((half_extents0 - p_01, half_extents0 + p_01)))
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i-3] = -half_extents0[i-3]
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(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec/norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(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
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
""" Get information on box/sphere collision.
:param H_g0: pose of the center of the box relative to the ground
:type H_g0: (4,4)-array
:param half_extents0: half lengths of the box
:type half_extents0: (3,)-array
:param p_g1: position of the center of the sphere relative to the ground
:type p_g1: (3,) array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the box and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on box 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/box_sphere_collision.svg
:width: 300px
**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/2, 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/2, 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/2, 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 (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
near_face = zeros(6)
near_face[0:3] = half_extents0 - p_01
near_face[3:6] = half_extents0 + p_01
i = argmin(near_face)
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i - 3] = -half_extents0[i - 3]
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 arange(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec / norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(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
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
"""
:param H_g0: pose of the box `H_{g0}`
:type H_g0: (4,4) array
:param half_extents0: half lengths of the box
:type half_extents0: (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/2, 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/2, 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/2, 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 (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
i = argmin(hstack((half_extents0 - p_01, half_extents0 + p_01)))
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i - 3] = -half_extents0[i - 3]
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(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec / norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(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)
