Planner

class mplib.Planner(
urdf: str | Path,
move_group: str,
*,
srdf: str | Path | None = None,
new_package_keyword: str = '',
use_convex: bool = False,
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,
objects: list[FCLObject] = [],
verbose: bool = False,
)[source]

Bases: object

Motion planner

__init__(
urdf: str | Path,
move_group: str,
*,
srdf: str | Path | None = None,
new_package_keyword: str = '',
use_convex: bool = False,
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,
objects: list[FCLObject] = [],
verbose: bool = False,
)[source]

Motion planner for robots.

References

http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html

Parameters:

  • urdf – Unified Robot Description Format file.

  • move_group – target link to move, usually the end-effector.

  • srdf – Semantic Robot Description Format file.

  • new_package_keyword – the string to replace package:// keyword

  • use_convex – if True, load collision mesh as convex mesh. If mesh is not convex, a RuntimeError will be raised.

  • user_link_names – names of links, the order matters. If empty, all links will be used.

  • user_joint_names – names of the joints to plan. If empty, all active joints will be used.

  • joint_vel_limits – maximum joint velocities for time parameterization, which should have the same length as self.move_group_joint_indices

  • joint_acc_limits – maximum joint accelerations for time parameterization, which should have the same length as self.move_group_joint_indices

  • objects – list of FCLObject as non-articulated collision objects

  • verbose – if True, print verbose logs for debugging

wrap_joint_limit(qpos: ndarray) → bool[source]

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

Parameters:

qpos – joint positions, angles of revolute joints might be modified. If not within_limits (returns False), qpos might not be fully wrapped.

Returns:

whether qpos can be wrapped to be within the joint limits.

pad_move_group_qpos(qpos, articulation=None)[source]

If qpos contains only the move_group joints, return qpos padded with current values of the remaining joints of articulation. Otherwise, verify number of joints and return.

Parameters:

  • qpos – joint positions

  • articulation – the articulation to get qpos from. If None, use self.robot

Returns:

joint positions with full dof

check_for_collision(
collision_function,
state: ndarray | None = None,
) → list[WorldCollisionResult][source]

Helper function to check for collision

Parameters:

state – all planned articulations qpos state. If None, use current qpos.

Returns:

A list of collisions.

check_for_self_collision(
state: ndarray | None = None,
) → list[WorldCollisionResult][source]

Check if the robot is in self-collision.

Parameters:

state – all planned articulations qpos state. If None, use current qpos.

Returns:

A list of collisions.

check_for_env_collision(
state: ndarray | None = None,
) → list[WorldCollisionResult][source]

Check if the robot is in collision with the environment

Parameters:

state – all planned articulations qpos state. If None, use current qpos.

Returns:

A list of collisions.

IK(
goal_pose: Pose,
start_qpos: ndarray,
mask: Sequence[bool] | ndarray | None = None,
*,
n_init_qpos: int = 20,
threshold: float = 1e-3,
return_closest: bool = False,
verbose: bool = False,
) → tuple[str, list[ndarray] | ndarray | None][source]

Compute inverse kinematics

Parameters:

  • goal_pose – goal pose

  • start_qpos – initial configuration, (ndof,) np.floating np.ndarray.

  • mask – qpos mask to disable IK sampling, (ndof,) bool np.ndarray.

  • n_init_qpos – number of random initial configurations to sample.

  • threshold – distance threshold for marking sampled IK as success. distance is position error norm + quaternion error norm.

  • return_closest – whether to return the qpos that is closest to start_qpos, considering equivalent joint values.

  • verbose – whether to print collision info if any collision exists.

Returns:

(status, q_goals)

status: IK status, “Success” if succeeded.

q_goals: list of sampled IK qpos, (ndof,) np.floating np.ndarray.

IK is successful if q_goals is not None. If return_closest, q_goals is np.ndarray if successful and None if not successful.

TOPP(path, step=0.1, verbose=False)[source]

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

update_point_cloud(points, resolution=1e-3, name='scene_pcd')[source]

Adds a point cloud as a collision object with given name to world. If the name is the same, the point cloud is simply updated.

Parameters:

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

  • resolution – resolution of the point OcTree

  • name – name of the point cloud collision object

remove_point_cloud(name='scene_pcd') → bool[source]

Removes the point cloud collision object with given name

Parameters:

name – name of the point cloud collision object

Returns:

True if success, False if the non-articulation object with given name does not exist

update_attached_object(
collision_geometry: CollisionGeometry,
pose: Pose,
name='attached_geom',
art_name=None,
link_id=-1,
)[source]

Attach given object (w/ collision geometry) to specified link of articulation

Parameters:

  • collision_geometry – FCL collision geometry

  • pose – attaching pose (relative pose from attached link to object)

  • name – name of the attached geometry.

  • art_name – name of the articulated object to attach to. If None, attach to self.robot.

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

update_attached_sphere(
radius: float,
pose: Pose,
art_name=None,
link_id=-1,
)[source]

Attach a sphere to some link

Parameters:

  • radius – radius of the sphere

  • pose – attaching pose (relative pose from attached link to object)

  • art_name – name of the articulated object to attach to. If None, attach to self.robot.

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

update_attached_box(
size: Sequence[float] | ndarray[tuple[Literal[3], Literal[1]], dtype[floating]],
pose: Pose,
art_name=None,
link_id=-1,
)[source]

Attach a box to some link

Parameters:

  • size – box side length

  • pose – attaching pose (relative pose from attached link to object)

  • art_name – name of the articulated object to attach to. If None, attach to self.robot.

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

update_attached_mesh(
mesh_path: str,
pose: Pose,
art_name=None,
link_id=-1,
)[source]

Attach a mesh to some link

Parameters:

  • mesh_path – path to a mesh file

  • pose – attaching pose (relative pose from attached link to object)

  • art_name – name of the articulated object to attach to. If None, attach to self.robot.

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

detach_object(name='attached_geom', also_remove=False) → bool[source]

Detact the attached object with given name

Parameters:

  • name – object name to detach

  • also_remove – whether to also remove object from world

Returns:

True if success, False if the object with given name is not attached

set_base_pose(pose: Pose)[source]

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

Parameters:

pose – pose of the base

remove_object(name) → bool[source]

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

plan_qpos(
goal_qposes: list[ndarray],
current_qpos: ndarray,
*,
time_step: float = 0.1,
rrt_range: float = 0.1,
planning_time: float = 1,
fix_joint_limits: bool = True,
fixed_joint_indices: list[int] | None = None,
simplify: bool = True,
constraint_function: Callable[[ndarray, ndarray], None] | None = None,
constraint_jacobian: Callable[[ndarray, ndarray], None] | None = None,
constraint_tolerance: float = 1e-3,
verbose: bool = False,
) → dict[str, str | ndarray | float64][source]

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

Parameters:

  • goal_qposes – list of target joint configurations, [(ndof,)]

  • 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

  • simplify – whether the planned path will be simplified. (constained 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_pose(
goal_pose: Pose,
current_qpos: ndarray,
mask: list[bool] | ndarray | None = None,
*,
time_step: float = 0.1,
rrt_range: float = 0.1,
planning_time: float = 1,
fix_joint_limits: bool = True,
wrt_world: bool = True,
simplify: bool = True,
constraint_function: Callable | None = None,
constraint_jacobian: Callable | None = None,
constraint_tolerance: float = 1e-3,
verbose: bool = False,
) → dict[str, str | ndarray | float64][source]

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

Parameters:

  • goal_pose – 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

  • 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_screw(
goal_pose: Pose,
current_qpos: ndarray,
*,
qpos_step: float = 0.1,
time_step: float = 0.1,
wrt_world: bool = True,
verbose: bool = False,
) → dict[str, str | ndarray | float64][source]

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

Parameters:

  • goal_pose – pose of the goal

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

  • qpos_step – size of the random step

  • time_step – time step for the discretization

  • 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