HLP-R Manipulation Utilities¶

ArmMoveIt 2 Details¶

class hlpr_manipulation_utils.arm_moveit2.ArmMoveIt(planning_frame='base_link', default_planner='RRTConnectkConfigDefault')[source]¶

MoveIt! wrapper for planning for the HLP-R robot’s arm(s).

This class provides a simplified interface to the planning capabilities of the MoveIt! ROS packages. To plan to a specific joint configuration:

a = ArmMoveit()
pose = [pi,pi,pi,pi]

a.goto_pose(pose,is_joint_pos=True)

Methods

box_table_scene()	*DEPRECATED* Left in for backwards compatibility
get_FK([root])	Find the end effector pose for the current joint configuration
get_FK_wpi([joints])	Find the end effector pose for a given joint configuration
get_IK(new_pose[, root, group_id])	Find the corresponding joint angles for an end effector pose
get_active_joints([group_id])	*DEPRECATED* Left in for backwards compatibility
get_current_pose([group_id])	Returns the current pose of the planning group
get_plan([group_id])	Generates a plan for reaching the current goal
merge_points(points, new_points[, time_between])	*DEPRECATED* Left in for backwards compatibility
move_robot(plan[, group_id])	Moves the robot according to a plan
move_through_ee_waypoints(targets[, group_id])	Uses MoveIt! to generate a plan then move the robot.
move_through_joint_waypoints(targets[, group_id])	Uses MoveIt! to generate a plan then move the robot.
move_through_waypoints(targets[, …])	Uses MoveIt! to generate a plan then move the robot.
move_to_ee_pose(target[, group_id])	Uses MoveIt! to generate a plan then move the robot.
move_to_joint_pose(target[, group_id])	Uses MoveIt! to generate a plan then move the robot.
move_to_pose(target[, is_joint_pos, group_id])	Uses MoveIt! to generate a plan then move the robot.
plan_ee_pos(target[, starting_config, group_id])	Plan a trajectory to reach a given end effector position
plan_ee_waypoints(targets[, group_id, …])	Generates a multi-segment plan to reach end effector waypoints
plan_jointTargetInput(target_joint[, group_id])	*DEPRECATED* Left in for backwards compatibility
plan_joint_pos(target[, starting_config, …])	Plan a trajectory to reach a given joint configuration
plan_joint_waypoints(targets[, group_id, …])	Generates a multi-segment plan to reach waypoints in joint space
plan_pose(target[, is_joint_pos, …])	Plan a trajectory to reach a given end effector or joint pose
plan_poseTargetInput(target_pose[, group_id])	*DEPRECATED* Left in for backwards compatibility
plan_targetInput(target, joint_flag[, group_id])	*DEPRECATED* Generic target planner that what type is specified
plan_targetInputWaypoint(targets, joint_flag)	*DEPRECATED* Left in for backwards compatibility
plan_waypoints(targets[, is_joint_pos, …])	Generates a multi-segment plan to reach waypoints in target space
set_ee_target(target[, group_id])	Set the end effector position target
set_joint_target(target[, group_id])	Set the joint configuration target
set_robot_state_joint_dict(joint_dict)	*DEPRECATED* Left in for backwards compatibility
set_robot_state_pose(traj)	*DEPRECATED* Left in for backwards compatibility
set_start_state([joint_config, group_id])	Set the starting state from which to plan
state_from_joints(joints)	Returns a RobotState object with given joint positions
state_from_trajectory(trajectory[, point_idx])	Returns a RobotState object with joint positions from trajectory
wayPointIK(wps[, numSteps, ik_root])	*DEPRECATED* Left in for backwards compatibility

box_table_scene()[source]¶

*DEPRECATED* Left in for backwards compatibility

get_FK(root='base_link')[source]¶

Find the end effector pose for the current joint configuration

Uses MoveIt! to compute the forward kinematics for a given joint configuration.

Parameters:	root : string, optional the root link (if none is provided, uses the planning frame)
Returns:	geometry_msgs/PoseStamped the pose of the end effector

get_FK_wpi(joints=None)[source]¶

Find the end effector pose for a given joint configuration

Uses the WPI Jaco wrapper to compute the forward kinematics for a given joint configuration.

Parameters:	joints : list, optional the joint positions for which to find the end effector position. If not provided, gives FK for all joints set to value of pi.
Returns:	geometry_msgs/PoseStamped the pose of the end effector

get_IK(new_pose, root=None, group_id=0)[source]¶

Find the corresponding joint angles for an end effector pose

Uses MoveIt! to compute the inverse kinematics for a given end effector pose.

Parameters:	new_pose : geometry_msgs/Pose the end effector pose root : string, optional the root link (if none is provided, uses the planning frame) group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	list The joint angles corresponding to the end effector pose

get_active_joints(group_id=0)[source]¶

*DEPRECATED* Left in for backwards compatibility

get_current_pose(group_id=0)[source]¶

Returns the current pose of the planning group

Parameters:	group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	list the joint positions, mapped into (-pi,pi)

get_plan(group_id=0)[source]¶

Generates a plan for reaching the current goal

Uses MoveIt! to generate a plan based on the previously-set starting position and target position.

Note

You must set the target and starting position before calling this function.

Parameters:	group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	RobotTrajectory the plan for reaching the target position

merge_points(points, new_points, time_between=0.1)[source]¶

*DEPRECATED* Left in for backwards compatibility

move_robot(plan, group_id=0)[source]¶

Moves the robot according to a plan

Uses MoveIt! to move the arm according to the provided plan.

Warning

The plans can be stitched together, but this can have unexpected issues since the plan boundaries can violate acceleration limits.

Parameters:	plan : RobotTrajectory a plan generated by MoveIt! group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	bool True on success

move_through_ee_waypoints(targets, group_id=0)[source]¶

Uses MoveIt! to generate a plan then move the robot.

Generates a plan to move the robot through the specified end effector or joint pose waypoints, then moves the robot. Returns True on success, otherwise returns False.

Parameters:	targets : list a list of end effector positions (geometry_msgs/Pose) See the documentation for plan_ee_waypoints. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	bool True on success

move_through_joint_waypoints(targets, group_id=0)[source]¶

Uses MoveIt! to generate a plan then move the robot.

Generates a plan to move the robot through the specified end effector or joint pose waypoints, then moves the robot. Returns True on success, otherwise returns False.

Parameters:	targets : list a list of joint positions (either list or dict) See the documentation for plan_joint_waypoints. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	bool True on success

move_through_waypoints(targets, is_joint_pos=False, group_id=0)[source]¶

Uses MoveIt! to generate a plan then move the robot.

Generates a plan to move the robot through the specified end effector or joint pose waypoints, then moves the robot. Returns True on success, otherwise returns False.

Parameters:	targets : list a list of either joint positions or end effector positions. See the documentation for plan_joint_waypoints and plan_ee_waypoints. is_joint_pos : bool, optional True if the targets are given in joint space (defaults to False) group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	bool True on success

move_to_ee_pose(target, group_id=0)[source]¶

Uses MoveIt! to generate a plan then move the robot.

Generates a plan to move the robot to the specified joint pose, then moves the robot. Returns True on success, otherwise returns False.

Parameters:	target : geometry_msgs/Pose an end effector position. See the documentation for plan_ee_waypoints. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	bool True on success

move_to_joint_pose(target, group_id=0)[source]¶

Uses MoveIt! to generate a plan then move the robot.

Generates a plan to move the robot to the specified joint pose, then moves the robot. Returns True on success, otherwise returns False.

Parameters:	target : list or dict a joint position (list or dict). See the documentation for plan_joint_waypoints. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	bool True on success

move_to_pose(target, is_joint_pos=False, group_id=0)[source]¶

Uses MoveIt! to generate a plan then move the robot.

Generates a plan to move the robot to the specified end effector or joint pose, then moves the robot. Returns True on success, otherwise returns False.

Parameters:

target : list, dict, or geometry_msgs/Pose either a joint position (list or dict) or end effector position. See the documentation for plan_joint_waypoints and plan_ee_waypoints. is_joint_pos : bool, optional True if the targets are given in joint space (defaults to False) group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.

Returns:

bool True on success

plan_ee_pos(target, starting_config=None, group_id=0)[source]¶

Plan a trajectory to reach a given end effector position

Uses MoveIt! to plan a trajectory to reach a given end effector position

Parameters:	target : geometry_msgs/Pose the desired pose of the end effector starting_config : RobotState, optional the starting configuration to plan from. If not set, will be set to the current state of the robot. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	RobotTrajectory the plan for reaching the target position

plan_ee_waypoints(targets, group_id=0, starting_config=None)[source]¶

Generates a multi-segment plan to reach end effector waypoints

Uses MoveIt! to generate a plan from target to target. One plan is generated for each segment.

Warning

The plans can be stitched together, but this can have unexpected issues since the plan boundaries can violate acceleration limits.

Parameters:	targets : list a list of geometry_msgs/Pose for the end effector group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now. starting_config : RobotState the starting state. If not provided this will default to the current state of the robot.
Returns:	list of RobotTrajectory the plan for reaching the target position

plan_jointTargetInput(target_joint, group_id=0)[source]¶

*DEPRECATED* Left in for backwards compatibility

plan_joint_pos(target, starting_config=None, group_id=0)[source]¶

Plan a trajectory to reach a given joint configuration

Uses MoveIt! to plan a trajectory to reach a given joint configuration

Parameters:

target : list or dict if a list, a list of positions for all active joints in the group; if a dictionary, a mapping of joint names to positions for the joints that should be moved (all other joints will be held at their current angles). starting_config : RobotState, optional the starting configuration to plan from. If not set, will be set to the current state of the robot. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.

Returns:

RobotTrajectory the plan for reaching the target position

plan_joint_waypoints(targets, group_id=0, starting_config=None)[source]¶

Generates a multi-segment plan to reach waypoints in joint space

Uses MoveIt! to generate a plan from target to target. One plan is generated for each segment.

Warning

The plans can be stitched together, but this can have unexpected issues since the plan boundaries can violate acceleration limits.

Parameters:	targets : list a list of joint targets (either list or dict; see documentation for plan_joint_pos() ) group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now. starting_config : RobotState the starting state. If not provided this will default to the current state of the robot.
Returns:	list of RobotTrajectory the plan for reaching the target position

plan_pose(target, is_joint_pos=False, starting_config=None, group_id=0)[source]¶

Plan a trajectory to reach a given end effector or joint pose

Uses MoveIt! to plan a trajectory to reach a given end effector position or joint configuration.

Parameters:

target : list, dict, or geometry_msgs/Pose either a joint position (list or dict) or end effector position. See the documentation for plan_joint_waypoints and plan_ee_waypoints. is_joint_pos : bool, optional True if the targets are given in joint space (defaults to False) group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.

Returns:

RobotTrajectory the plan for reaching the target position

plan_poseTargetInput(target_pose, group_id=0)[source]¶

*DEPRECATED* Left in for backwards compatibility

plan_targetInput(target, joint_flag, group_id=0)[source]¶

*DEPRECATED* Generic target planner that what type is specified

plan_targetInputWaypoint(targets, joint_flag, merged=False, current_joints=None, group_id=0)[source]¶

*DEPRECATED* Left in for backwards compatibility

plan_waypoints(targets, is_joint_pos=False, merge_plans=False, starting_config=None, group_id=0)[source]¶

Generates a multi-segment plan to reach waypoints in target space

Uses MoveIt! to generate a plan from target to target. One plan is generated for each segment.

Warning

The plans can be stitched together, but this can have unexpected issues since the plan boundaries can violate acceleration limits.

Parameters:

targets : list a list of either joint positions or end effector positions. See the documentation for plan_joint_waypoints and plan_ee_waypoints. is_joint_pos : bool, optional True if the targets are given in joint space (defaults to False) merge_plans : bool if True, return a single merged plan (see note above about potential issues) group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now. starting_config : RobotState the starting state. If not provided this will default to the current state of the robot.

Returns:

list of RobotTrajectory the plan for reaching the target position

set_ee_target(target, group_id=0)[source]¶

Set the end effector position target

Sets the MoveIt! target position in preparation for planning

Parameters:	target : geometry_msgs/Pose the desired pose of the end effector group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.

set_joint_target(target, group_id=0)[source]¶

Set the joint configuration target

Sets the MoveIt! target position in preparation for planning

Parameters:	target (list or dict) – if a list, a list of positions for all active joints in the group; if a dictionary, a mapping of joint names to positions for the joints that should be moved (all other joints will be held at their current angles). group_id (int) – the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.

set_robot_state_joint_dict(joint_dict)[source]¶

*DEPRECATED* Left in for backwards compatibility

set_robot_state_pose(traj)[source]¶

*DEPRECATED* Left in for backwards compatibility

set_start_state(joint_config=None, group_id=0)[source]¶

Set the starting state from which to plan

Sets the MoveIt! starting position in preparation for planning

Parameters:	joint_config (RobotState) – the starting configuration to plan from. If not set, will be set to the current state of the robot. group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.

state_from_joints(joints)[source]¶

Returns a RobotState object with given joint positions

Returns a RobotState object with the given joints set to the given positions. The joints may be given as a dict or list object. All other joints are taken from the current state.

Parameters:	target : list or dict if a list, a list of positions for all active joints in the group; if a dictionary, a mapping of joint names to positions for the joints that should be moved (all other joints will be held at their current angles). group_id : int, optional the index of the group for which to calculate the IK. Used for compatibility with future functionality; leave set to default for now.
Returns:	RobotState A RobotState object with only the given joints changed

state_from_trajectory(trajectory, point_idx=-1)[source]¶

Returns a RobotState object with joint positions from trajectory

Returns a RobotState object with joint positions taken from the given trajectory. By default, sets the position to the last point in the trajectory.

Parameters:	trajectory : JointTrajectory the trajectory from which to take the joint positions. point_idx : int which point in the trajectory to take the state from. Defaults to -1, taking the last point.
Returns:	RobotState A RobotState object with state corresponding to the given point in the trajectory.

wayPointIK(wps, numSteps=None, ik_root=None)[source]¶

*DEPRECATED* Left in for backwards compatibility