skrobot.models.pr2.PR2

class skrobot.models.pr2.PR2(use_tight_joint_limit=True)[source]

PR2 Robot Model.

Methods

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]])

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:

skrobot.coordinates.Coordinates

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

angle_vector(av=None, return_av=None)[source]

Get or set joint angle vector.

This is the core function for joint angle manipulation. If av is provided, it updates the angles of all joints. Joint limits are automatically enforced when setting angles.

Parameters:

av (numpy.ndarray or None) – Joint angle vector to set. If None, returns current joint angles. Values that violate joint limits are automatically clipped to valid range.
return_av (numpy.ndarray or None) – Pre-allocated array to store the result. If None, creates a new array. This can improve performance when called repeatedly in optimization loops.

Returns:

return_av – Current joint angle vector. Shape is (n_joints,) where n_joints is the total number of joint degrees of freedom.

Return type:

numpy.ndarray

Notes

This function handles:

Joint limit enforcement through min/max table lookup when available
Multi-DOF joints by processing multiple angles per joint
Efficient memory allocation when return_av is pre-allocated

The function uses CascadedCoords lazy evaluation, so coordinate transformations are automatically updated when joint angles change.

Examples

>>> import numpy as np
>>> from skrobot.models import PR2
>>> robot = PR2()
>>> # Get current joint angles
>>> current_angles = robot.angle_vector()
>>> # Set new joint angles (with automatic limit enforcement)
>>> new_angles = np.zeros(len(robot.joint_list))
>>> robot.angle_vector(new_angles)
>>> # Efficient repeated calls with pre-allocated array
>>> result_array = np.zeros(len(robot.joint_list))
>>> for i in range(100):
...     angles = robot.angle_vector(return_av=result_array)

apply_joint_limit_table_constraints(max_iterations=5)[source]

Apply joint limit table constraints iteratively.

For joints with bidirectional joint limit tables (where joint A’s limits depend on joint B and vice versa), this method iteratively clips joint angles until all constraints are satisfied.

Parameters:: max_iterations (int) – Maximum number of iterations for convergence.
Returns:: The angle vector after applying constraints.
Return type:: numpy.ndarray

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]

batch_inverse_kinematics(target_coords, move_target=None, link_list=None, position_mask=None, rotation_mask=None, rotation_mirror=None, stop=100, thre=0.001, rthre=np.float64(0.017453292519943295), initial_angles='current', alpha=1.0, attempts_per_pose=1, random_initial_range=0.7, translation_tolerance=None, rotation_tolerance=None, backend=None, use_base=False, base_weight=None, joint_list=None, invariant_joint_list=None, **kwargs)[source]

Solve batch inverse kinematics for multiple target poses.

This method efficiently processes multiple target poses in parallel, providing significant performance improvements over sequential IK solving.

Parameters:

target_coords (Union[np.ndarray, List[Coordinates]]) – Target poses as numpy array in shape (batch_size, 6) where each row is [x, y, z, roll, pitch, yaw] or (batch_size, 7) where quaternion is included [x, y, z, qx, qy, qz, qw], or list of Coordinates objects
move_target (Optional) – Target link or end-effector. If None, uses self.end_coords
link_list (Optional[List]) – (Legacy) List of links from root to target; each link’s link.joint is actuated. Equivalent to joint_list (the recommended spelling). If None, automatically computed from move_target.
position_mask (str, bool, list, numpy.ndarray, or None) – Specifies which position 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_mask (str, bool, list, numpy.ndarray, or None) – Specifies which rotation axes to CONSTRAIN. Same format as position_mask.
joint_list (list[skrobot.model.Joint] or list[list] or None) – (Recommended) Joints to actuate – the most direct way to express “which joints move” (same semantics as inverse_kinematics()). Mutually exclusive with link_list.
invariant_joint_list (list[skrobot.model.Joint] or None) – Joints to hold fixed during the solve. The actuated (variant) joints become the resolved chain minus these. Raises ValueError if it would remove every actuated joint (unless use_base supplies base DoFs).
rotation_mirror (str or None) – Mirror axis (‘x’, ‘y’, ‘z’) for allowing 180-degree rotations around the specified axis.
stop (int) – Maximum number of iterations
thre (float) – Position error threshold in meters
rthre (float) – Rotation error threshold in radians
initial_angles (Optional[Union[np.ndarray, str]]) – Initial joint angles. Can be: - “current”: Use current robot joint angles for all poses (default) - None or “random”: Use random initial angles - np.ndarray of shape (batch_size, ndof): Use provided initial angles Note: When attempts_per_pose > 1, defaults to “random” for better exploration
alpha (float) – Step size for gradient descent (0 < alpha <= 1)
attempts_per_pose (int) – Number of attempts with different random initial poses per target (default: 1)
random_initial_range (float) – Range for random initial poses as fraction of joint limits (0.0-1.0, default: 0.7)
translation_tolerance (list or None) – Per-axis position tolerance from target as [x_tol, y_tol, z_tol] in meters. If error on an axis is within tolerance, it’s treated as reached. Use None for no tolerance on an axis.
rotation_tolerance (list or None) – Per-axis rotation tolerance from target as [roll_tol, pitch_tol, yaw_tol] in radians. If error on an axis is within tolerance, it’s treated as reached.
backend (str or None) – Backend solver to use (‘numpy’ or ‘jax’). Default is None, which auto-selects JAX if available, otherwise falls back to NumPy. JAX backend provides faster computation through JIT compilation.
**kwargs (dict) – Additional keyword arguments

Returns:

List of joint angle solutions (each is ndarray matching self.angle_vector())
List of success flags indicating if IK was solved
List of attempt counts (only returned when attempts_per_pose > 1)

Return type:

Tuple[List[np.ndarray], List[bool], List[int]] or Tuple[List[np.ndarray], List[bool]]

Examples

>>> import numpy as np
>>> robot = Fetch()
>>> robot.reset_pose()
>>>
>>> # Multiple target poses
>>> target_poses = np.array([
...     [0.8, -0.3, 0.8, 0.0, np.deg2rad(30), np.deg2rad(-30)],
...     [0.7, -0.2, 0.9, 0.0, np.deg2rad(45), np.deg2rad(-15)],
... ])
>>> solutions, success_flags = robot.batch_inverse_kinematics(target_poses)
>>>
>>> # Apply first successful solution
>>> for i, (solution, success) in enumerate(zip(solutions, success_flags)):
...     if success:
...         robot.angle_vector(solution)
...         break

calc_cog_jacobian_from_link_list(link_list=None, translation_axis=True, update_mass_properties=True, col_offset=0, n_joint_dimension=None, jacobian=None)[source]

Compute the Jacobian of the centre of gravity (CoG).

The CoG Jacobian relates joint velocities to the world-frame velocity of the robot’s centre of gravity:

\[\mathbf{J}_c = \frac{1}{M} \sum_i m_i \mathbf{J}_{c_i}\]

where \(\mathbf{J}_{c_i}\) is the linear-velocity Jacobian of link i’s CoM, \(m_i\) is its mass, and \(M\) is the total robot mass. For a rotational joint j with world axis \(\hat{a}_j\) and origin \(\mathbf{p}_j\), \(\mathbf{J}_{c_i,j} = \hat{a}_j \times (\mathbf{c}_i^{\text{world}} - \mathbf{p}_j)\) if joint j is an ancestor of link i, and zero otherwise. For a linear (prismatic) joint, the column is \(\hat{a}_j\) for every affected link. The PlanarJoint and FloatingJoint virtual base joints used by use_base='planar' / '6dof' are handled by composing these primitives along the multi-DoF chain.

Parameters:

link_list (list, optional) – Joint chain(s) over which the Jacobian columns are filled. Either a flat list of links or a list of lists (multi-EE convention). If None, uses self.link_list.
translation_axis (str, bool, list, or numpy.ndarray) – Selects which CoG axes are constrained. True keeps all three (xyz); 'xy' returns a 2-row Jacobian for the horizontal projection of the CoM (the standard ZMP formulation); 'z' returns a 1-row Jacobian for the vertical CoM. Lists/arrays are passed to normalize_mask().
update_mass_properties (bool) – If True (default), recompute link world poses + total mass via update_mass_properties() before assembling the Jacobian. Pass False if the IK loop has already updated mass properties this iteration to avoid redundant work.
col_offset (int) – Column offset into the destination Jacobian. Used by inverse_kinematics_loop when stacking the CoG row(s) into the augmented system.
n_joint_dimension (int, optional) – Total joint DoF used by the IK problem. If None, derived from the union link list.
jacobian (numpy.ndarray, optional) – Pre-allocated output array of shape (n_axes, dim) where n_axes = sum(translation_axis_mask). If None, a new array is allocated.

Returns:

CoG Jacobian of shape (n_axes, n_joint_dimension).

Return type:

numpy.ndarray

calc_inverse_jacobian(jacobi, manipulability_limit=0.1, manipulability_gain=0.001, weight=None, *args, **kwargs)[source]

calc_inverse_kinematics_nspace_from_link_list(link_list, avoid_nspace_gain=0.01, union_link_list=None, n_joint_dimension=None, null_space=None, additional_nspace_list=None, weight=None)[source]

calc_inverse_kinematics_weight_from_link_list(link_list, avoid_weight_gain=1.0, union_link_list=None, n_joint_dimension=None, weight=None, additional_weight_list=[])[source]: Calculate all weight from link list.

calc_jacobian_for_interlocking_joints(link_list, interlocking_joint_pairs=None)[source]

calc_jacobian_from_joint_list(union_joint_list, move_target, joint_list=None, transform_coords=None, position_mask=None, rotation_mask=None, rotation_mirror=None, col_offset=0, dim=None, jacobian=None, additional_jacobi_dimension=0, n_joint_dimension=None, check_relevance='auto', *args, **kwargs)[source]

Compute the Jacobian with columns indexed directly by joints.

This is the joint_list analogue of calc_jacobian_from_link_list(). That method takes a link_list and derives the actuated joints indirectly via link.joint; here the joints are given directly, so the columns of the returned Jacobian correspond, in order, to union_joint_list (each joint contributing joint.joint_dof columns).

A joint contributes to a given move_target iff it appears in that target’s joint_list (the joints along the kinematic chain from the target back to the root) and the relevance check (check_relevance) allows it. The per-target membership rule is what lets the column set be an arbitrary union of joints, e.g. spanning multiple bodies or virtual contact joints.

With check_relevance='auto' (the default) this method is a complete superset of calc_jacobian_from_link_list(): for the robot’s own joints it reproduces that method’s relevance filtering exactly, while for joints outside the relevance table it falls back to pure membership. calc_jacobian_from_link_list is implemented by delegating here.

Parameters:

union_joint_list (list[skrobot.model.Joint]) – Joints forming the Jacobian columns, in order. The number of columns equals sum(j.joint_dof for j in union_joint_list).
move_target (skrobot.coordinates.Coordinates or list) – End-effector coordinate frame(s).
joint_list (list[skrobot.model.Joint] or list[list] or None) – Per-move_target list of joints contained in that target’s kinematic chain. Flat for a single target, nested (one list per target) for multiple targets. When None it is derived from each target as the chain from the target’s parent link back to the root.
transform_coords (skrobot.coordinates.Coordinates or list or None) – Frame(s) the error is expressed in. Defaults to move_target.
position_mask – Axis constraint specification, same semantics as calc_jacobian_from_link_list().
rotation_mask – Axis constraint specification, same semantics as calc_jacobian_from_link_list().
rotation_mirror – Axis constraint specification, same semantics as calc_jacobian_from_link_list().
col_offset (int) – Column index at which to start writing.
dim (int or None) – Row dimension of the Jacobian. Defaults to the sum of the per-target task dimensions plus additional_jacobi_dimension.
jacobian (numpy.ndarray or None) – Pre-allocated output matrix. Allocated when None.
additional_jacobi_dimension (int) – Extra rows to reserve below the kinematic block.
n_joint_dimension (int or None) – Column dimension. Defaults to the total DoF of union_joint_list.
check_relevance ({'auto', True, False}) – How to filter a joint that is in a target’s membership list but may not actually move it. 'auto' (default) uses the relevance table for known robot joints and trusts membership for unknown joints – a superset of calc_jacobian_from_link_list(). True always consults the relevance table (an unknown joint is excluded, exactly like _is_relevant()). False ignores relevance and trusts membership only.

Returns:

The Jacobian matrix.

Return type:

numpy.ndarray

calc_jacobian_from_link_list(move_target, link_list=None, transform_coords=None, position_mask=None, rotation_mask=None, rotation_mirror=None, col_offset=0, dim=None, jacobian=None, additional_jacobi_dimension=0, n_joint_dimension=None, *args, **kwargs)[source]

calc_joint_angle_from_min_max_table(index, j, av)[source]

calc_joint_angle_speed(union_vel, angle_speed=None, angle_speed_blending=0.5, jacobi=None, j_sharp=None, null_space=None, *args, **kwargs)[source]

calc_joint_angle_speed_gain(union_link_list, dav, periodic_time)[source]

calc_nspace_from_joint_limit(avoid_nspace_gain, union_link_list, weight)[source]: Calculate null-space according to joint limit.

calc_target_axis_dimension(rotation_mask, position_mask, rotation_mirror=None)[source]

Calculate target axis dimension from masks.

Parameters:

rotation_mask (numpy.ndarray or list) – Rotation mask (1=constrained, 0=free).
position_mask (numpy.ndarray or list) – Position mask (1=constrained, 0=free).
rotation_mirror (str or None) – Mirror axis for rotation. When specified, rotation dimension is always 3 (full) to handle 180-degree flips.

Returns:

dim – Total dimension of constrained axes.

Return type:

int

calc_target_joint_dimension(link_list)[source]

calc_union_link_list(link_list)[source]

calc_vel_for_cog(target_centroid_pos, translation_axis=True, cog_gain=1.0, update_mass_properties=True, centroid_offset_func=None)[source]

Compute the desired CoG velocity (delta) toward a world target.

Used by inverse_kinematics_loop together with calc_cog_jacobian_from_link_list() to drive the centre of gravity to a specified world-frame position.

Parameters:

target_centroid_pos (array-like) – Desired CoG position in world coordinates.
translation_axis (str, bool, list, or numpy.ndarray) – Selects which CoG axes participate in the error.
cog_gain (float) – Gain on the CoG error vector. 1.0 aims to close the error in one IK iteration; smaller values are more conservative.
update_mass_properties (bool) – If True, refresh self.update_mass_properties() before reading the current CoG. Pass False from inside the IK loop after a recent update to amortise cost.
centroid_offset_func (callable or None) – If provided, the result is shifted by centroid_offset_func() before computing the error. Used when a non-link-attached object (e.g. carried payload) should be considered part of the CoG.

Returns:

Desired velocity vector of length sum(axis_mask).

Return type:

numpy.ndarray

calc_vel_for_interlocking_joints(link_list, interlocking_joint_pairs=None)[source]

Calculate 0 velocity for keeping interlocking joint.

at the same joint angle.

calc_vel_from_pos(dif_pos, position_mask=True, p_limit=100.0)[source]

Calculate velocity from difference position

Parameters:

dif_pos (np.ndarray) – [m] order
position_mask (str, bool, list, or numpy.ndarray) – Specifies which position axes to CONSTRAIN
p_limit (float) – Position limit

Returns:

vel_p

Return type:

np.ndarray

calc_vel_from_rot(dif_rot, rotation_mask=True, r_limit=0.5, rotation_mirror=None)[source]

Calculate velocity from difference rotation

Parameters:

dif_rot (np.ndarray) – Rotation difference
rotation_mask (str, bool, list, or numpy.ndarray) – Specifies which rotation axes to CONSTRAIN
r_limit (float) – Rotation limit
rotation_mirror (str or None) – Mirror axis

Returns:

vel_r

Return type:

np.ndarray

calc_weight_from_joint_limit(avoid_weight_gain, link_list, union_link_list, weight, n_joint_dimension=None)[source]: Calculate weight according to joint limit.

centroid(update_mass_properties=True)[source]

Calculate total robot centroid (Center Of Gravity, COG) in world coordinates.

Parameters:: update_mass_properties (bool, optional) – If True, recalculate total mass properties for all links and return the total robot centroid. If False, return pre-computed centroid without recalculation. Default is True.
Returns:: 3D vector of robot centroid position [m] in world coordinates.
Return type:: numpy.ndarray

Notes

This method calculates the center of gravity (centroid) of the entire robot by considering the mass and centroid of each link. The calculation is based on the weighted average of all link centroids in world coordinates.

The centroid is calculated as: centroid = sum(mass_i * position_i) / sum(mass_i)

where mass_i and position_i are the mass and world position of link i.

Examples

>>> robot = RobotModel()
>>> cog = robot.centroid()  # Calculate and return centroid
>>> cog_cached = robot.centroid(update_mass_properties=False)  # Use cached values

changed()[source]

Return False

This is used for CascadedCoords compatibility

Returns:: False – always return False
Return type:: bool

collision_avoidance_link_pair_from_link_list(link_list, obstacles=None)[source]

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, 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:

numpy.ndarray

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:

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_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])

disable_hook()[source]

dissoc(child)[source]

static filter_movable_links(link_list)[source]

Filter link list to include only links with movable joints.

Parameters:: link_list (list of Link) – List of links to filter.
Returns:: Links that have movable (non-fixed) joints.
Return type:: list of Link

Examples

>>> from skrobot.models import PR2
>>> robot = PR2()
>>> all_links = robot.link_lists(robot.r_gripper_tool_frame, robot.root_link)
>>> movable = RobotModel.filter_movable_links(all_links)

find_joint_angle_limit_weight_from_union_link_list(union_link_list)[source]

find_link_path(src_link, target_link)[source]

Find paths of src_link to target_link

Parameters:

src_link (skrobot.model.link.Link) – source link.
target_link (skrobot.model.link.Link) – target link.

Returns:

ret – If the links are connected, return Link list. Otherwise, return an empty list.

Return type:

List[skrobot.model.link.Link]

find_link_route(to, frm=None)[source]

fix_leg_to_coords(fix_coords, leg='both', mid=0.5)[source]

Fix robot’s legs to a coords

In the Following codes, leged robot is assumed.

Parameters:

fix_coords (Coordinates) – target coordinate
leg (string) – [‘both’, ‘rleg’, ‘rleg’, ‘left’, ‘right’]
mid (float) – ratio of legs coord.

forward_kinematics(av=None, return_av=None)[source]

Compute forward kinematics.

This function sets joint angles and computes the forward kinematics to update the positions and orientations of all links in the robot.

Parameters:

av (numpy.ndarray or None) – Joint angle vector. If None, uses current joint angles.
return_av (numpy.ndarray or None) – Pre-allocated array to store the result. If None, creates a new array. This can improve performance when called repeatedly.

Returns:

av – Current angle vector after forward kinematics computation.

Return type:

numpy.ndarray

Notes

This is a wrapper function for angle_vector that provides a more intuitive name for computing forward kinematics. The function:

Sets joint angles if av is provided
Updates the kinematic chain (implicit through joint angle setting)
Returns the current angle vector

Important: After setting joint angles with joint.joint_angle(), you typically do not need to explicitly call forward_kinematics() because scikit-robot uses the CascadedCoords class which implements lazy evaluation. The coordinate transformations are automatically computed when needed (e.g., when accessing worldpos() or worldrot()). Explicit calls to forward_kinematics() are only necessary when you need to ensure all transformations are computed immediately.

Examples

>>> import numpy as np
>>> from skrobot.models import PR2
>>> robot = PR2()
>>> # Set specific joint angles and compute forward kinematics
>>> angles = np.zeros(len(robot.joint_list))
>>> current_av = robot.forward_kinematics(angles)
>>> # Get current configuration after forward kinematics
>>> pos = robot.rarm.end_coords.worldpos()

static from_robot_description(param_name='/robot_description', include_mimic_joints=True)[source]

Load URDF from ROS parameter server.

Supports both ROS1 and ROS2 based on the ROS_VERSION environment variable.

Parameters:

param_name (str) – Parameter name in the parameter server.
include_mimic_joints (bool, optional) – If True, mimic joints are included in the resulting RobotModel’s joint_list.

Returns:

Robot model loaded from URDF.

Return type:

RobotModel

static from_urdf(urdf_input, include_mimic_joints=True)[source]

Load URDF from a string or a file path.

Automatically detects if the input is a URDF string or a file path.

Parameters:

urdf_string (str) – Either the URDF model description as a string, or the path to a URDF file.
include_mimic_joints (bool, optional) – If True, mimic joints are included in the resulting RobotModel’s joint_list.

Returns:

Robot model loaded from the URDF.

Return type:

RobotModel

get_transform()[source]

Return Transform object

Returns:: transform – corresponding Transform to this coordinates
Return type:: skrobot.coordinates.base.Transform

gripper_distance(dist=None, arm='arms')[source]

Change gripper angle function

Parameters:

dist (None or float) – gripper distance. If dist is None, return gripper distance. If float value is given, change joint angle.
arm (str) – Specify target arm. You can specify ‘left_arm’, ‘right_arm’, ‘arms’, or legacy names ‘larm’, ‘rarm’.

Returns:

dist – The current gripper distance in meters. - If arm is ‘left_arm’ or ‘right_arm’, returns a single float. - If arm is ‘arms’, returns a list of two floats [right_dist, left_dist].

Return type:

float or list of float

ik_convergence_check(dif_pos, dif_rot, thre, rthre)[source]

Check IK convergence.

Parameters:

dif_pos (list of np.ndarray) – Position differences for each target.
dif_rot (list of np.ndarray) – Rotation differences for each target.
thre (list of float) – Position error thresholds.
rthre (list of float) – Rotation error thresholds.

Returns:

True if converged, False otherwise.

Return type:

bool

init_pose()[source]

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:

skrobot.coordinates.Coordinates

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.  ])

See also

slerp: Spherical linear interpolation (same as interpolate)
lerp: Linear interpolation

inverse_dynamics(external_forces=None, external_moments=None, external_coords=None, gravity=None, backend='numpy')[source]

Compute joint torques using inverse dynamics (Newton-Euler algorithm).

Parameters:

external_forces (list of np.ndarray, optional) – External forces [N] applied at external_coords.
external_moments (list of np.ndarray, optional) – External moments [Nm] applied at external_coords.
external_coords (list of coordinates, optional) – Coordinate frames where external forces/moments are applied.
gravity (np.ndarray, optional) – Gravity vector [m/s^2]. Defaults to [0, 0, -9.80665].
backend (str, optional) – Backend to use (‘numpy’ or ‘jax’). Defaults to ‘numpy’. JAX backend provides ~300x speedup but requires JAX installation.

Returns:

joint_torques – Joint torques [Nm] or forces [N] for linear joints.

Return type:

np.ndarray

inverse_kinematics(target_coords, move_target=None, link_list=None, joint_list=None, **kwargs)[source]

Solve inverse kinematics.

solve inverse kinematics, move move-target to target-coords look-at- target supports t, nil, float-vector, coords, list of float-vector, list of coords link-list is set by default based on move-target -> root link link-list.

joint_list is an alternative to link_list that names the joints to actuate directly (see the base inverse_kinematics); the two are mutually exclusive. When joint_list is given the default move_target -> root link_list is not inferred.

inverse_kinematics_args(union_link_list=None, position_mask=None, rotation_mask=None, rotation_mirror=None, additional_jacobi_dimension=0, **kwargs)[source]

inverse_kinematics_loop(dif_pos, dif_rot, move_target, link_list=None, target_coords=None, **kwargs)[source]

move move_target using dif_pos and dif_rot.

Parameters:: TODO
Return type:: TODO

inverse_kinematics_loop_for_look_at(move_target, look_at, link_list, rotation_mask='xy', position_mask=False, rthre=0.001, **kwargs)[source]

Solve look at inverse kinematics

Parameters:: look_at (list or np.ndarray or Coordinates)

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

static joint_limits_from_joint_list(joint_list)[source]: Compute joint limits from a list of joints.

static joint_list_from_link_list(link_list, ignore_fixed_joint=True)[source]

Generate a list of joints from a list of links.

Generate a list of joints from a list of links, optionally ignoring fixed joints.

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:

skrobot.coordinates.Coordinates

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

slerp: Spherical linear interpolation (constant angular velocity)
interpolate: Alias for slerp

link_lists(to, frm=None)[source]: Find link list from to link to frm link.

load_urdf(urdf, include_mimic_joints=True)[source]

load_urdf_file(file_obj, include_mimic_joints=True)[source]

Load robot model from URDF file.

This method parses a URDF file, creates the corresponding link and joint objects, and sets up the robot’s kinematic tree structure. It also loads visual and collision meshes.

Parameters:

file_obj (str or file-like object) – Path to the URDF file or a file-like object containing URDF data.
include_mimic_joints (bool, optional) – If True, mimic joints are included in the self.joint_list. If False, mimic joints are excluded from self.joint_list, although their mimic definitions are still processed and applied to the joints they mimic.

load_urdf_from_robot_description(param_name='/robot_description', include_mimic_joints=True, timeout_seconds=10)[source]

Load URDF from ROS parameter server and initialize the model.

Waits until the specified ROS parameter is available, reads the URDF content, saves it to a temporary file, and then loads it using load_urdf_file. Supports both ROS1 and ROS2.

Parameters:

param_name (str, optional) – The name of the ROS parameter containing the URDF XML string. Defaults to ‘/robot_description’.
include_mimic_joints (bool, optional) – If True, mimic joints are included in the self.joint_list. Passed directly to load_urdf_file.
timeout_seconds (int, optional) – Timeout to load urdf from robot_description. (ROS1 only)

look_at(coords, target=None, link_list=None)[source]

look_at_hand(coords)[source]

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:

skrobot.coordinates.Coordinates

move_end_pos(pos, wrt='local', *args, **kwargs)[source]

move_end_rot(angle, axis, wrt='local', *args, **kwargs)[source]

move_joints(union_vel, union_link_list=None, periodic_time=0.05, joint_args=None, move_joints_hook=None, *args, **kwargs)[source]

move_joints_avoidance(union_vel, union_link_list=None, link_list=None, n_joint_dimension=None, weight=None, null_space=None, avoid_nspace_gain=0.01, avoid_weight_gain=1.0, avoid_collision_distance=200, avoid_collision_null_gain=1.0, avoid_collision_joint_gain=1.0, collision_avoidance_link_pair=None, cog_gain=0.0, target_centroid_pos=None, centroid_offset_func=None, cog_translation_axis='z', cog_null_space=False, additional_weight_list=None, additional_nspace_list=None, jacobi=None, obstacles=None, *args, **kwargs)[source]

newcoords(target, pos=None, check_validity=True, relative_coords='local')[source]

Update this coordinate system with a new coordinate value.

This method updates the coordinates while maintaining the parent-child relationship. The key difference from Coordinates.newcoords is that this method handles the parent-child transformation automatically.

Parameters:

target (skrobot.coordinates.Coordinates or numpy.ndarray) – If pos is None, target represents a Coordinates instance that describes the new desired coordinate. If pos is provided, target represents a rotation matrix.
pos (numpy.ndarray or None) – The new translation vector.
check_validity (bool) – Whether to validate the inputs.
relative_coords (str or skrobot.coordinates.Coordinates, default 'local') –
Specifies the coordinate frame in which the target coordinates are expressed.
- ’local’ (default): The target represents coordinates in the local frame (relative to parent if it exists). This is the default for backward compatibility. Example: child.newcoords(c) directly sets child’s local transformation to c. For a child with parent, this means: child = parent * c (in world frame)
  
  Note: If you want to set world coordinates, you have two options:
  1. Manual conversion (complex): child.newcoords(parent.worldcoords().inverse_transformation() * world_target)
  2. Use relative_coords=’world’ (recommended): child.newcoords(world_target, relative_coords=’world’)
- ’world’: The target represents desired world coordinates. For child coordinates, the target is automatically converted to the parent’s local frame to maintain the correct world position. Example: child.newcoords(c, relative_coords=’world’) sets child such that child.worldcoords() equals c.
- ’local’: the target is already expressed in the child’s local frame.
- ’parent’: the target is given relative to the parent coordinate.
- Alternatively, a Coordinates instance can be provided as the reference frame.

Returns:

self

Return type:

CascadedCoords

Examples

>>> from skrobot.coordinates import make_cascoords
>>> parent = make_cascoords(pos=[10, 0, 0])
>>> child = make_cascoords(pos=[0, 5, 0])
>>> parent.assoc(child)
>>>
>>> # Setting world coordinates
>>> target_world = make_cascoords(pos=[15, 15, 15])
>>> child.newcoords(target_world, relative_coords='world')
>>> child.worldpos()
array([15., 15., 15.])
>>> child.translation  # Local position relative to parent
array([5., 15., 15.])
>>>
>>> # Setting local coordinates
>>> target_local = make_cascoords(pos=[2, 2, 2])
>>> child.newcoords(target_local, relative_coords='local')
>>> child.translation
array([2., 2., 2.])
>>> child.worldpos()  # World position is parent + local
array([12., 2., 2.])
>>>
>>> # Manual world coordinate setting using inverse_transformation
>>> world_target = make_cascoords(pos=[20, 20, 20])
>>> local_target = parent.worldcoords().inverse_transformation() * world_target
>>> child.newcoords(local_target)  # Default is 'local'
>>> child.worldpos()
array([20., 20., 20.])

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.

parent_orientation(v, wrt)[source]

parentcoords()[source]

reset_joint_angle_limit_weight(union_link_list)[source]

reset_manip_pose()[source]

reset_pose()[source]

rotate(theta, axis, wrt='local', skip_normalization=False)[source]

Rotate this coordinate.

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

Parameters:

theta (float) – radian
axis (str or numpy.ndarray) – ‘x’, ‘y’, ‘z’ or vector
wrt (str or Coordinates)
skip_normalization (bool) – if True, skip normalization for axis.

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:

mat (numpy.ndarray) – rotation matrix shape of (3, 3)
wrt (str or skrobot.coordinates.Coordinates) – with respect to.

Returns:

self

Return type:

skrobot.coordinates.Coordinates

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:: 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]))

static sanitize_joint_limits(min_angles, max_angles, joint_limit_eps=0.001)[source]

Sanitize joint limits.

Sanitize joint limits, replacing infinities and ensuring the range is non-zero.

self_collision_check()[source]

Return collision link pair

Note: This function uses trimesh.collision.CollisionManager internally. We need to install python-fcl to use this function. If you want to use this function, please install python-fcl by pip install python-fcl.

Returns:

is_collision (bool) – True if a collision occurred between any pair of objects and False otherwise
names (set of 2-tuple) – The set of pairwise collisions. Each tuple contains two names in alphabetical order indicating that the two corresponding objects are in collision.

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:

skrobot.coordinates.Coordinates

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

lerp: Linear interpolation (faster but non-constant angular velocity)
interpolate: Alias for slerp

torque_vector(force_list=None, moment_list=None, target_coords=None, calc_statics_p=True, dt=0.005, av=None, av_prev=None, av_next=None, root_coords=None, root_coords_prev=None, root_coords_next=None, gravity=None, backend='numpy')[source]

Calculate joint torques using inverse dynamics.

This method computes the joint torques required to achieve specified motions while balancing external forces using inverse dynamics.

Parameters:

force_list (list of np.ndarray, optional) – External forces [N] applied at target_coords.
moment_list (list of np.ndarray, optional) – External moments [Nm] applied at target_coords.
target_coords (list of coordinates, optional) – Coordinate frames where forces/moments are applied.
calc_statics_p (bool, optional) – If True, compute statics only. If False, compute full dynamics. Defaults to True.
dt (float, optional) – Time step for finite difference computation [s]. Defaults to 0.005.
av (np.ndarray, optional) – Current joint angle vector [rad]. If None, uses current angles.
av_prev (np.ndarray, optional) – Previous joint angle vector [rad]. For dynamics computation.
av_next (np.ndarray, optional) – Next joint angle vector [rad]. For dynamics computation.
root_coords (coordinates, optional) – Current root link coordinates. If None, uses current coordinates.
root_coords_prev (coordinates, optional) – Previous root link coordinates. For dynamics computation.
root_coords_next (coordinates, optional) – Next root link coordinates. For dynamics computation.
gravity (np.ndarray, optional) – Gravity vector [m/s^2]. Defaults to [0, 0, -9.81].
backend (str, optional) – Backend to use (‘numpy’ or ‘jax’). Defaults to ‘numpy’. JAX backend provides ~300x speedup but requires JAX installation.

Returns:

joint_torques – Joint torques [Nm] or forces [N] for linear joints required to achieve the specified motion and balance external forces.

Return type:

np.ndarray

Examples

>>> # Compute static torques to balance gravity
>>> torques = robot.torque_vector()
>>>
>>> # Compute torques with external force on end effector
>>> force = np.array([10, 0, 0])  # 10N in x direction
>>> torques = robot.torque_vector(
...     force_list=[force],
...     target_coords=[robot.rarm.end_coords]
... )
>>>
>>> # Compute dynamic torques for motion
>>> torques = robot.torque_vector(
...     calc_statics_p=False,
...     av_prev=prev_angles,
...     av=curr_angles,
...     av_next=next_angles,
...     dt=0.01
... )
>>>
>>> # Use JAX backend for ~300x speedup
>>> torques = robot.torque_vector(backend='jax')

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]

update_mass_properties()[source]

Update robot mass properties by summing over all links.

Returns:: Dictionary containing total mass, center of mass, and inertia tensor.
Return type:: dict

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:: numpy.ndarray

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:: numpy.ndarray

Examples

>>> from skrobot.coordinates import make_coords
>>> c = make_coords()
>>> c.world_quaternion_xyzw()
array([0., 0., 0., 1.])

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

arm: Arm kinematic chain for single-arm robots (alias for rarm).

arm_end_coords: Arm end effector coordinates for single-arm robots.

default_urdf_path

descendants

dimension

Return dimension of this coordinate

Returns:: len(self.translation) – dimension of this coordinate
Return type:: int

head

interlocking_joint_pairs

Interlocking joint pairs.

pairs are [(joint0, joint1), …] If users want to use interlocking joints, please overwrite this method.

joint_max_angles

joint_min_angles

larm

larm_with_torso

leaf_links

Get all leaf links (links with no children).

Leaf links are potential end-effector candidates as they are at the end of kinematic chains.

Returns:: List of links that have no child links.
Return type:: list of Link

Examples

>>> from skrobot.models import PR2
>>> robot = PR2()
>>> leaf_links = robot.leaf_links
>>> print([link.name for link in leaf_links])

left_arm

left_arm_end_coords

left_arm_with_torso

left_leg: Left leg kinematic chain (alias for lleg).

left_leg_end_coords: Left leg end effector coordinates (alias for lleg_end_coords).

lleg

name

Return this coordinate’s name

Returns:: self._name – name of this coordinate
Return type:: str

parent

quaternion

Property of quaternion in [w, x, y, z] format

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])

quaternion_wxyz

Property of quaternion in [w, x, y, z] format

Returns:: q – [w, x, y, z] quaternion
Return type:: numpy.ndarray

quaternion_xyzw

Property of quaternion in [x, y, z, w] format

Returns:: q – [x, y, z, w] quaternion
Return type:: numpy.ndarray

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 ])

rarm

rarm_with_torso

right_arm

right_arm_end_coords

right_arm_with_torso

right_leg: Right leg kinematic chain (alias for rleg).

right_leg_end_coords: Right leg end effector coordinates (alias for rleg_end_coords).

rleg

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]])

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:: numpy.ndarray

See also

rotation: The original property (same as rotation_matrix)

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])

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:: numpy.ndarray

See also

translation: The original property (same as translation_vector)

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