skrobot.interfaces._pybullet.PybulletRobotInterface

class skrobot.interfaces._pybullet.PybulletRobotInterface(robot, urdf_path=None, use_fixed_base=False, connect=1, *args, **kwargs)[source]

Pybullet Interface Class

Parameters:

robot (skrobot.model.RobotModel) – robot model
urdf_path (None or str) – urdf path. If this value is None, get urdf_path from robot.urdf_path.
use_fixed_base (bool) – If this value is True, robot in pybullet simulator will be fixed.
connect (int) – pybullet’s connection mode. If you have already connected to pybullet physics server, specify the server id. The default value is 1 (pybullet.GUI).

Examples

>>> from skrobot.models import PR2
>>> from skrobot.interfaces import PybulletRobotInterface
>>> robot_model = PR2()
>>> interface = PybulletRobotInterface(robot_model)

If you have already connected to pybullet physics server

>>> import pybullet
>>> client_id = pybullet.connect(pybullet.GUI)
>>> robot_model = PR2()
>>> interface = PybulletRobotInterface(robot_model, connect=client_id)

Methods

T()[source]

Return 4x4 homogeneous transformation matrix.

Returns:: matrix – homogeneous transformation matrix shape of (4, 4)
Return type:: numpy.ndarray

Examples

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

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(angle_vector=None, realtime_simulation=None)[source]

Send a angle vector to pybullet’s physics engine.

Parameters:

angle_vector (None or numpy.ndarray) – angle vector. If None, send self.robot.angle_vector()
realtime_simulation (None or bool) – If this value is True, send angle_vector by pybullet.setJointMotorControl2.

Returns:

angle_vector – return sent angle vector.

Return type:

numpy.ndarray

static available()[source]

Check Pybullet is available.

Returns:: _available – If False, pybullet is not available.
Return type:: bool

axis(ax)[source]

changed()[source]

Return False

This is used for CascadedCoords compatibility

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

coords()[source]: Return a deep copy of the Coordinates.

copy()[source]: Return a deep copy of the Coordinates.

copy_coords()[source]: Return a deep copy of the Coordinates.

copy_worldcoords()[source]: Return a deep copy of the Coordinates.

difference_position(coords, 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]

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:

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_rotate_vector(v)[source]

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

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

load_bullet()[source]

Load bullet configurations.

This function internally called.

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

newcoords(c, pos=None)[source]: Update of position and orientation.

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

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

Rotate this robot by given theta and axis.

For more detail, please see docs of skrobot.coordinates.Coordinates.rotate. The difference between the rotate, this function internally call pybullet.resetBasePositionAndOrientation.

Parameters:

theta (float) – radian
wrt (string or skrobot.coordinates.Coordinates)

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

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

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

sync()[source]: Synchronize pybullet pose to robot_model.

transform(c, wrt='local')[source]

Transform this coordinate by coords based on wrt

For more detail, please see docs of skrobot.coordinates.Coordinates.transform. The difference between the transform, this function internally call pybullet.resetBasePositionAndOrientation.

Parameters:

c (skrobot.coordinates.Coordinates) – coordinate
wrt (string 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.

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 robot in simulator.

For more detail, please see docs of skrobot.coordinates.Coordinates.translate. The difference between the translate, this function internally call pybullet.resetBasePositionAndOrientation.

Parameters:

vec (list or np.ndarray) – shape of (3,) translation vector. unit is [m] order.
wrt (string or Coordinates (optional)) – translate with respect to wrt.

wait_interpolation(thresh=0.05, timeout=60.0)[source]

Wait robot movement.

This function usually called after self.angle_vector. Wait while the robot joints are moving or until time of timeout. This function called internally pybullet.stepSimulation().

Parameters:

thresh (float) – velocity threshold for detecting movement stop.
timeout (float) – maximum time of timeout.

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]: Return thisself

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

dimension

Return dimension of this coordinate

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

name

Return this coordinate’s name

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

pose

Getter of Pose in pybullet physics simulator.

Wrapper of pybullet.getBasePositionAndOrientation.

Returns:: pose – pose of this robot in the physics simulator.
Return type:: skrobot.coordinates.Coordinates

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

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