skrobot.coordinates.Coordinates
- class skrobot.coordinates.Coordinates(pos=None, rot=None, name=None, hook=None, check_validity=True, input_quaternion_order='wxyz')[source]
Coordinates class to manipulate rotation and translation.
- Parameters:
pos (list or numpy.ndarray or None) – shape of (3,) translation vector. or 4x4 homogeneous transformation matrix. If the homogeneous transformation matrix is given, rot will be overwritten. If this value is None, set [0, 0, 0] vector as default.
rot (list or numpy.ndarray or None) – we can take 3x3 rotation matrix or [yaw, pitch, roll] or quaternion [w, x, y, z] order If this value is None, set the identity matrix as default.
name (str or None) – name of this coordinates
check_validity (bool (optional)) – Default True. If this value is True, check whether an input rotation and an input translation are valid.
Methods
- T()[source]
Return 4x4 homogeneous transformation matrix.
- Returns:
matrix – homogeneous transformation matrix shape of (4, 4)
- Return type:
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]])
- align_axis_to_direction(direction, axis='z', wrt='world', eps=0.005)[source]
Align the specified axis of this coordinate to point in the given direction.
Rotates this coordinate system so that the specified axis points in the direction of the given vector.
- Parameters:
direction (list or numpy.ndarray) – Target direction vector [x, y, z]. The axis will be aligned to point in this direction.
axis (str or list or numpy.ndarray, optional) – The axis to align. Can be ‘x’, ‘y’, ‘z’, or a custom axis vector. Default is ‘z’.
wrt (str or skrobot.coordinates.Coordinates, optional) – Reference frame for the direction vector. ‘world’: direction is in world frame (default) ‘local’: direction is in this coordinate’s local frame Coordinates: direction is in the given coordinate’s frame
eps (float, optional) – Tolerance for detecting parallel/anti-parallel cases. Default is 0.005.
- Returns:
self – Returns self for method chaining.
- Return type:
Examples
>>> import numpy as np >>> from skrobot.coordinates import Coordinates >>> c = Coordinates() >>> c.align_axis_to_direction([1, 0, 0]) # Align z-axis to x direction >>> c.z_axis array([1., 0., 0.], dtype=float32)
>>> c = Coordinates() >>> c.align_axis_to_direction([0, 1, 0], axis='x') # Align x-axis to y direction >>> c.x_axis array([0., 1., 0.], dtype=float32)
>>> c = Coordinates().rotate(np.pi / 2, 'z') >>> c.align_axis_to_direction([1, 0, 0], wrt='local') # Direction in local frame
- changed()[source]
Return False
This is used for CascadedCoords compatibility
- Returns:
False – always return False
- Return type:
- difference_position(coords, position_mask=True, **kwargs)[source]
Return differences in position of given coords.
- Parameters:
coords (skrobot.coordinates.Coordinates) – given coordinates
position_mask (str, bool, list, or numpy.ndarray) – Specifies which axes to CONSTRAIN. - True: constrain all axes (default) - False/None: no constraint - ‘z’: constrain z-axis only - ‘xy’: constrain x,y axes - [1, 1, 0]: direct specification (constrain x,y)
- Returns:
dif_pos – difference position of self coordinates and coords considering position_mask.
- Return type:
Examples
>>> from skrobot.coordinates import Coordinates >>> 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]) >>> c1.difference_position(c2, position_mask='z') array([ 0. , 0. , -0.2])
- difference_rotation(coords, rotation_mask=True, rotation_mirror=None, **kwargs)[source]
Return differences in rotation of given coords.
- Parameters:
coords (skrobot.coordinates.Coordinates) – given coordinates
rotation_mask (str, bool, list, or numpy.ndarray) – Specifies which rotation axes to CONSTRAIN. - True: constrain all axes (default) - False/None: no constraint - ‘z’: constrain z-axis only - ‘xy’: constrain x,y axes - [1, 1, 0]: direct specification (constrain x,y)
rotation_mirror (str or None) – Mirror axis (‘x’, ‘y’, ‘z’) for allowing 180-degree rotations around the specified axis.
- Returns:
dif_rot – difference rotation of self coordinates and coords considering rotation_mask.
- Return type:
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_mask=False) array([0, 0, 0]) >>> coord1.difference_rotation(coord2, rotation_mask='yz') array([ 0. , 1.17434985, 1.05738936])
Using mirror option:
>>> coord1 = Coordinates() >>> coord2 = Coordinates().rotate(pi, 'x') >>> coord1.difference_rotation(coord2, rotation_mask=True, ... rotation_mirror='x') array([-2.99951957e-32, 0.00000000e+00, 0.00000000e+00])
- get_transform()[source]
Return Transform object
- Returns:
transform – corresponding Transform to this coordinates
- Return type:
skrobot.coordinates.base.Transform
- interpolate(other, ratio)[source]
Interpolate between this coordinate and another coordinate.
This is an alias for
slerp(). Returns a new coordinate that is interpolated between this coordinate and the other coordinate using spherical linear interpolation (SLERP).- Parameters:
other (skrobot.coordinates.Coordinates) – The target coordinate to interpolate towards.
ratio (float) – Interpolation ratio in [0, 1]. 0.0 returns this coordinate, 1.0 returns the other coordinate, 0.5 returns the midpoint.
- Returns:
interpolated – A new coordinate interpolated between self and other.
- Return type:
Examples
>>> from skrobot.coordinates import Coordinates >>> c1 = Coordinates() >>> c2 = Coordinates(pos=[1, 0, 0]) >>> c_mid = c1.interpolate(c2, 0.5) >>> c_mid.translation array([0.5, 0. , 0. ])
>>> c_quarter = c1.interpolate(c2, 0.25) >>> c_quarter.translation array([0.25, 0. , 0. ])
- inverse_transform_vector(vec)[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:
- 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:
- lerp(other, ratio)[source]
Linear interpolation between this coordinate and another.
Performs linear interpolation (LERP) between this coordinate and another coordinate. LERP is faster than SLERP but does not provide constant angular velocity for rotation.
- Parameters:
other (skrobot.coordinates.Coordinates) – The target coordinate to interpolate towards.
ratio (float) – Interpolation ratio in [0, 1]. 0.0 returns this coordinate, 1.0 returns the other coordinate, 0.5 returns the midpoint.
- Returns:
interpolated – A new coordinate interpolated between self and other using LERP.
- Return type:
Examples
>>> from skrobot.coordinates import Coordinates >>> c1 = Coordinates() >>> c2 = Coordinates(pos=[2, 2, 2]) >>> c_mid = c1.lerp(c2, 0.5) >>> c_mid.translation array([1., 1., 1.])
See also
slerpSpherical linear interpolation (constant angular velocity)
interpolateAlias for slerp
- move_coords(target_coords, local_coords)[source]
Transform this coordinate so that local_coords to target_coords.
- Parameters:
target_coords (skrobot.coordinates.Coordinates) – target coords.
local_coords (skrobot.coordinates.Coordinates) – local coords to be aligned.
- Returns:
self.worldcoords() – world coordinates.
- Return type:
- newcoords(c, pos=None, check_validity=True, relative_coords=None)[source]
Update of coords is always done through newcoords.
- Parameters:
c (skrobot.coordinates.Coordinates or numpy.ndarray) – If pos is None, c represents a Coordinates instance. If pos is given, c represents a rotation matrix.
pos (numpy.ndarray or None) – New translation.
check_validity (bool) – If True, check whether the input rotation and translation are valid.
relative_coords (skrobot.coordinates.Coordinates or str or None) –
Specifies the coordinate frame in which the input coordinates are expressed.
None or ‘local’: The input coordinates are treated as local coordinates. The coordinates are directly set without transformation. Example: coord.newcoords(c) directly sets coord to c’s values. This is equivalent to: coord = c (for root coordinates)
Note: If you want to set world coordinates, you have two options:
Manual conversion (complex): coord.newcoords(parent.worldcoords().inverse_transformation() * world_target)
Use relative_coords=’world’ (recommended): coord.newcoords(world_target, relative_coords=’world’)
’world’: The input coordinates are treated as world coordinates. They are transformed to the local frame before being set. Example: coord.newcoords(c, relative_coords=’world’) sets coord such that coord.worldcoords() equals c.
’parent’: The input coordinates are relative to the parent coordinate. Only meaningful for CascadedCoords with a parent. Example: child.newcoords(c, relative_coords=’parent’) sets child’s position relative to its parent.
Coordinates instance: The input is relative to the given coordinate frame. Example: coord.newcoords(c, relative_coords=ref) sets coord such that ref.transform(c) becomes coord’s world coordinates.
Examples
>>> from skrobot.coordinates import make_coords >>> coord = make_coords(pos=[1, 0, 0]) >>> >>> # Direct assignment (default behavior) >>> new_c = make_coords(pos=[2, 2, 2]) >>> coord.newcoords(new_c) >>> coord.translation array([2., 2., 2.]) >>> >>> # World coordinate specification >>> coord = make_coords(pos=[1, 0, 0]).rotate(np.pi/2, 'z') >>> world_c = make_coords(pos=[3, 3, 3]) >>> coord.newcoords(world_c, relative_coords='world') >>> coord.worldpos() # Will be [3, 3, 3] array([3., 3., 3.]) >>> >>> # Understanding the inverse_transformation relationship >>> parent = make_coords(pos=[5, 5, 5]).rotate(np.pi/4, 'z') >>> child = make_coords() >>> child.parent = parent # Simulating parent-child relationship >>> >>> # To set child's world position to [10, 10, 10] using local coords: >>> world_target = make_coords(pos=[10, 10, 10]) >>> local_coords = parent.inverse_transformation() * world_target >>> child.newcoords(local_coords) # Sets local coords relative to parent >>> # Verify: parent * local_coords = world_target >>> (parent * child).worldpos() array([10., 10., 10.])
- orient_with_matrix(rotation_matrix, wrt='world')[source]
Force update this coordinate system’s rotation.
- Parameters:
rotation_matrix (numpy.ndarray) – 3x3 rotation matrix.
wrt (str or skrobot.coordinates.Coordinates) – reference coordinates.
- rotate(theta, axis=None, wrt='local', skip_normalization=False)[source]
Rotate this coordinate by given theta and axis.
This coordinate system is rotated relative to theta radians around the axis axis. Note that this function does not change a position of this coordinate. If you want to rotate this coordinates around with world frame, you can use transform function. Please see examples.
- Parameters:
theta (float) – relartive rotation angle in radian.
axis (str or None or numpy.ndarray) – axis of rotation. The value of axis is represented as wrt frame.
wrt (str or skrobot.coordinates.Coordinates)
skip_normalization (bool) – if True, skip normalization for axis.
- Returns:
self
- Return type:
Examples
>>> from skrobot.coordinates import Coordinates >>> from numpy import pi >>> c = Coordinates() >>> c.translate((1.0, 0, 0)) >>> c.rotate(pi / 2.0, 'z', wrt='local') >>> c.translation array([1., 0., 0.])
>>> c.transform(Coordinates().rotate(np.pi / 2.0, 'z'), wrt='world') >>> c.translation array([0., 1., 0.])
- 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:
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(mat, wrt='local')[source]
Rotate this coordinate by given rotation matrix.
This is a subroutine of self.rotate function.
- Parameters:
mat (numpy.ndarray) – rotation matrix shape of (3, 3)
wrt (str or skrobot.coordinates.Coordinates) – with respect to.
- Returns:
self
- Return type:
- rpy_angle()[source]
Return a pair of rpy angles of this coordinates.
Deprecated since version This: method is deprecated and confusing. Use matrix2ypr() or matrix2rpy() instead.
- Returns:
rpy_angle(self._rotation) – a pair of rpy angles. See also skrobot.coordinates.math.rpy_angle
- Return type:
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]))
- slerp(other, ratio)[source]
Spherical linear interpolation between this coordinate and another.
Performs spherical linear interpolation (SLERP) between this coordinate and another coordinate. SLERP provides constant angular velocity rotation interpolation.
- Parameters:
other (skrobot.coordinates.Coordinates) – The target coordinate to interpolate towards.
ratio (float) – Interpolation ratio in [0, 1]. 0.0 returns this coordinate, 1.0 returns the other coordinate, 0.5 returns the midpoint.
- Returns:
interpolated – A new coordinate interpolated between self and other using SLERP.
- Return type:
Examples
>>> from skrobot.coordinates import Coordinates >>> import numpy as np >>> c1 = Coordinates() >>> c2 = Coordinates(pos=[1, 0, 0]).rotate(np.pi / 2, 'z') >>> c_mid = c1.slerp(c2, 0.5) >>> c_mid.translation array([0.5, 0. , 0. ])
See also
lerpLinear interpolation (faster but non-constant angular velocity)
interpolateAlias for slerp
- transform(c, wrt='local', out=None)[source]
Transform this coordinates by coords based on wrt
Note that this function changes this coordinates translation and rotation. If you would like not to change this coordinates, Please use copy_worldcoords() or give out.
- Parameters:
c (skrobot.coordinates.Coordinates) – coordinate
wrt (str or skrobot.coordinates.Coordinates) – If wrt is ‘local’ or self, multiply c from the right. If wrt is ‘world’ or ‘parent’ or self.parent, transform c with respect to worldcoord. 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 this coordinate
- Return type:
Examples
- 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:
- 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])
- world_position_quaternion_wxyz()[source]
Return world position and quaternion (wxyz).
This is a convenience method for getting both world position and world quaternion.
- Returns:
position (numpy.ndarray) – [x, y, z] world position.
quaternion (numpy.ndarray) – [w, x, y, z] world quaternion.
Examples
>>> from skrobot.coordinates import make_coords >>> c = make_coords(pos=[1, 2, 3]) >>> pos, quat = c.world_position_quaternion_wxyz() >>> pos array([1., 2., 3.]) >>> quat array([1., 0., 0., 0.])
- world_position_quaternion_xyzw()[source]
Return world position and quaternion (xyzw).
This is a convenience method for getting both world position and world quaternion in a format commonly used for 3D transforms (e.g., Three.js, ROS geometry_msgs).
- Returns:
position (numpy.ndarray) – [x, y, z] world position.
quaternion (numpy.ndarray) – [x, y, z, w] world quaternion.
Examples
>>> from skrobot.coordinates import make_coords >>> c = make_coords(pos=[1, 2, 3]) >>> pos, quat = c.world_position_quaternion_xyzw() >>> pos array([1., 2., 3.]) >>> quat array([0., 0., 0., 1.])
- world_quaternion_wxyz()[source]
Return world quaternion in [w, x, y, z] format.
- Returns:
q – [w, x, y, z] quaternion representing world rotation.
- Return type:
Examples
>>> from skrobot.coordinates import make_coords >>> c = make_coords() >>> c.world_quaternion_wxyz() array([1., 0., 0., 0.])
- world_quaternion_xyzw()[source]
Return world quaternion in [x, y, z, w] format.
- Returns:
q – [x, y, z, w] quaternion representing world rotation.
- Return type:
Examples
>>> from skrobot.coordinates import make_coords >>> c = make_coords() >>> c.world_quaternion_xyzw() array([0., 0., 0., 1.])
- worldpos()[source]
Return translation of this coordinate
See also skrobot.coordinates.Coordinates.translation
- Returns:
self.translation – translation of this coordinate
- Return type:
- worldrot()[source]
Return rotation of this coordinate
See also skrobot.coordinates.Coordinates.rotation
- Returns:
self._rotation – rotation matrix of this coordinate
- Return type:
- __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:
- __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:
Attributes
- dimension
Return dimension of this coordinate
- Returns:
len(self.translation) – dimension of this coordinate
- Return type:
- quaternion
Property of quaternion in [w, x, y, z] format
- Returns:
q – [w, x, y, z] quaternion
- Return type:
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])
- quaternion_wxyz
Property of quaternion in [w, x, y, z] format
- Returns:
q – [w, x, y, z] quaternion
- Return type:
- quaternion_xyzw
Property of quaternion in [x, y, z, w] format
- Returns:
q – [x, y, z, w] quaternion
- Return type:
Examples
>>> from numpy import pi >>> from skrobot.coordinates import make_coords >>> c = make_coords() >>> c.quaternion_xyzw array([0., 0., 0., 1.]) >>> c.rotate(pi / 3, 'y').rotate(pi / 5, 'z') >>> c.quaternion_xyzw array([0.1545085 , 0.47552826, 0.26761657, 0.8236391 ])
- rotation
Return rotation matrix of this coordinates.
- Returns:
self._rotation – 3x3 rotation matrix
- Return type:
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]])
- rotation_matrix
Alias for rotation property for better clarity.
Using rotation_matrix makes it more explicit that this returns a 3x3 rotation matrix, not angles or quaternions.
- Returns:
3x3 rotation matrix
- Return type:
See also
rotationThe original property (same as rotation_matrix)
- translation
Return translation of this coordinates.
- Returns:
self._translation – vector shape of (3, ). unit is [m]
- Return type:
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])
- translation_vector
Alias for translation property for better clarity.
Using translation_vector makes it more explicit that this returns a 3D translation vector, providing consistency with rotation_matrix.
- Returns:
3D translation vector [x, y, z] in meters
- Return type:
See also
translationThe original property (same as translation_vector)
- x_axis
Return x axis vector of this coordinates.
- Returns:
axis – x axis.
- Return type:
- y_axis
Return y axis vector of this coordinates.
- Returns:
axis – y axis.
- Return type:
- z_axis
Return z axis vector of this coordinates.
- Returns:
axis – z axis.
- Return type: