skrobot.coordinates.CascadedCoords

class skrobot.coordinates.CascadedCoords(parent=None, *args, **kwargs)[source]

Methods

T()[source]

Return 4x4 homogeneous transformation matrix.

Returns:

matrix – homogeneous transformation matrix shape of (4, 4)

Return type:

numpy.ndarray

Examples

>>> from numpy import pi
>>> from skrobot.coordinates import make_coords
>>> c = make_coords()
>>> c.T()
array([[1., 0., 0., 0.],
       [0., 1., 0., 0.],
       [0., 0., 1., 0.],
       [0., 0., 0., 1.]])
>>> c.translate([0.1, 0.2, 0.3])
>>> c.rotate(pi / 2.0, 'y')
array([[ 2.22044605e-16,  0.00000000e+00,  1.00000000e+00,
         1.00000000e-01],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00,
         2.00000000e-01],
       [-1.00000000e+00,  0.00000000e+00,  2.22044605e-16,
         3.00000000e-01],
       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,
         1.00000000e+00]])
assoc(child, relative_coords='world', force=False, **kwargs)[source]

Associate child coords to this coordinate system.

If relative_coords is None or ‘world’, the translation and rotation of childcoords in the world coordinate system do not change. If relative_coords is specified, childcoords is assoced at translation and rotation of relative_coords. By default, if child is already assoced to some other coords, raise an exception. But if force is True, you can overwrite the existing assoc relation.

Parameters:
  • child (CascadedCoords) – child coordinates.

  • relative_coords (None or Coordinates or str) – child coordinates’ relative coordinates.

  • force (bool) – predicate for overwriting the existing assoc-relation

Returns:

child – assoced child.

Return type:

CascadedCoords

Examples

>>> from skrobot.coordinates import CascadedCoords
>>> parent_coords = CascadedCoords(pos=[1, 0, 0])
>>> child_coords = CascadedCoords(pos=[1, 1, 0])
>>> parent_coords.assoc(child_coords)
#<CascadedCoords 0x7f1d30e29510 1.000 1.000 0.000 / 0.0 -0.0 0.0>
>>> child_coords.worldpos()
array([1., 1., 0.])
>>> child_coords.translation
array([0., 1., 0.])

None and ‘world’ have the same meaning.

>>> parent_coords = CascadedCoords(pos=[1, 0, 0])
>>> child_coords = CascadedCoords(pos=[1, 1, 0])
>>> parent_coords.assoc(child_coords, relative_coords='world')
#<CascadedCoords 0x7f1d30e29510 1.000 1.000 0.000 / 0.0 -0.0 0.0>
>>> child_coords.worldpos()
array([1., 1., 0.])

If relative_coords is ‘local’, child is associated at world translation and world rotation of child from this coordinate system.

>>> parent_coords = CascadedCoords(pos=[1, 0, 0])
>>> child_coords = CascadedCoords(pos=[1, 1, 0])
>>> parent_coords.assoc(child_coords, relative_coords='local')
>>> child_coords.worldpos()
array([2., 1., 0.])
>>> child_coords.translation
array([1., 1., 0.])
axis(ax)[source]
changed()[source]

Return False

This is used for CascadedCoords compatibility

Returns:

False – always return False

Return type:

bool

coords()[source]

Return a deep copy of the Coordinates.

copy()[source]

Return a deep copy of the Coordinates.

copy_coords()[source]

Return a deep copy of the Coordinates.

copy_worldcoords()[source]

Return a deep copy of the Coordinates.

difference_position(coords, translation_axis=True)[source]

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 – difference position of self coordinates and coords considering translation_axis.

Return type:

numpy.ndarray

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])
difference_rotation(coords, rotation_axis=True)[source]

Return differences in rotation of given coords.

Parameters:
  • coords (skrobot.coordinates.Coordinates) – given coordinates

  • rotation_axis (str or bool or None (optional)) – we can take ‘x’, ‘y’, ‘z’, ‘xx’, ‘yy’, ‘zz’, ‘xm’, ‘ym’, ‘zm’, ‘xy’, ‘yx’, ‘yz’, ‘zy’, ‘zx’, ‘xz’, True or False(None).

Returns:

dif_rot – difference rotation of self coordinates and coords considering rotation_axis.

Return type:

numpy.ndarray

Examples

>>> from numpy import pi
>>> from skrobot.coordinates import Coordinates
>>> from skrobot.coordinates.math import rpy_matrix
>>> coord1 = Coordinates()
>>> coord2 = Coordinates(rot=rpy_matrix(pi / 2.0, pi / 3.0, pi / 5.0))
>>> coord1.difference_rotation(coord2)
array([-0.32855112,  1.17434985,  1.05738936])
>>> coord1.difference_rotation(coord2, rotation_axis=False)
array([0, 0, 0])
>>> coord1.difference_rotation(coord2, rotation_axis='x')
array([0.        , 1.36034952, 0.78539816])
>>> coord1.difference_rotation(coord2, rotation_axis='y')
array([0.35398131, 0.        , 0.97442695])
>>> coord1.difference_rotation(coord2, rotation_axis='z')
array([-0.88435715,  0.74192175,  0.        ])

Using mirror option [‘xm’, ‘ym’, ‘zm’], you can allow differences of mirror direction.

>>> coord1 = Coordinates()
>>> coord2 = Coordinates().rotate(pi, 'x')
>>> coord1.difference_rotation(coord2, 'xm')
array([-2.99951957e-32,  0.00000000e+00,  0.00000000e+00])
>>> coord1 = Coordinates()
>>> coord2 = Coordinates().rotate(pi / 2.0, 'x')
>>> coord1.difference_rotation(coord2, 'xm')
array([-1.57079633,  0.        ,  0.        ])
disable_hook()[source]
dissoc(child)[source]
get_transform()[source]

Return Transform object

Returns:

transform – corrensponding Transform to this coordinates

Return type:

skrobot.coordinates.base.Transform

inverse_rotate_vector(v)[source]
inverse_transform_vector(v)[source]

Transform vector in world coordinates to local coordinates

Parameters:

vec (numpy.ndarray) – 3d vector. We can take batch of vector like (batch_size, 3)

Returns:

transformed_point – transformed point

Return type:

numpy.ndarray

inverse_transformation(dest=None)[source]

Return a invese transformation of this coordinate system.

Create a new coordinate with inverse transformation of this coordinate system.

\[\begin{split}\left( \begin{array}{ccc} R^{-1} & - R^{-1} p \\ 0 & 1 \end{array} \right)\end{split}\]
Parameters:

dest (None or skrobot.coordinates.Coordinates) – If dest is given, the result of transformation is in-placed to dest.

Returns:

dest – result of inverse transformation.

Return type:

skrobot.coordinates.Coordinates

move_coords(target_coords, local_coords)[source]

Transform this coordinate so that local_coords to target_coords.

Parameters:
Returns:

self.worldcoords() – world coordinates.

Return type:

skrobot.coordinates.Coordinates

newcoords(c, pos=None, check_validity=True)[source]

Update this coordinates.

This function records that this CascadedCoords has changed and recursively records the change to descendants of this CascadedCoords.

Parameters:
  • c (skrobot.coordinates.Coordinates or numpy.ndarray) – If pos is None, c means new Coordinates. If pos is given, c means rotation matrix.

  • pos (numpy.ndarray or None) – new translation.

  • check_validity (bool) – If this value is True, check whether an input rotation and an input translation are valid.

orient_with_matrix(rotation_matrix, wrt='world')[source]

Force update this coordinate system’s rotation.

Parameters:
parent_orientation(v, wrt)[source]
parentcoords()[source]
rotate(theta, axis, wrt='local')[source]

Rotate this coordinate.

Rotate this coordinate relative to axis by theta radians with respect to wrt.

Parameters:
Return type:

self

rotate_vector(v)[source]

Rotate 3-dimensional vector using rotation of this coordinate

Parameters:

v (numpy.ndarray) – vector shape of (3,)

Returns:

np.matmul(self.rotation, v) – rotated vector

Return type:

numpy.ndarray

Examples

>>> from skrobot.coordinates import Coordinates
>>> from numpy import pi
>>> c = Coordinates().rotate(pi, 'z')
>>> c.rotate_vector([1, 2, 3])
array([-1., -2.,  3.])
rotate_with_matrix(matrix, wrt)[source]

Rotate this coordinate by given rotation matrix.

This is a subroutine of self.rotate function.

Parameters:
Returns:

self

Return type:

skrobot.coordinates.Coordinates

rpy_angle()[source]

Return a pair of rpy angles of this coordinates.

Returns:

rpy_angle(self.rotation) – a pair of rpy angles. See also skrobot.coordinates.math.rpy_angle

Return type:

tuple(numpy.ndarray, numpy.ndarray)

Examples

>>> import numpy as np
>>> from skrobot.coordinates import Coordinates
>>> c = Coordinates().rotate(np.pi / 2.0, 'x').rotate(np.pi / 3.0, 'z')
>>> r.rpy_angle()
(array([ 3.84592537e-16, -1.04719755e+00,  1.57079633e+00]),
array([ 3.14159265, -2.0943951 , -1.57079633]))
transform(c, wrt='local', out=None)[source]

Transform this coordinates

Parameters:
  • c (skrobot.coordinates.Coordinates) – coordinates

  • wrt (str or skrobot.coordinates.Coordinates) – transform this coordinates with respect to wrt. If wrt is ‘local’ or self, multiply c from the right. If wrt is ‘parent’ or self.parent, transform c with respect to parentcoords. (multiply c from the left.) If wrt is Coordinates, transform c with respect to c.

  • out (None or skrobot.coordinates.Coordinates) – If the out is specified, set new coordinates to out. Note that if the out is given, these coordinates don’t change.

Returns:

self – return self

Return type:

skrobot.coordinates.CascadedCoords

transform_vector(v)[source]

“Return vector represented at world frame.

Vector v given in the local coords is converted to world representation.

Parameters:

v (numpy.ndarray) – 3d vector. We can take batch of vector like (batch_size, 3)

Returns:

transformed_point – transformed point

Return type:

numpy.ndarray

transformation(c2, wrt='local')[source]
translate(vec, wrt='local')[source]

Translate this coordinates.

Note that this function changes this coordinates self. So if you don’t want to change this class, use copy_worldcoords()

Parameters:
  • vec (list or numpy.ndarray) – shape of (3,) translation vector. unit is [m] order.

  • wrt (str or Coordinates (optional)) – translate with respect to wrt.

Examples

>>> import numpy as np
>>> from skrobot.coordinates import Coordinates
>>> c = Coordinates()
>>> c.translation
array([0., 0., 0.], dtype=float32)
>>> c.translate([0.1, 0.2, 0.3])
>>> c.translation
array([0.1, 0.2, 0.3], dtype=float32)
>>> c = Coordinates()
>>> c.copy_worldcoords().translate([0.1, 0.2, 0.3])
>>> c.translation
array([0., 0., 0.], dtype=float32)
>>> c = Coordinates().rotate(np.pi / 2.0, 'y')
>>> c.translate([0.1, 0.2, 0.3])
>>> c.translation
array([ 0.3,  0.2, -0.1])
>>> c = Coordinates().rotate(np.pi / 2.0, 'y')
>>> c.translate([0.1, 0.2, 0.3], 'world')
>>> c.translation
array([0.1, 0.2, 0.3])
update(force=False)[source]
worldcoords()[source]

Calculate rotation and position in the world.

worldpos()[source]

Return translation of this coordinate

See also skrobot.coordinates.Coordinates.translation

Returns:

self.translation – translation of this coordinate

Return type:

numpy.ndarray

worldrot()[source]

Return rotation of this coordinate

See also skrobot.coordinates.Coordinates.rotation

Returns:

self.rotation – rotation matrix of this coordinate

Return type:

numpy.ndarray

__eq__(value, /)

Return self==value.

__ne__(value, /)

Return self!=value.

__lt__(value, /)

Return self<value.

__le__(value, /)

Return self<=value.

__gt__(value, /)

Return self>value.

__ge__(value, /)

Return self>=value.

__mul__(other_c)[source]

Return Transformed Coordinates.

Note that this function creates new Coordinates and does not change translation and rotation, unlike transform function.

Parameters:

other_c (skrobot.coordinates.Coordinates) – input coordinates.

Returns:

out – transformed coordinates multiplied other_c from the right. T = T_{self} T_{other_c}.

Return type:

skrobot.coordinates.Coordinates

__pow__(exponent)[source]

Return exponential homogeneous matrix.

If exponent equals -1, return inverse transformation of this coords.

Parameters:

exponent (numbers.Number) – exponent value. If exponent equals -1, return inverse transformation of this coords. In current, support only -1 case.

Returns:

out – output.

Return type:

skrobot.coordinates.Coordinates

Attributes

descendants
dimension

Return dimension of this coordinate

Returns:

len(self.translation) – dimension of this coordinate

Return type:

int

dual_quaternion

Property of DualQuaternion

Return DualQuaternion representation of this coordinate.

Returns:

DualQuaternion – DualQuaternion representation of this coordinate

Return type:

skrobot.coordinates.dual_quaternion.DualQuaternion

name

Return this coordinate’s name

Returns:

self._name – name of this coordinate

Return type:

str

parent
quaternion

Property of quaternion

Returns:

q – [w, x, y, z] quaternion

Return type:

numpy.ndarray

Examples

>>> from numpy import pi
>>> from skrobot.coordinates import make_coords
>>> c = make_coords()
>>> c.quaternion
array([1., 0., 0., 0.])
>>> c.rotate(pi / 3, 'y').rotate(pi / 5, 'z')
>>> c.quaternion
array([0.8236391 , 0.1545085 , 0.47552826, 0.26761657])
rotation

Return rotation matrix of this coordinates.

Returns:

self._rotation – 3x3 rotation matrix

Return type:

numpy.ndarray

Examples

>>> import numpy as np
>>> from skrobot.coordinates import Coordinates
>>> c = Coordinates()
>>> c.rotation
array([[1., 0., 0.],
       [0., 1., 0.],
       [0., 0., 1.]])
>>> c.rotate(np.pi / 2.0, 'y')
>>> c.rotation
array([[ 2.22044605e-16,  0.00000000e+00,  1.00000000e+00],
       [ 0.00000000e+00,  1.00000000e+00,  0.00000000e+00],
       [-1.00000000e+00,  0.00000000e+00,  2.22044605e-16]])
translation

Return translation of this coordinates.

Returns:

self._translation – vector shape of (3, ). unit is [m]

Return type:

numpy.ndarray

Examples

>>> from skrobot.coordinates import Coordinates
>>> c = Coordinates()
>>> c.translation
array([0., 0., 0.])
>>> c.translate([0.1, 0.2, 0.3])
>>> c.translation
array([0.1, 0.2, 0.3])
x_axis

Return x axis vector of this coordinates.

Returns:

axis – x axis.

Return type:

numpy.ndarray

y_axis

Return y axis vector of this coordinates.

Returns:

axis – y axis.

Return type:

numpy.ndarray

z_axis

Return z axis vector of this coordinates.

Returns:

axis – z axis.

Return type:

numpy.ndarray