skrobot.models.pr2.PR2¶
- class skrobot.models.pr2.PR2(use_tight_joint_limit=True)[source]¶
PR2 Robot Model.
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]])
- angle_vector(av=None, return_av=None)[source]¶
Returns angle vector
If av is given, it updates angles of all joint. If given av violate min/max range, the value is modified.
- 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:
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.])
- 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_from_link_list(move_target, link_list=None, transform_coords=None, rotation_axis=None, translation_axis=None, col_offset=0, dim=None, jacobian=None, additional_jacobi_dimension=0, n_joint_dimension=None, *args, **kwargs)[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_nspace_from_joint_limit(avoid_nspace_gain, union_link_list, weight)[source]¶
Calculate null-space according to joint limit.
- calc_target_axis_dimension(rotation_axis, translation_axis)[source]¶
rotation-axis, translation-axis -> both list and atom OK.
- 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, translation_axis, p_limit=100.0)[source]¶
Calculate velocity from difference position
- Parameters:
dif_pos (np.ndarray) – [m] order
translation_axis (str) – see calc_dif_with_axis
- Returns:
vel_p
- 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.
- changed()[source]¶
Return False
This is used for CascadedCoords compatibility
- Returns:
False – always return False
- Return type:
- compute_qp_common(target_coords, move_target, dt, link_list=None, gain=0.85, weight=1.0, translation_axis=True, rotation_axis=True, dof_limit_gain=0.5)[source]¶
- compute_velocity(target_coords, move_target, dt, link_list=None, gain=0.85, weight=1.0, translation_axis=True, rotation_axis=True, dof_limit_gain=0.5, fast=True, sym_proj=False, solver='cvxopt', *args, **kwargs)[source]¶
- 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:
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:
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. ])
- 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]
- 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.
- get_transform()[source]¶
Return Transform object
- Returns:
transform – corrensponding Transform to this coordinates
- Return type:
skrobot.coordinates.base.Transform
- ik_convergence_check(dif_pos, dif_rot, rotation_axis, translation_axis, thre, rthre, centroid_thre=None, target_centroid_pos=None, centroid_offset_func=None, cog_translation_axis=None, update_mass_properties=True)[source]¶
check ik convergence.
- Parameters:
dif_pos (list of np.ndarray) –
dif_rot (list of np.ndarray) –
translation_axis (list of axis) –
rotation_axis (list of axis) – see _wrap_axis
- inverse_kinematics(target_coords, move_target=None, link_list=None, **kwargs)[source]¶
Solve inverse kinematics.
solve inverse kinematics, move move-target to target-coords look-at- target suppots 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.
- inverse_kinematics_args(union_link_list=None, rotation_axis=None, translation_axis=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_axis='z', translation_axis=False, rthre=0.001, **kwargs)[source]¶
Solve look at inverse kinematics
- Parameters:
look_at (list or np.ndarray or Coordinates) –
- inverse_kinematics_optimization(target_coords, move_target=None, link_list=None, regularization_parameter=None, init_angle_vector=None, translation_axis=True, rotation_axis=True, stop=100, dt=0.005, inverse_kinematics_hook=[], thre=0.001, rthre=0.017453292519943295, *args, **kwargs)[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:
- 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:
- 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:
- 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(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:
rotation_matrix (numpy.ndarray) – 3x3 rotation matrix.
wrt (str or skrobot.coordinates.Coordinates) – reference coordinates.
- rotate(theta, axis, wrt='local')[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) –
- 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:
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:
- 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:
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]))
- self_collision_check()[source]¶
Return collision link pair
- 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.
- 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:
- 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])
- 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
- default_urdf_path¶
- descendants¶
- dimension¶
Return dimension of this coordinate
- Returns:
len(self.translation) – dimension of this coordinate
- Return type:
- dual_quaternion¶
Property of DualQuaternion
Return DualQuaternion representation of this coordinate.
- Returns:
DualQuaternion – DualQuaternion representation of this coordinate
- Return type:
- 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¶
- lleg¶
- parent¶
- quaternion¶
Property of quaternion
- 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])
- rarm¶
- rleg¶
- 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]])
- 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])
- 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: