def difference_position(self, coords, translation_axis=True):
"""Return differences in positoin of given coords.
Parameters
----------
coords : skrobot.coordinates.Coordinates
given coordinates
translation_axis : str or bool or None (optional)
we can take 'x', 'y', 'z', 'xy', 'yz', 'zx', 'xx', 'yy', 'zz',
True or False(None).
Returns
-------
dif_pos : numpy.ndarray
difference position of self coordinates and coords
considering translation_axis.
Examples
--------
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates import transform_coords
>>> from numpy import pi
>>> c1 = Coordinates().translate([0.1, 0.2, 0.3]).rotate(
... pi / 3.0, 'x')
>>> c2 = Coordinates().translate([0.3, -0.3, 0.1]).rotate(
... pi / 2.0, 'y')
>>> c1.difference_position(c2)
array([ 0.2 , -0.42320508, 0.3330127 ])
>>> c1 = Coordinates().translate([0.1, 0.2, 0.3]).rotate(0, 'x')
>>> c2 = Coordinates().translate([0.3, -0.3, 0.1]).rotate(
... pi / 3.0, 'x')
>>> c1.difference_position(c2)
array([ 0.2, -0.5, -0.2])
"""
dif_pos = self.inverse_transform_vector(coords.worldpos())
translation_axis = _wrap_axis(translation_axis)
dif_pos[translation_axis == 1] = 0.0
return dif_pos
def axis(self, ax):
ax = _wrap_axis(ax)
return self.rotate_vector(ax)
def orient_coords_to_axis(target_coords, v, axis='z', eps=0.005):
"""Orient axis to the direction
Orient axis in target_coords to the direction specified by v.
Parameters
----------
target_coords : skrobot.coordinates.Coordinates
v : list or numpy.ndarray
position of target [x, y, z]
axis : list or string or numpy.ndarray
see _wrap_axis function
eps : float (optional)
eps
Returns
-------
target_coords : skrobot.coordinates.Coordinates
Examples
--------
>>> import numpy as np
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates.geo import orient_coords_to_axis
>>> c = Coordinates()
>>> oriented_coords = orient_coords_to_axis(c, [1, 0, 0])
>>> oriented_coords.translation
array([0., 0., 0.])
>>> oriented_coords.rpy_angle()
(array([0. , 1.57079633, 0. ]),
array([3.14159265, 1.57079633, 3.14159265]))
>>> c = Coordinates(pos=[0, 1, 0])
>>> oriented_coords = orient_coords_to_axis(c, [0, 1, 0])
>>> oriented_coords.translation
array([0., 1., 0.])
>>> oriented_coords.rpy_angle()
(array([ 0. , -0. , -1.57079633]),
array([ 3.14159265, -3.14159265, 1.57079633]))
>>> c = Coordinates(pos=[0, 1, 0]).rotate(np.pi / 3, 'y')
>>> oriented_coords = orient_coords_to_axis(c, [0, 1, 0])
>>> oriented_coords.translation
array([0., 1., 0.])
>>> oriented_coords.rpy_angle()
(array([-5.15256299e-17, 1.04719755e+00, -1.57079633e+00]),
array([3.14159265, 2.0943951 , 1.57079633]))
"""
v = np.array(v, 'f')
if np.linalg.norm(v) == 0.0:
v = np.array([0, 0, 1], 'f')
nv = normalize_vector(v)
axis = _wrap_axis(axis)
ax = target_coords.rotate_vector(axis)
rot_axis = np.cross(ax, nv)
rot_angle_cos = np.clip(np.dot(nv, ax), -1.0, 1.0)
if np.isclose(rot_angle_cos, 1.0, atol=eps):
return target_coords
elif np.isclose(rot_angle_cos, -1.0, atol=eps):
for rot_axis2 in [np.array([1, 0, 0]), np.array([0, 1, 0])]:
rot_angle_cos2 = np.dot(ax, rot_axis2)
if not np.isclose(abs(rot_angle_cos2), 1.0, atol=eps):
rot_axis = rot_axis2 - rot_angle_cos2 * ax
break
target_coords.rotate(
np.arccos(rot_angle_cos), rot_axis, 'world')
return target_coords