Motion Planner

class pyrobot.algorithms.motion_planner.MotionPlanner(configs, world, ros_launch_manager=None, robots={}, sensors={}, algorithms={})

Bases: pyrobot.algorithms.algorithm.Algorithm

Base class of Motion Planning algorithms. Specifically, this algorithm handles computing and executing of motion plans.

The algorithm should take a robot model urdf, specified in a tree of joint frames, and an end-effector frame, in the constructor. Motion plans should be computed w.r.t that urdf and end-effector frame.

__init__(configs, world, ros_launch_manager=None, robots={}, sensors={}, algorithms={})

Initialize self. See help(type(self)) for accurate signature.

check_cfg()

This function checks required configs shared by all motion planning instances in constructor.

Specifically, it checks for:

1. The motion planning algorithm handles one robot, and one robot only

2. The robot has an arm

3. The arm has a base frame, an end-effector frame, and urdf description.

For any algorithm specific config checks, please extend the check_cfg function

with customized algorithm config checks after calling this function using super().check_cfg()

plan_end_effector_pose(position, orientation)

This function takes in position and orientation of a desired end effector pose,

generate a motion plan to the desired pose, and execute the plan.

Args:

position: position vector as list-like objects in the form of [x, y, z]. Unit: meters. orientation: orientation vector or matrix with one of the following forms:

1. euler angles, as list-like object of form [x, y, z]

2. quaternion, as list-like object of form [x, y, z, w]

3. a 3x3 3D rotation matrix

Returns:

plan: list of timestamped joint angle points along the planned trajectory. Unit: radius.

i.e. for a 5DoF arm with (joint1, joint2, …, joint5) in urdf, this function should return a list of points with point.positions, point.velocities and point.accelarations as vectors of size 5 (angle1, angle2, …, angle5)

If a plan cannot be generated or the generated plan cannot be executed, this function should raise an error.

plan_joint_angles(target_joint)

This function takes in desired joint position,

generate a motion plan to the desired pose, and execute the plan.

Args:

target_joint: list-like object of joint angles correspond to the queried end effector, Unit: radius.

for a 5DoF arm with (joint1, joint2, …, joint5) in urdf, this function should return a vector with size 5 of (angle1, angle2, …, angle5)

Returns:

plan: list of timestamped joint angle points along the planned trajectory. Unit: radius.

i.e. for a 5DoF arm with (joint1, joint2, …, joint5) in urdf, this function should return a list of points with point.positions, point.velocities and point.accelarations as vectors of size 5 (angle1, angle2, …, angle5)

If a plan cannot be generated or the generated plan cannot be executed, this function should raise an error.