Planner

class mplib.planner.Planner(urdf: str, move_group: str, srdf: str = '', package_keyword_replacement: str = '', user_link_names: Sequence[str] = [], user_joint_names: Sequence[str] = [], joint_vel_limits: Sequence[float] | ndarray | None = None, joint_acc_limits: Sequence[float] | ndarray | None = None, **kwargs)

Bases: object

Motion planner

IK(goal_pose, start_qpos, mask=None, n_init_qpos=20, threshold=0.001)

Inverse kinematics

Parameters:

• goal_pose – [x,y,z,qw,qx,qy,qz] pose of the goal

• start_qpos – initial configuration

• mask – if the value at a given index is True, the joint is not used in the IK

• n_init_qpos – number of random initial configurations

• threshold – threshold for the distance between the goal pose and the result pose

TOPP(path, step=0.1, verbose=False)

Time-Optimal Path Parameterization

Parameters:

• path – numpy array of shape (n, dof)

• step – step size for the discretization

• verbose – if True, will print the log of TOPPRA

check_for_collision(collision_function, articulation: ArticulatedModel | None = None, qpos: ndarray | None = None) → list

helper function to check for collision

check_for_env_collision(articulation: ArticulatedModel | None = None, qpos: ndarray | None = None, with_point_cloud=False, use_attach=False) → list

Check if the robot is in collision with the environment

Parameters:

• articulation – robot model. if none will be self.robot

• qpos – robot configuration. if none will be the current pose

• with_point_cloud – whether to check collision against point cloud

• use_attach – whether to include the object attached to the end effector in collision checking

Returns:

A list of collisions.

check_for_self_collision(articulation: ArticulatedModel | None = None, qpos: ndarray | None = None) → list

Check if the robot is in self-collision.

Parameters:

• articulation – robot model. if none will be self.robot

• qpos – robot configuration. if none will be the current pose

Returns:

A list of collisions.

distance_6D(p1, q1, p2, q2)

compute the distance between two poses

Parameters:

• p1 – position of pose 1

• q1 – quaternion of pose 1

• p2 – position of pose 2

• q2 – quaternion of pose 2

generate_collision_pair(sample_time=1000000, echo_freq=100000)

We read the srdf file to get the link pairs that should not collide. If not provided, we need to randomly sample configurations to find the link pairs that will always collide.

pad_qpos(qpos, articulation=None)

if the user does not provide the full qpos but only the move_group joints, pad the qpos with the rest of the joints

plan_qpos_to_pose(goal_pose, current_qpos, mask=None, time_step=0.1, rrt_range=0.1, planning_time=1, fix_joint_limits=True, use_point_cloud=False, use_attach=False, wrt_world=True, planner_name='RRTConnect', no_simplification=False, constraint_function=None, constraint_jacobian=None, constraint_tolerance=0.001, verbose=False)

plan from a start configuration to a goal pose of the end-effector

Parameters:

• goal_pose – [x,y,z,qw,qx,qy,qz] pose of the goal

• current_qpos – current joint configuration (either full or move_group joints)

• mask – if the value at a given index is True, the joint is not used in the IK

• time_step – time step for TOPPRA (time parameterization of path)

• rrt_range – step size for RRT

• planning_time – time limit for RRT

• fix_joint_limits – if True, will clip the joint configuration to be within the joint limits

• use_point_cloud – if True, will use the point cloud to avoid collision

• use_attach – if True, will consider the attached tool collision when planning

• wrt_world – if true, interpret the target pose with respect to the world frame instead of the base frame

• verbose – if True, will print the log of OMPL and TOPPRA

plan_qpos_to_qpos(goal_qposes: list, current_qpos, time_step=0.1, rrt_range=0.1, planning_time=1, fix_joint_limits=True, use_point_cloud=False, use_attach=False, planner_name='RRTConnect', no_simplification=False, constraint_function=None, constraint_jacobian=None, constraint_tolerance=0.001, fixed_joint_indices=None, verbose=False)

plan a path from a specified joint position to a goal pose

Parameters:

• goal_pose – 7D pose of the end-effector [x,y,z,qw,qx,qy,qz]

• current_qpos – current joint configuration (either full or move_group joints)

• mask – mask for IK. When set, the IK will leave certain joints out of planning

• time_step – time step for TOPP

• rrt_range – step size for RRT

• planning_time – time limit for RRT

• fix_joint_limits – if True, will clip the joint configuration to be within the joint limits

• use_point_cloud – if True, will use the point cloud to avoid collision

• use_attach – if True, will consider the attached tool collision when planning

• planner_name – planner name pick from {“RRTConnect”, “RRTstar”}

• no_simplification – if true, will not simplify the path. constraint planning does not support simplification

• constraint_function – evals to 0 when constraint is satisfied

• constraint_jacobian – jacobian of constraint_function

• constraint_tolerance – tolerance for constraint_function

• fixed_joint_indices – list of indices of joints that are fixed during planning

• verbose – if True, will print the log of OMPL and TOPPRA

plan_screw(target_pose, qpos, qpos_step=0.1, time_step=0.1, use_point_cloud=False, use_attach=False, wrt_world=True, verbose=False)

Plan from a start configuration to a goal pose of the end-effector using screw motion

Parameters:

• target_pose – [x, y, z, qw, qx, qy, qz] pose of the goal

• qpos – current joint configuration (either full or move_group joints)

• qpos_step – size of the random step for RRT

• time_step – time step for the discretization

• use_point_cloud – if True, will use the point cloud to avoid collision

• use_attach – if True, will use the attached tool to avoid collision

• wrt_world – if True, interpret the target pose with respect to the world frame instead of the base frame

• verbose – if True, will print the log of TOPPRA

remove_normal_object(name)

returns true if the object was removed, false if it was not found

replace_package_keyword(package_keyword_replacement)

some ROS URDF files use package:// keyword to refer the package dir replace it with the given string (default is empty)

Parameters:

package_keyword_replacement – the string to replace ‘package://’ keyword

set_base_pose(pose)

tell the planner where the base of the robot is w.r.t the world frame

Parameters:

pose – [x,y,z,qw,qx,qy,qz] pose of the base

set_normal_object(name, collision_object)

adds or updates a non-articulated collision object in the scene

transform_goal_to_wrt_base(goal_pose)

update_attached_box(size, pose, link_id=-1)

attach a box to some link

Parameters:

• size – [x,y,z] size of the box

• pose – [x,y,z,qw,qx,qy,qz] pose of the box

• link_id – if not provided, the end effector will be the target.

update_attached_mesh(mesh_path, pose, link_id=-1)

attach a mesh to some link

Parameters:

• mesh_path – path to the mesh

• pose – [x,y,z,qw,qx,qy,qz] pose of the mesh

• link_id – if not provided, the end effector will be the target.

update_attached_sphere(radius, pose, link_id=-1)

attach a sphere to some link

Parameters:

• radius – radius of the sphere

• pose – [x,y,z,qw,qx,qy,qz] pose of the sphere

• link_id – if not provided, the end effector will be the target.

update_attached_tool(fcl_collision_geometry, pose, link_id=-1)

helper function to update the attached tool

update_point_cloud(pc, radius=0.001)

Parameters:

• pc – numpy array of shape (n, 3)

• radius – radius of each point. This gives a buffer around each point that planner will avoid

wrap_joint_limit(q) → bool

Checks if the joint configuration is within the joint limits. For revolute joints, the joint angle is wrapped to be within [q_min, q_min+2*pi)

Parameters:

q – joint configuration, angles of revolute joints might be modified

Returns:

True if q can be wrapped to be within the joint limits