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)
# 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
##### About adjoint matrix #####################################################
import arboris.adjointmatrix as Adm
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 _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 _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):
""" 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)
