7.9.1. moveit_wrapper.MoveitWrapper

class moveit_wrapper.MoveitWrapper(libtf, planning_time=5.0)

Bases: object

Convenience Moveit Wrapper for Sasha.

__init__(libtf, planning_time=5.0)

Initializes the MoveitWrapper.

Parameters:

libtf (tf_wrapper) – Instance of the tf_wrapper class
planning_time (float, optional) – The time to plan a trajectory, by default 5.0

Methods

`__init__`(libtf[, planning_time])	Initializes the MoveitWrapper.
`add_box`(name, frame, pose, size)	Add a box to the planning scene.
`add_cylinder`(name, frame, pose, height, radius)	Add a cylinder to the planning scene.
`add_model_from_sdf`(frame, point, euler, sdf)	Add a model from a sdf-file to the planning scene.
`add_orientation_constraint`(reference_frame, ...)	Adds an orientation constraint to the whole body until it is cleared
`all_close`(goal, actual, pos_tolerance, ...)	Judges whether the actual value is within the tolerance for the goal value.
`attach_object`(object_name[, touch_links])	Attaches an object to the robot eef
`clear_path_constraints`()	Clears the path constraints of the whole body
`current_pose_close_to_target`(target_pose[, ...])	Checks if the current pose is close to the target pose
`detach_all_objects`()	Detaches all objects from the robot eef
`get_attached_objects`([object_names])	Returns the attached objects
`get_current_eef_pose`([eef_link, reference_frame])	Returns the current pose of the end effector link in the specified frame
`get_current_pose`(frame)	Returns the current pose of the robot base in the specified frame
`get_link_names`([group])	Returns the link names of the specified group
`get_object_poses`(object_names)	Returns the poses of the objects in the planning scene
`get_objects`([object_names])	Returns the objects in the planning scene
`get_planning_frame`([group])	Returns the planning frame of the specified group
`gripper_grasp`()	Closes the gripper to grasp an object.
`gripper_open`()	Opens the gripper.
`init_moveit`(planning_time)	Initializes the moveit commander and returns the whole_body, gripper, scene and robot objects
`place`(object, pose)	Places an object at the specified pose
`set_endeffector`(eef)	Sets which link should be used as the end effector
`set_support_surface`(surface_name)	Sets the support surface for the whole body
`set_whole_body_acc`([acc])	Set the maximum acceleration of the whole_body.
`set_whole_body_vel`([vel])	Set the maximum speed of the whole_body.
`whole_body_IK`(x, y, z, q[, frame, ...])	Plans and executes a motion to the target pose.
`whole_body_IK_cartesian`(x, y, z, q[, ...])	Plan and execute a motion to the target pose using inverse kinematics control of the whole body.
`whole_body_IK_cartesian_plan`(waypoints, ...)	Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints.
`whole_body_IK_cartesian_with_timeout`(x, y, z, q)	Plans and executes a motion to the target pose for multiple coordinates with a timeout.
`whole_body_plan_and_go`(pose_target)	Plans and executes a motion to the target pose

add_box(name, frame, pose, size)

Add a box to the planning scene.

Parameters:

name (str) – The name of the box
frame (str) – The frame of the passed pose
pose (geometry_msgs/Point, geometry_msgs/Pose) – Pose of the box relative to the frame. If geometry_msgs/Point is passed, the orientation is set to (0, 0, 0, 1), otherwise the orientation is set to the passed value.
size (list[float]) – The size of the box as (x, y, z)

add_cylinder(name, frame, pose, height, radius)

Add a cylinder to the planning scene.

Parameters:

name (str) – The name of the cylinder
frame (str) – The frame of the passed pose
pose (geometry_msgs/Point, geometry_msgs/Pose) – Pose of the cylinder relative to the frame. If geometry_msgs/Point is passed, the orientation is set to (0, 0, 0, 1), otherwise the orientation is set to the passed value
height (float) – The height of the cylinder in meters
radius (float) – The radius of the cylinder in meters

add_model_from_sdf(frame, point, euler, sdf)

Add a model from a sdf-file to the planning scene.

Parameters:

frame (str) – The name of the frame
point (geometry_msgs/Point) – The coordinates to add the model
euler (geometry_msgs/Point) – The euler angles of the model to be added
sdf (str) – The path to the sdf file

add_orientation_constraint(reference_frame, end_effector_link, orientation, tolerances_xyz=[0.3, 0.3, 0.3])

Adds an orientation constraint to the whole body until it is cleared

Parameters:

reference_frame (str) – The name of the reference frame (i.e. the frame in which the orientation constrain is defined in)
end_effector_link (str) – The name of the end effector link which should be constrained
orientation (geometry_msgs/Quaternion) – The target orientation of the end effector link (i.e. the eef should have this orientation during the motion)
tolerances_xyz (list[float], optional) – The tolerances for the orientation constraint, by default [0.3, 0.3, 0.3] (radians)

all_close(goal, actual, pos_tolerance, ori_tolerance)

Judges whether the actual value is within the tolerance for the goal value.

Parameters:

goal (list[float], geometry_msgs/Pose, geometry_msgs/PoseStamped) – The goal value
actual (list[float], geometry_msgs/Pose, geometry_msgs/PoseStamped) – The actual value
pos_tolerance (float) – The tolerance for the positions
ori_tolerance (float) – The tolerance for the orientations

Returns:

True if the value is within the tolerance

Return type:

bool

attach_object(object_name, touch_links=[])

Attaches an object to the robot eef

Parameters:

object_name (str) – The name of the object to attach. Must be a known object in the planning scene.
touch_links (list[str], optional) – The links that are allowed to touch the object (e.g. parts of the gripper), by default []

clear_path_constraints(): Clears the path constraints of the whole body

current_pose_close_to_target(target_pose, pos_tolerance=0.04, ori_tolerance=0.17)

Checks if the current pose is close to the target pose

Parameters:

target_pose (geometry_msgs/Pose) – The target pose
pos_tolerance (float, optional) – The tolerance for the positions, by default 0.04
ori_tolerance (float, optional) – The tolerance for the orientations, by default 0.17

Returns:

True if the current pose is close to the target pose

Return type:

bool

detach_all_objects()

Detaches all objects from the robot eef

Parameters:: object_name (str) – The name of the object to detach. Must be a known object in the planning scene.

get_attached_objects(object_names=[])

Returns the attached objects

Parameters:: object_names (list[str], optional) – The names of the objects to return. If empty, all attached objects are returned, by default []
Returns:: The attached objects. The keys are the object names and the values are the objects (moveit_msgs/AttachedCollisionObject)
Return type:: dict

get_current_eef_pose(eef_link='hand_palm_link', reference_frame='map')

Returns the current pose of the end effector link in the specified frame

Parameters:

eef_link (str, optional) – The name of the end effector link, by default “hand_palm_link”
reference_frame (str, optional) – The name of the reference frame to transform the eef pose to, by default “map”

get_current_pose(frame)

Returns the current pose of the robot base in the specified frame

Parameters:: frame (str) – The frame to transform the base pose to
Returns:: The current pose of the robot base in the specified frame
Return type:: geometry_msgs/PoseStamped

get_link_names(group=None)

Returns the link names of the specified group

Parameters:: group (str, optional) – The name of the group, by default None
Returns:: All link names of the specified group.
Return type:: list[str]

get_object_poses(object_names)

Returns the poses of the objects in the planning scene

Parameters:: object_names (list[str]) – The names of the objects to return
Returns:: The poses of the objects in the planning scene. The keys are the object names and the values are the poses (geometry_msgs/Pose). The frame of the poses is the planning frame.
Return type:: dict

get_objects(object_names=[])

Returns the objects in the planning scene

Parameters:: object_names (list[str], optional) – The names of the objects to return. If empty, all objects are returned, by default []
Returns:: The objects in the planning scene. The keys are the object names and the values are the objects (moveit_msgs/CollisionObject)
Return type:: dict

get_planning_frame(group='whole_body')

Returns the planning frame of the specified group

Parameters:: group (str, optional) – The name of the group, by default “whole_body”
Returns:: The planning frame of the specified group
Return type:: str

gripper_grasp(): Closes the gripper to grasp an object.

gripper_open(): Opens the gripper. The gripper is opened by moving the hand_motor_joint to 1.0

init_moveit(planning_time)

Initializes the moveit commander and returns the whole_body, gripper, scene and robot objects

Parameters:

planning_time (float) – The time to plan a trajectory

Returns:

whole_body (moveit_commander.MoveGroupCommander) – The whole body move group commander
gripper (moveit_commander.MoveGroupCommander) – The gripper move group commander
scene (moveit_commander.PlanningSceneInterface) – The planning scene interface
robot (moveit_commander.RobotCommander) – The robot commander

place(object, pose)

Places an object at the specified pose

Parameters:

object (str) – The name of the object to place
pose (geometry_msgs/Pose) – The pose to place the object at

Return type:

not None if the object was placed successfully, None otherwise

set_endeffector(eef)

Sets which link should be used as the end effector

Parameters:: eef (str) – The name of the end effector link

set_support_surface(surface_name): Sets the support surface for the whole body

set_whole_body_acc(acc=1.0)

Set the maximum acceleration of the whole_body.

Parameters:: acc (float, optional) – Maximum acceleration value. The value must be between 0.0 and 1.0, by default 1.0

set_whole_body_vel(vel=1.0)

Set the maximum speed of the whole_body.

Parameters:: vel (float, optional) – Maximum speed value. The value must be between 0.0 and 1.0, by default 1.0

whole_body_IK(x, y, z, q, frame='odom', pos_tolerance=0.02, ori_tolerance=0.17)

Plans and executes a motion to the target pose.

Parameters:

x (float) – Target x-coordinate of the end-effector
y (float) – Target y-coordinate of the end-effector
z (float) – Target z-coordinate of the end-effector
q (geometry_msgs/Quaternion) – Target orientation of the end-effector
frame (str, optional) – The name of the reference frame, by default “odom”
pos_tolerance (float, optional) – The tolerance for the positions, by default 0.02. Used to check if the IK was successful
ori_tolerance (float, optional) – The tolerance for the orientations, by default 0.17. Used to check if the IK was successful

Returns:

True if the IK was successful, False otherwise

Return type:

bool

whole_body_IK_cartesian(x, y, z, q, is_avoid_obstacle=True, eef_step=0.01, fraction_th=0.5, pos_tolerance=0.04, ori_tolerance=0.17)

Plan and execute a motion to the target pose using inverse kinematics control of the whole body.

Assumes the coordinates to be in the planning scene frame and uses Time.now() as a stamp.

Parameters:

x (float) – Target x-coordinate of the end-effector
y (float) – Target y-coordinate of the end-effector
z (float) – Target z-coordinate of the end-effector
q (geometry_msgs/Quaternion) – Target orientation of the end-effector
is_avoid_obstacle (bool, optional) – Whether obstacles should be avoided, by default True
eef_step (float, optional) – The step size for the calculation, by default 0.01
fraction_th (float, optional) – Threshold for the fraction of the path. The fraction itself indicates the percentage of how much of the path was followed. If the fraction of the plan is smaller than the value of the fraction threshold, the function does not return a plan, by default 0.5
pos_tolerance (float, optional) – The tolerance for the positions, by default 0.04
ori_tolerance (float, optional) – The tolerance for the orientations, by default 0.17

Returns:

True if the IK was successfully planned and executed, False otherwise

Return type:

bool

whole_body_IK_cartesian_plan(waypoints, is_avoid_obstacle, eef_step, fraction_th)

Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints.

Configurations are computed for every eef_step meters.

Parameters:

waypoints (list[geometry_msgs/Pose]) – The waypoints to plan a path for. The frame of the poses should be the same as the planning frame
is_avoid_obstacle (bool) – Whether obstacles should be avoided
eef_step (float) – The step size for the calculation
fraction_th (float) – Threshold for the fraction of the path. The fraction itself indicates the percentage of how much of the path was followed. If the fraction of the plan is smaller than the value of the fraction threshold, the function does not return a plan.

Returns:

The computed plan

Return type:

moveit_commander.RobotTrajectory

whole_body_IK_cartesian_with_timeout(x, y, z, q, is_avoid_obstacle=True, eef_step=0.01, fraction_th=0.5, pos_tolerance=0.02, ori_tolerance=0.17, timeout=5.0)

Plans and executes a motion to the target pose for multiple coordinates with a timeout.

Parameters:

x (float) – Target x-coordinate of the end-effector
y (float) – Target y-coordinate of the end-effector
z (float) – Target z-coordinate of the end-effector
q (geometry_msgs/Quaternion) – Target orientation of the end-effector
is_avoid_obstacle (bool, optional) – Whether obstacles should be avoided, by default True
eef_step (float, optional) – The step size for the calculation, by default 0.01
fraction_th (float, optional) – Threshold for the fraction of the path. The fraction itself indicates the percentage of how much of the path was followed. If the fraction of the plan is smaller than the value of the fraction threshold, the function does not return a plan, by default 0.5
pos_tolerance (float, optional) – The tolerance for the positions, by default 0.02
ori_tolerance (float, optional) – The tolerance for the orientations, by default 0.17
timeout (float, optional) – The timeout in seconds, by default 5.0. If the timeout is reached, the function aborts.

Returns:

True if the IK was successfully planned and executed, False otherwise

Return type:

bool

whole_body_plan_and_go(pose_target)

Plans and executes a motion to the target pose

Parameters:: pose_target (geometry_msgs/PoseStamped) – The target pose
Returns:: True if the plan was found and executed, False otherwise
Return type:: bool