RoboticsToolBox

class RoboticsToolBox.Bestman_sim_ur5e_vacuum_long(client, visualizer, cfg)

Bases: Bestman_sim

A class representing a simulation for the Bestman robot equipped with a UR5e arm and a vacuum gripper.

__init__(client, visualizer, cfg)

Initialize the Bestman_sim_ur5e_vacuum_long with the given parameters.

Parameters:

• client (int) – The PyBullet client ID.

• visualizer (bool) – Flag indicating whether visualization is enabled.

• cfg (dict) – Configuration settings.

sim_open_vacuum_gripper(object)

Activate or deactivate the gripper for a movable object.

Parameters:

• object (str) – The name or ID of the object related to gripper action.

• link_id (int) – The ID of the link on the object to be grasped.

• value (int) – 0 or 1, where 0 means deactivate (ungrasp) and 1 means activate (grasp).

sim_close_vacuum_gripper()

remove fixed constraint

sim_create_movable_constraint(object, link_id)

create constraint between end effector and joint

Parameters:

• object (str) – The name or ID of the object related to gripper action.

• link_id (int) – The ID of the link on the object to create constraint.

sim_remove_movable_constraint()

remove constraint between end effector and joint

pick(object)

Pick up the specified object.

Parameters:

object (str) – The name or ID of the object to be picked up.

place(place_pose)

Place an object at the specified place pose.

Parameters:

place_pose (Pose) – The pose to place object

pick_place(object, place_pose)

Perform a pick and place operation.

Parameters:

• object (str) – The name or ID of the object to be picked up.

• place_pose (Pose) – The pose to place object.

class RoboticsToolBox.Bestman_sim_panda(client, visualizer, cfg)

Bases: Bestman_sim

A class representing a simulation for the Bestman robot equipped with a Panda arm.

__init__(client, visualizer, cfg)

Initialize the Bestman_sim_panda with the given parameters.

Parameters:

• client (int) – The PyBullet client ID.

• visualizer (bool) – Flag indicating whether visualization is enabled.

• cfg (dict) – Configuration settings.

sim_open_gripper()

open gripper

sim_close_gripper()

close gripper

sim_move_gripper(open_width)

move gripper to special width

Parameters:

open_width (float) – gripper open width

sim_create_gripper_constraint(object, link_id)

Activate or deactivate the gripper for a movable object.

Parameters:

• object (str) – The name or ID of the object related to gripper action.

• link_id (int) – The ID of the link on the object to be grasped.

• value (int) – 0 or 1, where 0 means deactivate (ungrasp) and 1 means activate (grasp).

sim_remove_gripper_constraint()

remove constraint

pick(pick_pose)

Pick up the specified object without collision.

Parameters:

pick_pose (Pose) – pick pose

place(place_pose)

Place an object at the specified goal pose without collision.

Parameters:

place_pose (Pose) – The pose where the object will be placed.

pick_place(object, goal_pose)

Perform a pick and place operation.

Parameters:

• object (str) – The name or ID of the object to be picked up.

• goal_pose (Pose) – The pose where the object will be placed.

align_grasp_pose_to_tcp(z_init, target_pose)

Computes the homogeneous transformation matrix that aligns the z-axis of the target pose to the z-axis of the initial pose.

Parameters:

• R_init (np.ndarray) – 4x4 homogeneous matrix representing the initial rotation and translation of the end of the robot.

• R_target (np.ndarray) – 4x4 homogeneous matrix representing the rotation and translation of the target grasp.

Returns:

4x4 homogeneous transformation matrix that aligns the z-axis of the target to the z-axis of the initial pose.

Return type:

np.ndarray

class RoboticsToolBox.Bestman_sim(client, visualizer, cfg)

A basic class for BestMan robot

client

The pybullet client object.

Type:

object

client_id

The client id returned by the pybullet client.

Type:

int

target_distance

The target distance for the PID controller.

Type:

float

controller

The PID controller object.

Type:

object

rotated

A flag indicating whether the object has been rotated.

Type:

bool

init_pos

The initial position object.

Type:

object

base_id

The base id of the URDF model.

Type:

int

arm_id

The arm id of the URDF model.

Type:

int

arm_joints_idx

A list of joint indexes.

Type:

list

arm_height

The height of the arm.

Type:

float

end_effector_index

The index of the end effector.

Type:

int

tcp_link

The tcp link index.

Type:

int

tcp_height

The height of the tcp link.

Type:

float

visualizer

The visualizer object.

Type:

object

gripper_id

The gripper id. Initialized to None.

Type:

None

__init__(client, visualizer, cfg)

Initialize a new robot.

Parameters:

• client (object) – The pybullet client object.

• visualizer (object) – The visualizer object.

• cfg (object) – Configuration object containing parameters for the PID controller and robot setup.

sim_get_base_id()

Retrieves the ID of the robot base.

Returns:

The ID of the robot base.

Return type:

int

sim_get_current_base_pose()

Retrieves the current position and orientation of the robot base.

Returns:

A Pose object representing the current pose of the base.

The pose contains the position as a list [x, y, z] and the orientation as a list [roll, pitch, yaw].

Return type:

Pose

sim_stop_base()

Stops the movement of the robot base by setting its velocity to zero.

Returns:

True if the base has been successfully stopped.

Return type:

bool

sim_rotate_base(target_yaw, gradual=True, step_size=0.02, delay_time=0.05)

Rotate base to a specified yaw angle. Can be done gradually or at once.

Parameters:

• target_yaw (float) – The target yaw angle (in radians) for the base.

• gradual (bool) – If True, the rotation is done gradually. Otherwise, it’s instant.

• step_size (float, optional) – Angle increment for each step in radians. Only used if gradual=True.

• delay_time (float, optional) – Delay in seconds after each step. Only used if gradual=True.

sim_action(output)

Ajust base position using PID controller’s output

Parameters:

output (float) – The output of the PID controller, which is used to calculate the new position of the robot’s base.

sim_move_base_to_waypoint(waypoint, threshold=0.01)

Move base to waypoint The robot first rotates towards the target, and then moves towards it in a straight line. The movement is controlled by a controller (assumed to be a PID controller) that adjusts the velocity of the robot based on the distance to the target.

Parameters:

waypoint (Pose) – The target pose (position and orientation) for the robot. This should be an instance of a Pose class, which is assumed to have ‘x’ and ‘y’ properties.

sim_navigate_base(goal_base_pose, path, threshold=0.05, enable_plot=False)

Navigate a robot from its current position to a specified goal position

Parameters:

goal_base_pose (Pose) – The target pose (position and orientation) for the robot.

sim_calculate_nav_error(goal_pose)

Calculate the navigation error for performing pick-and-place manipulation of an object using a robot arm.

Parameters:

• goal_position – The desired goal position for the target object.

• goal_orientation – The desired goal orientation for the target object.

sim_get_arm_id()

Retrieves the ID of the robot arm.

Returns:

The ID of the robot arm.

Return type:

int

sim_get_DOF()

Retrieves the degree of freedom (DOF) of the robot arm.

Returns:

The degree of freedom of the robot arm.

Return type:

int

sim_get_arm_joint_idx()

Retrieves the indices of the joints in the robot arm.

Returns:

A list of indices for the joints in the robot arm.

Return type:

list

sim_get_arm_all_joint_idx()

Retrieves the indices of the joints in the robot arm.

Returns:

A list of indices for the joints in the robot arm.

Return type:

list

sim_get_tcp_link()

Retrieves the TCP (Tool Center Point) link of the robot arm.

Returns:

The TCP link of the robot arm.

Return type:

str

sim_get_tcp_link_height()

Retrieves the TCP (Tool Center Point) link height of the robot arm.

Returns:

The TCP link of the robot arm.

Return type:

str

sim_get_end_effector_link()

Retrieves the end effector link id

Returns:

The end effector link id of the robot arm.

Return type:

str

sim_get_arm_all_jointInfo()

get all arm joints info

Returns:

joint info list of all arm active joint

Return type:

list

sim_get_joint_bounds()

Retrieves the joint bounds of the robot arm.

By default, this function reads the joint bounds from pybullet.

Returns:

A list of tuples representing the joint bounds, where each tuple contains the minimum and maximum values for a joint.

Return type:

list

sim_get_current_joint_values()

Retrieve arm’s joint angle

sim_get_current_end_effector_pose()

Retrieve arm’s end effect information

sim_set_arm_to_joint_values(joint_values)

Set arm to move to a specific set of joint angles, witout considering physics

Parameters:

joint_values – A list of desired joint angles (in radians) for each joint of the arm.

sim_debug_set_arm_to_joint_values()

Manually set each joint value of the arm for debugging purposes.

sim_move_arm_to_joint_values(joint_values, threshold=0.015, timeout=0.05)

Move arm to move to a specific set of joint angles, with considering physics

Parameters:

joint_values – A list of desired joint angles (in radians) for each joint of the arm.

sim_joints_to_cartesian(joint_values)

Transforms the robot arm’s joint angles to its Cartesian coordinates.

Parameters:

joint_values (list) – A list of joint angles for the robot arm.

Returns:

A tuple containing the Cartesian coordinates (position and orientation) of the robot arm.

Return type:

tuple

sim_cartesian_to_joints(pose, max_iterations=1000, threshold=0.0001)

Transforms the robot arm’s Cartesian coordinates to its joint angles.

Parameters:

• position (list) – The Cartesian position of the robot arm.

• orientation (list) – The Cartesian orientation of the robot arm.

Returns:

A list of joint angles corresponding to the given Cartesian coordinates.

Return type:

list

sim_rotate_end_effector(angle)

Rotate the end effector of the robot arm by a specified angle.

Parameters:

angle (float) – The desired rotation angle in radians.

sim_move_end_effector_to_goal_pose(end_effector_goal_pose, steps=10, threshold=0.1)

Move arm’s end effector to a target position.

Parameters:

end_effector_goal_pose (Pose) – The desired pose of the end effector (includes both position and orientation).

sim_execute_trajectory(trajectory, threshold=0.1, enable_plot=False)

Execute the path planned by Planner

Parameters:

trajectory – List, each element is a list of angles, corresponding to a transformation

sim_calculate_IK_error(goal_pose)

Calculate the inverse kinematics (IK) error for performing pick-and-place manipulation of an object using a robot arm.

Parameters:

• goal_position – The desired goal position for the target object.

• goal_orientation – The desired goal orientation for the target object.

sim_get_robot_size()

Retrieves the maximum size of the robot (arm and base) in meters.

Returns:

The maximum size of the robot in meters.

Return type:

float

sim_sync_base_arm_pose()

Synchronizes the pose of the robot arm with the base.

This function ensures that the positions of the robot arm and base are aligned.

sim_update_camera()

sim_get_camera_pose()

sim_get_camera_rgb_image(enable_show=False, enable_save=False, filename=None)

sim_get_camera_depth_image(enable_show=False, enable_save=False, filename=None)

sim_get_camera_3d_points(enable_show=False)

sim_visualize_camera_3d_points()

sim_trans_camera_to_world(pose)

class RoboticsToolBox.Pose(position, orientation)

A class representing a 3D pose consisting of position and orientation.

__init__(position, orientation)

Initialize a new Pose object.

Parameters:

• position (list / np.ndarray) – A list or array of three floats representing the position in 3D space.

• orientation (list, tuple, np.ndarray) – A list, tuple, or array representing the orientation in 3D space. It can be either Euler angles (3 elements), a quaternion (4 elements), or a 3x3 rotation matrix.

get_position()

get position

Returns:

position

Return type:

list

get_orientation(type='quaternion')

get orientation

Returns:

orientation

Return type:

list

print(pose_description='', type='quaternion')

print pose