Bodies

This module provides a utility class for creating rigid bodies in PyBullet.

class pyb_utils.bodies.BulletBody(position, collision_uid, visual_uid, mass=1, orientation=None, client_id=0, **kwargs)[source]

Generic rigid body in PyBullet.

Parameters:

position (iterable) – The \((x, y, z)\) position of the body in the world.
collision_uid (int) – ID of the collision shape to use.
visual_uid (int) – ID of the visual shape to use.
mass (float) – mass of the body; defaults to 1.
orientation (iterable) – A quaternion \((x, y, z, w)\) representing the orientation of the body; if not provided, orientation is aligned with the world frame axes.
client_id (int) – physics client ID; only required if connected to multiple servers.

apply_wrench(force=None, torque=None, position=None, frame=1)[source]

Apply a wrench (i.e., force and torque) to the body about its inertial frame.

Parameters:

force (iterable) – Force to apply to the body. If not provided, no force is applied.
torque (iterable) – Torque to apply to the body. If not provided, no torque is applied.
position (iterable) – Position at which to apply the force. If not provided, force acts at the origin of the specified frame. Has no effect on the torque.
frame (int) – The coordinate frame (i.e., orientation). Can be either pyb.LINK_FRAME (the default) or pyb.WORLD_FRAME.

classmethod box(position, half_extents, color=(1, 0, 0, 1), client_id=0, **kwargs)[source]

Create a cuboid body.

Parameters:

position (iterable) – The (x, y, z) position of the box in the world.
half_extents (iterable) – The three half lengths of the box.
color (iterable) – The (r, g, b, α) color of the box.
client_id (int) – Physics client ID; only required if connected to multiple servers.

classmethod capsule(position, radius, height, color=(1, 0, 0, 1), client_id=0, **kwargs)[source]

Create a capsular body. A capsule is a cylinder with half-spheres on the ends.

The capsule is oriented along its z-axis, which corresponds to the height.

Parameters:

position (iterable) – The (x, y, z) position of the capsule in the world.
radius (float) – The radius of the capsule.
height (float) – The height of the capsule.
color (iterable) – The (r, g, b, α) color of the capsule.
client_id (int) – Physics client ID; only required if connected to multiple servers.

classmethod cylinder(position, radius, height, color=(1, 0, 0, 1), client_id=0, **kwargs)[source]

Create a cylindrical body.

The cylinder is oriented along its z-axis, which corresponds to the height.

Parameters:

position (iterable) – The (x, y, z) position of the cylinder in the world.
radius (float) – The radius of the cylinder.
height (float) – The height of the cylinder.
color (iterable) – The (r, g, b, α) color of the cylinder.
client_id (int) – Physics client ID; only required if connected to multiple servers.

get_pose(as_rotation_matrix=False)[source]

Get the position and orientation of the body’s inertial frame.

Parameters:: as_rotation_matrix (bool) – Set to True to return the orientation as a rotation matrix, False to return a quaternion.
Returns:: A tuple containing the position and orientation of the link’s center of mass in the world frame. If as_rotation_matrix=True, then the orientation is represented as a \(3\times3\) rotation matrix. If False, then it is represented as a quaternion in xyzw order.

get_velocity()[source]

Get the velocity of the body’s inertial frame.

Returns:: A tuple (linear, angular) containing the linear and angular velocity, each an array of length 3.

set_pose(position=None, orientation=None)[source]

Set the position and orientation of the body’s inertial frame.

Parameters:

position (iterable) – The \((x, y, z)\) position of the body in the world.
orientation (iterable) – A quaternion \((x, y, z, w)\) representing the orientation of the body.

set_velocity(linear=None, angular=None)[source]

Set the velocity of the body’s inertial frame.

Parameters:

linear (iterable) – Linear velocity of the body. If not provided, linear velocity is unchanged.
angular (iterable) – Angular velocity of the body. If not provided, angular velocity is unchanged.

classmethod sphere(position, radius, color=(1, 0, 0, 1), client_id=0, **kwargs)[source]

Create a spherical body.

Parameters:

position (iterable) – The (x, y, z) position of the sphere in the world.
radius (float) – The radius of the sphere.
color (iterable) – The (r, g, b, α) color of the sphere.
client_id (int) – Physics client ID; only required if connected to multiple servers.

Camera

This module provides a utility Camera class for PyBullet.

class pyb_utils.camera.Camera(view_matrix, near=0.1, far=1000.0, fov=60.0, width=1280, height=720)[source]

A PyBullet camera, which captures images of the simulation environment.

Parameters:

view_matrix (iterable) – The camera’s view matrix.
near (float) – Near value
far (float) – Far value
fov (float) – Field of view
width (int) – Width of the image in pixels
height (int) – Height of the image in pixels

classmethod from_camera_position(target_position, camera_position, near=0.1, far=1000.0, fov=60.0, width=1280, height=720)[source]

Construct a new camera from target and camera positions.

Parameters:

target_position (iterable) – The position of the camera’s target point.
camera_position (iterable) – The position of the camera in the world.
near (float) – Near value
far (float) – Far value
fov (float) – Field of view
width (int) – Width of the image in pixels
height (int) – Height of the image in pixels

classmethod from_distance_rpy(target_position, distance, roll=0, pitch=0, yaw=0, near=0.1, far=1000.0, fov=60.0, width=1280, height=720)[source]

Construct a new camera from target position, distance, and roll, pitch, yaw angles.

Parameters:

target_position (iterable) – The position of the camera’s target point.
distance (float) – The distance of camera from target.
roll (float) – Roll of the camera’s orientation.
pitch (float) – Pitch of the camera’s orientation.
yaw (float) – Yaw of the camera’s orientation.
near (float) – Near value
far (float) – Far value
fov (float) – Field of view
width (int) – Width of the image in pixels
height (int) – Height of the image in pixels

get_frame()[source]

Get a frame from the camera.

Returns:: A tuple (rgba, depth, seg), where rgba is the RGBA color data of shape (height, width, 4), depth is the depth buffer of shape (height, width), and seg is the segmentation mask of shape (height, width).

get_point_cloud(depth=None)[source]

Convert depth buffer to 3D point cloud in world coordinates.

See https://stackoverflow.com/a/62247245 for the main source of this code.

Parameters:

depth (np.ndarray) – Optionally, the user can provide the depth buffer from a previous call to Camera.get_frame(). If not provided, self.get_frame() is called to retrieve this data.

Returns:

An array of shape (height, width, 3) representing the points seen
by the camera.

linearize_depth(depth=None)[source]

Convert depth map to actual distance from camera plane.

See https://stackoverflow.com/a/6657284.

Parameters:: depth (np.ndarray) – Optionally, the user can provide the depth buffer from a previous call to Camera.get_frame(). If not provided, self.get_frame() is called to retrieve this data.
Returns:: Linearized depth buffer, consisting of the actual depth values from the camera plane.

save_frame(filename, rgba=None)[source]

Save a frame to a file.

Parameters:

filename (str) – The name of the image file.
rgba (np.ndarray) – Optionally, the user can provide the RGBA data from a previous call to Camera.get_frame(). If not provided, self.get_frame() is called to retrieve this data.

set_camera_pose(position, target)[source]

Change the position and target of the camera.

Parameters:

position (iterable) – The position of the camera in the world.
target (iterable) – The position of the camera’s target point.

class pyb_utils.camera.FrameRecorder(camera, fps=25, dirname=None)[source]

Recorder for a set of frames from a PyBullet simulation.

This is an alternative to the VideoRecorder, which directly saves a video.

Parameters:

camera (Camera) – Camera object to use for rendering frames.
fps (int) – Frames per second. Each fps frames will be played over one second of video.
dirname (str) – The name of the directory to write the frames to. The directory will be created if it doesn’t already exist.

capture_frame(t, rgba=None)[source]

Capture and save a frame if enough time has past since the last capture.

This does nothing if less than 1. / fps time has elapsed since the last frame was captured.

Parameters:

t (float) – The current time.
rgba – Image data as returned by Camera.get_frame(). Otherwise, self.camera.get_frame is called to retrieve this data.

class pyb_utils.camera.VideoRecorder(filename, camera, fps, codec='mp4v')[source]

Recorder for a video of a PyBullet simulation.

Parameters:

filename (str) – The name of the file to write the video to.
camera (Camera) – Camera object to use for rendering frames.
fps (int) – Frames per second. Each fps frames will be played over one second of video.

capture_frame(rgba=None)[source]

Capture a frame and write it to the video.

Parameters:: rgba (np.ndarray) – Optionally, the user can provide the RGBA data from a previous call to Camera.get_frame(). This can be used to avoid multiple renderings with the camera. If not provided, self.camera.get_frame() is called to retrieve this data.

Contact

pyb_utils.contact.get_point_contact_wrench(point, origin=None)[source]

Compute the contact wrench at a contact point.

The wrench is the force and torque acting on body A due to contact with body B at contact point point. The wrench is aligned with the world frame.

Parameters:

point (ContactPoint) – The contact point, as returned by pyb_utils.named_tuples.getContactPoints().
origin – The origin point about which to compute the torque. If None (the default), then the torque is computed about the world frame origin.

Returns:

A tuple (force, torque) representing the contact wrench on body A.

pyb_utils.contact.get_points_contact_wrench(points, origin=None)[source]

Compute the contact wrench resulting from a set of contact points.

The wrench is the force and torque acting on body A due to contact with body B at contact points points. The wrench is aligned with the world frame.

Parameters:

points – List of contact points, as returned by pyb_utils.named_tuples.getContactPoints().
origin – The origin point about which to compute the torque. If None (the default), then the torque is computed about the world frame origin.

Returns:

A tuple (force, torque) representing the contact wrench.

pyb_utils.contact.get_total_contact_wrench(bodyA, bodyB, linkIndexA=-2, linkIndexB=-2, origin=None, max_contacts=None, physicsClientId=0)[source]

Compute the contact wrench resulting from a set of contact points.

The wrench is the force and torque acting on body A due to contact with body B. The wrench is aligned with the world frame.

If pyb_utils.named_tuples.getContactPoints() has already been called, you can use the get_points_contact_wrench() function instead.

Parameters:

bodyA (int) – The UID of the first body.
bodyB (int) – The UID of the second body.
linkIndexA (int) – The index of the link on the first body (optional). If not provided, checks all links on the body.
linkIndexB (int) – The index of the link on the second body (optional). If not provided, checks all links on the body.
origin – The origin point about which to compute the torque. If None (the default), then the torque is computed about the world frame origin.
max_contacts – The maximum number of contact points expected (optional).
physicsClientId (int) – The UID of the physics server to use.

Raises:

AssertionError – If max_contacts is not None and the number of contact points exceeds max_contacts.

Returns:

A tuple (force, torque) representing the contact wrench.

Collision

This module provides utilities for collision checking between objects.

class pyb_utils.collision.CollisionDetector(client_id, collision_pairs)[source]

Detector of collisions between bodies.

Parameters:

client_id (int) – Index of the physics server used for collision queries.
collision_pairs (list) – A list of collision pairs, where each element is a 2-tuple representing two collision bodies. Each collision body is represented as either a single int representing a body UID, a tuple (int, int) representing the body UID and link index, or a tuple (int, str) representing the body UID and the link name.

compute_distances(max_distance=10.0)[source]

Compute closest distances for a given configuration.

Parameters:: max_distance (float) – Bodies farther apart than this distance are not queried by PyBullet, the return value for the distance between such bodies will be max_distance.
Returns:: An array of distances, one per pair of collision objects. Each distance is at most max_distance, even if the actual distance is greater.

in_collision(margin=0)[source]

Check if a configuration is in collision.

Parameters:: margin (float) – Distance at which objects are considered in collision. Default is 0.
Returns:: True if configuration q is in collision, False otherwise.

Frame

This modules provides visible oriented frames for debugging the simulation.

pyb_utils.frame.debug_frame(size: float, obj_uid: int, link_index: int) → Tuple[int][source]

Attach a frame to a link for debugging purposes.

Parameters:

size (float) – The length of each arm of the frame.
obj_uid (int) – UID of the body to attach the frame to.
link_index (int) – Index of the link on the body to attach the frame to. Use -1 for the base link.

Returns:

pybullet ids of the lines

Return type:

tuple[int]

pyb_utils.frame.debug_frame_world(size: float, origin: Iterable[float], orientation: Iterable[float] = (0, 0, 0, 1), line_width: float = 1) → Tuple[int][source]

Attach a frame to the world for debugging purposes.

Parameters:

size (float) – The length of each arm of the frame.
origin (iterable) – The origin of the frame.
orientation (iterable) – A quaternion \((x, y, z, w)\) represented the frame’s orientation.
line_width (float) – Width of the lines that make up the frame.

Returns:

pybullet ids of the lines

Return type:

tuple[int]

Ghost

This module provides utilities for “ghost” (visual-only) objects.

class pyb_utils.ghost.GhostObject(visual_uid, position=None, orientation=None, parent_body_uid=None, parent_link_index=-1, client_id=0)[source]

A purely visual PyBullet object.

The GhostObject can be “attached” to another body and positioned relative to that body, or it can be positioned at an absolute location in the world frame.

Parameters:

visual_uid (int) – The UID of the visual PyBullet object to use.
position (iterable) – The object’s position (optional). Defaults to (0, 0, 0). If parent_body_uid is also specified, then the position is relative to that body, otherwise it is relative to the world.
orientation (iterable) – The object’s orientation, as a quaternion \((x, y, z, w)\) (optional). Defaults to (0, 0, 0, 1). If parent_body_uid is also specified, then the orientation is relative to that body, otherwise it is relative to the world.
parent_body_uid (int) – The UID of an existing body to “attach” this object to (optional).
parent_link_index (int) – The index of the link on the parent body to attach this object to (optional). Defaults to -1 (the base link).

classmethod box(position, half_extents, parent_body_uid=None, parent_link_index=-1, color=(1, 0, 0, 1), client_id=0)[source]

Create a cuboid ghost.

Parameters:

position (iterable) – The (x, y, z) position of the box in the world.
half_extents (iterable) – The three half lengths of the box.
parent_body_uid (int) – The UID of an existing body to “attach” this object to (optional).
parent_link_index (int) – The index of the link on the parent body to attach this object to (optional). Defaults to -1 (the base link).
color (iterable) – The (r, g, b, α) color of the box.

classmethod sphere(radius, position=None, parent_body_uid=None, parent_link_index=-1, color=(1, 0, 0, 1), client_id=0)[source]

Create a spherical ghost object.

Parameters:

radius (float) – The radius of the sphere.
position (iterable) – The object’s position (optional). Defaults to (0, 0, 0). If parent_body_uid is also specified, then the position is relative to that body, otherwise it is relative to the world.
parent_body_uid (int) – The UID of an existing body to “attach” this object to (optional).
parent_link_index (int) – The index of the link on the parent body to attach this object to (optional). Defaults to -1 (the base link).
color (iterable) – The (r, g, b, α) color of the sphere (optional). Defaults to red.

update(position=None, orientation=None)[source]

Update the pose of the object.

If the object has a parent, then this should be called every time the simulation rendering is updated. The object’s pose in the world is updated to reflect the parent’s new pose. If position or orientation is supplied, then the pose relative to the parent is updated.

Otherwise, this function can be used to change the object’s absolute pose in the world.

Parameters:

position (iterable) – The new position of the object (optional).
orientation (iterable) – The new orientation of the object, as a quaternion \((x, y, z, w)\) (optional).

Math

The module provides basic quaternion operations.

pyb_utils.math.matrix_to_quaternion(C)[source]

Convert a rotation matrix to a quaternion.

Parameters:: C – A \(3\times3\) rotation matrix.
Returns:: The quaternion \((x, y, z, w)\) representing the same rotation as C.

pyb_utils.math.quaternion_multiply(q0, q1, normalize=True)[source]

The Hamilton product of two quaternions.

Parameters:

q0 (iterable) – The first quaternion, in \((x, y, z, w)\) order.
q1 (iterable) – The second quaternion, in \((x, y, z, w)\) order.
normalize (bool) – Normalize the quaterions to ensure they are unit.

Returns:

The quaternion representing the Hamilton product \(q_0\otimes q_1\). This quaternion represents the compound rotation obtained by rotating by \(q_0\) and then \(q_1\).

pyb_utils.math.quaternion_rotate(q, r)[source]

Rotate a point by the rotation represented by a quaternion.

Parameters:

q (iterable) – The quaternion \((x, y, z, w)\) representing the rotation.
r (iterable) – The point to rotate.

Returns:

The rotated point.

pyb_utils.math.quaternion_to_matrix(q)[source]

Convert a quaternion to a rotation matrix.

Parameters:: q (iterable) – A quaternion \((x, y, z, w)\).
Returns:: The \(3\times3\) rotation matrix representing the same rotation matrix as q.

pyb_utils.math.quatx(angle)[source]

Quaternion representing a rotation of angle about the x-axis.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The quaternion \((x, y, z, w)\) representing the rotation.

pyb_utils.math.quaty(angle)[source]

Quaternion representing a rotation of angle about the y-axis.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The quaternion \((x, y, z, w)\) representing the rotation.

pyb_utils.math.quatz(angle)[source]

Quaternion representing a rotation of angle about the z-axis.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The quaternion \((x, y, z, w)\) representing the rotation.

pyb_utils.math.rot2(angle)[source]

2D rotation matrix representing angle.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The \(2\times 2\) rotation matrix \(C\in SO(2)\) representing the rotation.

pyb_utils.math.rotx(angle)[source]

Matrix representing a rotation of angle about the x-axis.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The \(3\times 3\) matrix \(C\in SO(3)\) representing the rotation.

pyb_utils.math.roty(angle)[source]

Matrix representing a rotation of angle about the y-axis.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The \(3\times 3\) matrix \(C\in SO(3)\) representing the rotation.

pyb_utils.math.rotz(angle)[source]

Matrix representing a rotation of angle about the z-axis.

Parameters:: angle (float) – The angle of rotation in radians.
Returns:: The \(3\times 3\) matrix \(C\in SO(3)\) representing the rotation.

Named Tuples

Wrappers for PyBullet methods that return large tuples.

This submodule returns the same information in a named tuple, so the fields can be easily identified.

pyb_utils.named_tuples.getClosestPoints(bodyA, bodyB, distance, linkIndexA=-2, linkIndexB=-2, physicsClientId=0)[source]

Get the closest points between two bodies or specific links.

Parameters:

bodyA (int) – The UID of the first body.
bodyB (int) – The UID of the second body.
linkIndexA (int) – The index of the link on the first body (optional). If not provided, checks all links on the body.
linkIndexB (int) – The index of the link on the second body (optional). If not provided, checks all links on the body.
physicsClientId (int) – The UID of the physics server to use.

Returns:

A list of ContactPoint representing the closest points. Normal force is always zero.

pyb_utils.named_tuples.getConstraintInfo(constraintUniqueId, physicsClientId=0)[source]

Get information about a constraint.

Parameters:

constraintUniqueId (int) – UID of the constraint
physicsClientId (int) – UID of the physics server to use.

Return type:

ConstraintInfo

pyb_utils.named_tuples.getContactPoints(bodyA=-1, bodyB=-1, linkIndexA=-2, linkIndexB=-2, physicsClientId=0)[source]

Get the contact points between two bodies or specific links.

Parameters:

bodyA (int) – The UID of the first body.
bodyB (int) – The UID of the second body.
linkIndexA (int) – The index of the link on the first body (optional). If not provided, checks all links on the body.
linkIndexB (int) – The index of the link on the second body (optional). If not provided, checks all links on the body.
physicsClientId (int) – The UID of the physics server to use.

Returns:

A list of ContactPoint.

pyb_utils.named_tuples.getDynamicsInfo(bodyUniqueId, linkIndex, physicsClientId=0)[source]

Get information about the dynamic properties of a link.

Parameters:

bodyUniqueId (int) – The UID of the body.
linkIndex (int) – The index of the link on the body.
physicsClientId (int) – The UID of the physics server to use.

Return type:

DynamicsInfo

pyb_utils.named_tuples.getJointInfo(bodyUniqueId, jointIndex, physicsClientId=0, decode=None)[source]

Get information about a joint.

The one difference from the PyBullet API is the addition of the optional decode argument.

If decode is not None, then it is used to decode the strings of bytes returned by the PyBullet API for the jointName and linkName fields.

Parameters:

bodyUniqueId (int) – UID of the body with the joint.
jointIndex (int) – Index of the joint on the body.
physicsClientId (int) – The UID of the physics server to use.
decode (str) – The encoding to use to convert the bytes fields to strings.

Return type:

JointInfo

pyb_utils.named_tuples.getJointState(bodyUniqueId, jointIndex, physicsClientId=0)[source]

Get the state of a joint.

Parameters:

bodyUniqueId (int) – UID of the body with the joint.
jointIndex (int) – Index of the joint on the body.
physicsClientId (int) – The UID of the physics server to use.

Return type:

JointState

pyb_utils.named_tuples.getJointStates(bodyUniqueId, jointIndices, physicsClientId=0)[source]

Get the state of multiple joints at once.

Parameters:

bodyUniqueId (int) – UID of the body with the joint.
jointIndices – List of indices of joints on the body.
physicsClientId (int) – The UID of the physics server to use.

Returns:

A list of JointState.

pyb_utils.named_tuples.getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity=False, computeForwardKinematics=False, physicsClientId=0)[source]

Get the state of a link.

One difference from the PyBullet API is that if computeLinkVelocity=False, we still return the worldLinkLinearVelocity and worldLinkAngularVelocity fields, but they are set to None. PyBullet just doesn’t return the fields, such that the tuple has 6 fields rather than 8.

Parameters:

bodyUniqueId (int) – UID of the body with the link.
linkIndex (int) – Index of the link on the body. Note that the link index corresponds to the index of its parent joint.
computeLinkVelocity (bool) – If True, compute the velocity of the link’s CoM in the world frame. Otherwise, these fields are set to None.
computeForwardKinematics (bool) – If True, update the position and orientation of the link in the world frame. Otherwise, the position and orientation will not be up to date if the simulation has been stepped.
physicsClientId (int) – The UID of the physics server to use.

Return type:

LinkState

pyb_utils.named_tuples.getLinkStates(bodyUniqueId, linkIndices, computeLinkVelocity=False, computeForwardKinematics=False, physicsClientId=0)[source]

Get the state of multiple links at once.

One difference from the PyBullet API is that if computeLinkVelocity=False, we still return the worldLinkLinearVelocity and worldLinkAngularVelocity fields, but they are set to None. PyBullet just doesn’t return the fields, such that the tuple has 6 fields rather than 8.

Parameters:

bodyUniqueId (int) – UID of the body with the link.
linkIndices – List of indices of the link on the body. Note that the link index corresponds to the index of its parent joint.
computeLinkVelocity (bool) – If True, compute the velocity of the link’s CoM in the world frame. Otherwise, these fields are set to None.
computeForwardKinematics (bool) – If True, update the position and orientation of the link in the world frame. Otherwise, the position and orientation will not be up to date if the simulation has been stepped.
physicsClientId (int) – The UID of the physics server to use.

Returns:

List of LinkState.

class pyb_utils.named_tuples.DynamicsInfo(mass, lateralFriction, localInertiaDiagonal, localInerialPos, localInertialOrn, restitution, rollingFriction, spinningFriction, contactDamping, contactStiffness, bodyType, collisionMargin)

class pyb_utils.named_tuples.ContactPoint(contactFlag, bodyUniqueIdA, bodyUniqueIdB, linkIndexA, linkIndexB, positionOnA, positionOnB, contactNormalOnB, contactDistance, normalForce, lateralFriction1, lateralFrictionDir1, lateralFriction2, lateralFrictionDir2)

class pyb_utils.named_tuples.ConstraintInfo(parentBodyUniqueId, parentJointIndex, childBodyUniqueId, childLinkIndex, constraintType, jointAxis, jointPivotInParent, jointPivotInChild, jointFrameOrientationParent, jointFrameOrientationChild, maxAppliedForce, gearRatio, gearAuxLink, relativePositionTarget, erp)

class pyb_utils.named_tuples.JointInfo(jointIndex, jointName, jointType, qIndex, uIndex, flags, jointDamping, jointFriction, jointLowerLimit, jointUpperLimit, jointMaxForce, jointMaxVelocity, linkName, jointAxis, parentFramePos, parentFrameOrn, parentIndex)

class pyb_utils.named_tuples.JointState(jointPosition, jointVelocity, jointReactionForces, appliedJointMotorTorque)

class pyb_utils.named_tuples.LinkState(linkWorldPosition, linkWorldOrientation, localInertialFramePosition, localInertialFrameOrientation, worldLinkFramePosition, worldLinkFrameOrientation, worldLinkLinearVelocity, worldLinkAngularVelocity)

Robots

This module provides a simple wrapper for a velocity- or torque-controlled robot.

class pyb_utils.robots.Robot(uid, tool_link_name=None, actuated_joint_names=None, client_id=0)[source]

Wrapper for a PyBullet robot.

uid

The unique ID of the underlying PyBullet robot body.

Type:: int

client_id

The unique ID of the physics server this robot belongs to.

Type:: int

tool_idx

The index of the tool joint/link (in PyBullet each link has the same index as its parent joint).

Type:: int

Parameters:

uid (int) – The UID of the body representing the robot.
tool_link_name (str) – If provided, use this link as the end effector. If it is None, then the last link is used. The index of this link is stored in self.tool_idx.
actuated_joint_names (iterable) – An optional list of actuated joint names. These joints will be “actuated”, in that they take commands from the command_ methods. If not provided, all moveable joints are considered actuated. To have a free-moving joint (whether actuated or not), see set_joint_friction_forces().
client_id (int) – The physics server UID to connect to, if multiple servers are being used.

Raises:

ValueError – If tool_link_name is provided but the robot has no link with that name.

command_effort(u)[source]

Send a joint effort (force/torque) command to the robot.

Parameters:: u (iterable) – The joint effort to command. Must be of length self.num_actuated_joints.

command_velocity(u)[source]

Send a joint velocity command to the robot.

Parameters:: u (iterable) – The joint velocity to command. Must be of length self.num_actuated_joints.

compute_link_com_jacobian(q=None, link_idx=None)[source]

Compute the Jacobian around the link’s center of mass.

Parameters:

q – The joint configuration at which to compute the Jacobian. If no configuration is given, then the current one is used.
link_idx – The index of the link to compute the Jacobian for. The end effector link self.tool_idx is used if not given.

Returns:

The \(6\times n\) Jacobian matrix, where \(n\) is the number of joints.

compute_link_frame_jacobian(q=None, link_idx=None)[source]

Compute the Jacobian around the URDF link frame.

This is the same as calling compute_link_jacobian with offset=None.

Parameters:

q – The joint configuration at which to compute the Jacobian. If no configuration is given, then the current one is used.
link_idx – The index of the link to compute the Jacobian for. The end effector link self.tool_idx is used if not given.

Returns:

The \(6\times n\) Jacobian matrix, where \(n\) is the number of joints.

compute_link_jacobian(q=None, link_idx=None, offset=None)[source]

Get the Jacobian of a point on a link at the given configuration.

When offset is None or zeros, the Jacobian is computed around the URDF link frame (i.e., the parent joint position).

Parameters:

q – The joint configuration at which to compute the Jacobian. If no configuration is given, then the current one is used.
link_idx – The index of the link to compute the Jacobian for. The end effector link self.tool_idx is used if not given.
offset – Offset from the parent joint position at which to compute the Jacobian. Defaults to zero (no offset).

Returns:

The \(6\times n\) Jacobian matrix, where \(n\) is the number of joints.

get_joint_index(name)[source]

Get the index of the joint named name.

Parameters:: name (str) – The name of the joint.
Returns:: The index of the joint.
Raises:: ValueError – If the robot has no joint named name.

get_joint_states()[source]

Get position and velocity of the robot’s moveable joints.

Returns:: A tuple (q, v) where q is the robot’s joint configuration and v is the joint velocity.

get_link_com_pose(link_idx=None, as_rotation_matrix=False)[source]

Get the pose of a link’s center of mass.

The pose is computed about the link’s center of mass with respect to the world frame and expressed in the world frame.

Let \(\mathcal{F}_w\) be the world frame, \(\mathcal{F}_f\) be the link’s URDF frame, and let \(\mathcal{F}_c\) be the link’s CoM frame. This function returns the position \(\boldsymbol{r}^{cw}_w\) and orientation quaternion \(\boldsymbol{Q}_{wc}\). The relationship between \((\boldsymbol{r}^{cw}_w,\boldsymbol{Q}_{wc})\) from this function and \((\boldsymbol{r}^{fw}_w,\boldsymbol{Q}_{wf})\) from get_link_frame_pose() is

\[\begin{split}\boldsymbol{r}^{cw}_w &= \boldsymbol{r}^{fw}_w + \boldsymbol{C}_{wf}\boldsymbol{r}^{cf}_f \\ \boldsymbol{Q}_{wc} &= \boldsymbol{Q}_{wf} \otimes \boldsymbol{Q}_{fc},\end{split}\]

where \(\boldsymbol{C}_{wf}\) is the rotation matrix representing the same rotation as \(\boldsymbol{Q}_{wf}\), and \(\boldsymbol{r}^{cf}_f\) and \(\boldsymbol{Q}_{fc}\) are the position and quaternion from LinkState.localInertialFramePosition and LinkState.localInertialFrameOrientation, respectively, obtained from a call to pyb_utils.named_tuples.getLinkState(). The symbol \(\otimes\) refers to Hamilton/quaternion multiplication.

Parameters:

link_idx – Index of the link to use. If not provided, defaults to the end effector self.tool_idx.
as_rotation_matrix (bool) – Set to True to return the orientation as a rotation matrix, False to return a quaternion.

Returns:

A tuple containing the position and orientation of the link’s center of mass in the world frame. If as_rotation_matrix=True, then the orientation is represented as a \(3\times3\) rotation matrix. If False, then it is represented as a quaternion in xyzw order.

get_link_com_velocity(link_idx=None)[source]

Get the velocity of a link’s center of mass with respect to the world.

With reference to the documentation of get_link_com_pose(), the relationship between the CoM velocity \((\boldsymbol{v}^{cw}_w,\boldsymbol{\omega}^{cw}_w)\) from this function and \((\boldsymbol{v}^{fw}_w,\boldsymbol{\omega}^{fw}_w)\) from get_link_frame_velocity() is

\[\begin{split}\boldsymbol{v}^{cw}_w &= \boldsymbol{v}^{fw}_w + \boldsymbol{\omega}^{fw}_w\times\boldsymbol{C}_{wf}\boldsymbol{r}^{cf}_f \\ \boldsymbol{\omega}^{cw}_w &= \boldsymbol{\omega}^{fw}_w,\end{split}\]

where \(\times\) denotes the cross product.

Parameters:: link_idx – Index of the link to use. If not provided, defaults to the end effector self.tool_idx.
Returns:: A tuple containing the linear and angular velocity vectors for the link’s center of mass in the world frame.

get_link_frame_pose(link_idx=None, as_rotation_matrix=False)[source]

Get the pose of a link’s URDF frame.

The pose is computed about the link’s parent joint position, which is its URDF frame.

Parameters:

link_idx – Index of the link to use. If not provided, defaults to the end effector self.tool_idx.
as_rotation_matrix (bool) – Set to True to return the orientation as a rotation matrix, False to return a quaternion.

Returns:

A tuple containing the position and orientation of the link’s frame with respect to the world. If as_rotation_matrix=True, then the orientation is represented as a \(3\times3\) rotation matrix. If False, then it is represented as a quaternion in xyzw order.

get_link_frame_velocity(link_idx=None)[source]

Get the velocity of a link’s URDF frame with respect to the world.

Parameters:: link_idx – Index of the link to use. If not provided, defaults to the end effector self.tool_idx.
Returns:: A tuple containing the linear and angular velocity vectors for the link’s URDF frame with respect to the world.

get_link_index(name)[source]

Get the index of the link named name.

Parameters:: name (str) – The name of the link.
Returns:: The index of the link.
Raises:: ValueError – If the robot has no link named name.

reset_joint_configuration(q)[source]

Reset the robot to a particular configuration.

It is best not to do this during a simulation, as it overrides all dynamic effects.

Parameters:: q (iterable) – The vector of joint values to set. Must be of length self.num_moveable_joints.

set_joint_friction_forces(forces, joint_indices=None)[source]

Set the friction forces at each joint.

Set the force to a small value (or zero) to have free-moving joints.

Parameters:

forces (iterable) – The friction forces for the specified joints.
joint_indices (iterable) – An optional list of joint indices to specify forces for. If not provided, then it is assumed forces are being provided for all moveable joints.

property actuated_joint_names

A list of the actuated joint names. This is the same as the list of joint names unless actuated_joint_names is provided to the constructor.

Type:: list

property all_joint_names

A list of all joint names, including fixed and moveable joints.

Type:: list

property link_names

A list of all link names.

Type:: list

property moveable_joint_names

A list of the names of the moveable joints.

Type:: list

property num_actuated_joints

The number of actuated (i.e., controlled) joints. This should never be more than num_joints.

Type:: int

property num_moveable_joints

The number of moveable joints.

Type:: int

property num_total_joints

The total number of joints, including fixed and moveable joints.

Type:: int

Utils

pyb_utils.utils.get_urdf_path()[source]

Obtain the path to the extra URDFs packaged with pyb_utils.

This can be easily integrated into PyBullet using:

pybullet.setAdditionalSearchPath(pyb_utils.get_urdf_path())

Returns:: The path to the directory containing extra pyb_utils URDFs.
Return type:: str