def get_cube_names(self, pose):
point = PoseStamped()
point.header.frame_id = 'base_link'
point.header.stamp = rospy.Time()
point.pose = pose
tf_listener = TransformListener()
rospy.sleep(1.0)
return tf_listener.transformPose('/map', point)
def transform_pose(_pose2D, src_frame='CameraTop_frame', dst_frame='/base_footprint', timeout=3):
tl = TransformListener()
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time()
pose_stamped.header.frame_id = src_frame
pose_stamped.pose = Pose(Point(_pose2D.x, _pose2D.y, 0.0), Quaternion())
try:
tl.waitForTransform(target_frame=dst_frame, source_frame=src_frame,
time=rospy.Time(), timeout=rospy.Duration(timeout))
pose_transf = tl.transformPose(dst_frame, pose_stamped)
except Exception as e:
rospy.logwarn("Transformation failed!!! %s" % str(e))
return _pose2D
return Pose2D(pose_transf.pose.position.x, pose_transf.pose.position.y, 0.0)
def transform_pose(pose, src_frame, dst_frame, timeout=10):
"""
Transforms the given pose from src_frame to dst_frame.
:param src_frame
:param dst_frame
:param timeout the amount of time allowed (in secs) for a transformation (default 3)
"""
if str(type(pose)) != str(type(Pose())):
rospy.logwarn(colors.BACKGROUND_RED + "The 'pose' should be a Pose object, not '%s'.%s" % (str(type(pose)).split('\'')[1], colors.NATIVE_COLOR))
from tf.listener import TransformListener
assert timeout >= 1
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time()
pose_stamped.header.frame_id = src_frame
pose_stamped.pose = pose
global _tl
if not _tl:
_tl = TransformListener()
rospy.sleep(0.5)
timeout -= 0.5
rospy.loginfo("Transforming position from %s to %s coordinates..." % (
src_frame, dst_frame))
# If something fails we'll return the original pose (for testing
# with mocks when tf isn't available)
result = pose
try:
_tl.waitForTransform(
target_frame=dst_frame, source_frame=src_frame,
time=rospy.Time(), timeout=rospy.Duration(timeout))
pose_transf = _tl.transformPose(dst_frame, pose_stamped)
result = pose_transf.pose
except Exception as e:
rospy.logwarn(colors.BACKGROUND_RED + "Warning! Pose transformation failed! %s%s" % (str(e), colors.NATIVE_COLOR))
return result
class MoveGroupInterface(object):
## @brief Constructor for this utility
## @param group Name of the MoveIt! group to command
## @param frame Name of the fixed frame in which planning happens
## @param listener A TF listener instance (optional, will create a new one if None)
## @param plan_only Should we only plan, but not execute?
def __init__(self, group, frame, listener=None, plan_only=False):
self._group = group
self._fixed_frame = frame
self._action = actionlib.SimpleActionClient('move_group',
MoveGroupAction)
self._action.wait_for_server()
if listener == None:
self._listener = TransformListener()
else:
self._listener = listener
self.plan_only = plan_only
self.planner_id = None
self.planning_time = 15.0
def get_move_action(self):
return self._action
## @brief Move the arm to set of joint position goals
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_scene_diff",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
## @brief Move the arm, based on a goal pose_stamped for the end effector.
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
# Check arguments
supported_args = ("max_velocity_scaling_factor",
"planner_id",
"planning_time",
"plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
return self._action.get_result()
else:
return None
## @brief Sets the planner_id used for all future planning requests.
## @param planner_id The string for the planner id, set to None to clear
def setPlannerId(self, planner_id):
self.planner_id = str(planner_id)
## @brief Set default planning time to be used for future planning request.
def setPlanningTime(self, time):
self.planning_time = time
class arm_server_node(object):
"""
This node is an action server, to help simplify arm movement,
using moveit_commander
"""
_feedback = elevator.msg.SimpleTargetFeedback()
_result = elevator.msg.SimpleTargetResult()
def __init__(self, name):
# wait for moveit
while not "/move_group/result" in dict(
rospy.get_published_topics()).keys():
rospy.sleep(2)
self.group = MoveGroupCommander("arm")
self.group.set_start_state_to_current_state()
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.set_pose_reference_frame("/base_footprint")
self.group.set_max_velocity_scaling_factor(1)
self.group.set_num_planning_attempts(50)
self.group.set_planning_time(10)
self.group.set_goal_position_tolerance(0.01)
self.group.set_goal_orientation_tolerance(0.02)
self.tf_listener = TransformListener()
self._action_name = name
self._as = actionlib.SimpleActionServer(
self._action_name,
elevator.msg.SimpleTargetAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
def execute_cb(self, goal):
eef_pose = self.group.get_current_pose("gripper_link")
self._feedback.x = math.fabs(goal.x - eef_pose.pose.position.x)
self._feedback.y = math.fabs(goal.y - eef_pose.pose.position.y)
self._feedback.z = math.fabs(goal.z - eef_pose.pose.position.z)
self._feedback.distance = math.sqrt(
math.pow(self._feedback.x, 2) + math.pow(self._feedback.y, 2) +
math.pow(self._feedback.z, 2))
# publish the feedback
self._as.publish_feedback(self._feedback)
# start executing the action
if goal.frame_id[0] != '/':
rospy.loginfo('[%s]: Executing, moving arm to "%s" pose' %
(self._action_name, goal.frame_id))
self.group.set_named_target(goal.frame_id)
named_plan = self.group.plan()
self.group.execute(named_plan)
else:
rospy.loginfo(
'[%s]: Executing, moving gripper from (%.3f, %.3f, %.3f) to (%.3f, %.3f, %.3f)'
% (self._action_name, eef_pose.pose.position.x,
eef_pose.pose.position.y, eef_pose.pose.position.z, goal.x,
goal.y, goal.z))
origin_goal = PoseStamped()
origin_goal.header.frame_id = "/gripper_link"
origin_goal.pose.position.x = goal.x
origin_goal.pose.position.y = goal.y
origin_goal.pose.position.z = goal.z
transformed_goal = self.tf_listener.transformPose(
"/base_footprint", origin_goal)
transformed_goal.pose.orientation.x = 0
transformed_goal.pose.orientation.y = 0
transformed_goal.pose.orientation.z = 0
transformed_goal.pose.orientation.w = 0
self.group.set_pose_target(transformed_goal)
plan = self.group.plan()
self.group.execute(plan)
class Arm(object):
"""Arm controls the robot's arm.
Joint space control:
joints = ArmJoints()
# Fill out joint states
arm = fetch_api.Arm()
arm.move_to_joints(joints)
"""
def __init__(self):
self._joint_client = actionlib.SimpleActionClient(
JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction)
self._joint_client.wait_for_server(rospy.Duration(10))
self._move_group_client = actionlib.SimpleActionClient(
MOVE_GROUP_ACTION_SERVER, MoveGroupAction)
self._move_group_client.wait_for_server(rospy.Duration(10))
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self._tf_listener = TransformListener()
def move_to_joints(self, joint_state):
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names.extend(ArmJoints.names())
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions.extend(joint_state.values())
point.time_from_start = rospy.Duration(TIME_FROM_START)
goal.trajectory.points.append(point)
self._joint_client.send_goal(goal)
self._joint_client.wait_for_result(rospy.Duration(10))
def move_to_joint_goal(self,
joints,
allowed_planning_time=10.0,
execution_timeout=rospy.Duration(15.0),
group_name='arm',
num_planning_attempts=1,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
joints: A list of (name, value) for the arm joints.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_joint_goal(*zip(*joints))
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.planner_id = ''
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
# success = self._move_group_client.wait_for_result(
# rospy.Duration(execution_timeout))
success = self._move_group_client.wait_for_result(
execution_timeout)
if not success:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def move_to_pose(self,
pose_stamped,
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
orientation_constraint=None,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
orientation_constraint: moveit_msgs/OrientationConstraint. An
orientation constraint for the entire path.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_pose_goal(pose_stamped)
if orientation_constraint is not None:
goal_builder.orientation_constraint = orientation_constraint
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def move_to_pose_with_seed(self,
pose_stamped,
seed_state=None,
trajectory_constraint=[],
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
orientation_constraint=None,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper.
seed_state: sensor_msgs/JointState. The start joint state
to search for solution.
trajectory_constraint: array of moveit_msgs/Constraints. The trajectory
constraint.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
orientation_constraint: moveit_msgs/OrientationConstraint. An
orientation constraint for the entire path.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_pose_goal(pose_stamped)
if orientation_constraint is not None:
goal_builder.add_path_orientation_constraint(
orientation_constraint)
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
if seed_state:
goal_builder.start_state.joint_state = seed_state
if trajectory_constraint:
goal_builder.add_trajectory_orientation_constraint(
trajectory_constraint)
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def straight_move_to_pose(self, group, plan):
"""Moves the end-effector to a pose in a straight line.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
plan: the plan for the arm to follow
Returns:
string describing the error if an error occurred, else None.
"""
# Execute path
result = group.execute(plan, wait=True)
if not result:
return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN)
else:
return None
def straight_move_to_pose_check(self,
group,
pose_stamped,
ee_step=0.025,
jump_threshold=2.0,
avoid_collisions=True):
"""Checks if we can move the end-effector to a pose in a straight line.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the
gripper.
ee_step: float. The distance in meters to interpolate the path.
jump_threshold: float. The maximum allowable distance in the arm's
configuration space allowed between two poses in the path. Used to
prevent "jumps" in the IK solution.
avoid_collisions: bool. Whether to check for obstacles or not.
Returns:
the generated plan if the arm can move ina straight line, else None.
"""
# Transform pose into planning frame
self._tf_listener.waitForTransform(pose_stamped.header.frame_id,
group.get_planning_frame(),
rospy.Time.now(),
rospy.Duration(1.0))
try:
pose_transformed = self._tf_listener.transformPose(
group.get_planning_frame(), pose_stamped)
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr('Unable to transform pose from frame {} to {}'.format(
pose_stamped.header.frame_id, group.get_planning_frame()))
return moveit_error_string(
MoveItErrorCodes.FRAME_TRANSFORM_FAILURE)
# Compute path
plan, fraction = group.compute_cartesian_path(
[group.get_current_pose().pose, pose_transformed.pose], ee_step,
jump_threshold, avoid_collisions)
if fraction < 1:
return None
else:
return plan
def check_pose(self,
pose_stamped,
allowed_planning_time=10.0,
group_name='arm',
tolerance=0.01):
return self.move_to_pose(pose_stamped,
allowed_planning_time=allowed_planning_time,
group_name=group_name,
tolerance=tolerance,
plan_only=True)
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: A list of (name, value) for the arm joints if the IK solution
was found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
joint_state = response.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
joints.append((name, position))
return joints
def compute_fk(self, joint_state):
"""Computes forward kinematics for the given joint state.
Args:
joint_state: sensor_msgs/JointState.
Returns: A geometry_msgs/PoseStamped of the wrist position, False otherwise.
"""
request = GetPositionFKRequest()
request.header.frame_id = 'base_link'
request.fk_link_names = ['wrist_roll_link']
request.robot_state.joint_state = joint_state
response = self._compute_fk(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
return response.pose_stamped
def get_cartesian_path(self,
group,
start_joint_state,
waypoints,
ee_step=0.025,
jump_threshold=2.0,
avoid_collisions=True):
"""Returns a robot trajectory which passes through all the waypoints specified.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
start_joint_state: sensor_msgs/JointState.
waypoints: geometry_msgs/Pose[]. Waypoints to pass through.
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the
gripper.
ee_step: float. The distance in meters to interpolate the path.
jump_threshold: float. The maximum allowable distance in the arm's
configuration space allowed between two poses in the path. Used to
prevent "jumps" in the IK solution.
avoid_collisions: bool. Whether to check for obstacles or not.
Returns: A RobotTrajectory to follow, False otherwise.
"""
# request = GetCartesianPathRequest()
# request.header.frame_id = 'base_link'
# request.start_state.joint_state = start_joint_state
# request.group_name = ARM_GROUP_NAME
# request.waypoints = waypoints
# request.max_step = 0.025
# request.jump_threshold = 2.0
# request.avoid_collisions = True
# response = self._get_cartesian_path(request)
# error_str = moveit_error_string(response.error_code.val)
# success = error_str == 'SUCCESS'
# if not success:
# return False
# return response.solution
# Compute path
plan, fraction = group.compute_cartesian_path(waypoints, ee_step,
jump_threshold,
avoid_collisions)
if fraction < 1:
return None
else:
return plan
def execute_trajectory(self, group, trajectory):
"""Moves the end-effector along the given trajectory.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
trajectory: the RobotTrajectory to follow.
Return:
string describing the error if an error occurred, else None.
"""
result = group.execute(trajectory, wait=True)
if not result:
return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN)
else:
return None
def cancel_all_goals(self):
self._move_group_client.cancel_all_goals()
self._joint_client.cancel_all_goals()
class MoveItGoalBuilder(object):
"""Builds a MoveGroupGoal.
Example:
# To do a reachability check from the current robot pose.
builder = MoveItGoalBuilder()
builder.set_pose_goal(pose_stamped)
builder.allowed_planning_time = 5
builder.plan_only = True
goal = builder.build()
# To move to a current robot pose with a few options changed.
builder = MoveItGoalBuilder()
builder.set_pose_goal(pose_stamped)
builder.replan = True
builder.replan_attempts = 10
goal = builder.build()
Here are the most common class attributes you might set before calling
build(), and their default values:
allowed_planning_time: float=10. How much time to allow the planner,
in seconds.
group_name: string='arm'. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int=1. How many times to compute the same plan (most
planners are randomized). The shortest plan will be used.
plan_only: bool=False. Whether to only compute the plan but not execute it.
replan: bool=False. Whether to come up with a new plan if there was an
error executing the original plan.
replan_attempts: int=5. How many times to do replanning.
replay_delay: float=1. How long to wait in between replanning, in seconds.
tolerance: float=0.01. The tolerance, in meters for the goal pose.
"""
def __init__(self):
self.allowed_planning_time = 10.0
self.fixed_frame = 'base_link'
self.gripper_frame = 'wrist_roll_link'
self.group_name = 'arm'
self.planning_scene_diff = moveit_msgs.msg.PlanningScene()
self.planning_scene_diff.is_diff = True
self.planning_scene_diff.robot_state.is_diff = True
self.look_around = False
self.max_acceleration_scaling_factor = 0
self.max_velocity_scaling_factor = 0
self.num_planning_attempts = 1
self.plan_only = False
self.planner_id = 'RRTConnectkConfigDefault'
self.replan = False
self.replan_attempts = 5
self.replan_delay = 1
self.start_state = moveit_msgs.msg.RobotState()
self.start_state.is_diff = True
self.tolerance = 0.01
self._orientation_contraints = []
self._pose_goal = None
self._joint_names = None
self._joint_positions = None
self._tf_listener = TransformListener()
def set_pose_goal(self, pose_stamped):
"""Sets a pose goal.
Pose and joint goals are mutually exclusive. The most recently set goal
wins.
Args:
pose_stamped: A geometry_msgs/PoseStamped.
"""
self._pose_goal = pose_stamped
self._joint_names = None
self._joint_positions = None
def set_joint_goal(self, joint_names, joint_positions):
"""Set a joint-space planning goal.
Args:
joint_names: A list of strings. The names of the joints in the goal.
joint_positions: A list of floats. The joint angles to achieve.
"""
self._joint_names = joint_names
self._joint_positions = joint_positions
self._pose_goal = None
def add_path_orientation_contraint(self, o_constraint):
"""Adds an orientation constraint to the path.
Args:
o_constraint: A moveit_msgs/OrientationConstraint.
"""
self._orientation_contraints.append(copy.deepcopy(o_constraint))
self.planner_id = 'RRTConnectkConfigDefault'
def build(self, tf_timeout=rospy.Duration(5.0)):
"""Builds the goal message.
To set a pose or joint goal, call set_pose_goal or set_joint_goal
before calling build. To add a path orientation constraint, call
add_path_orientation_constraint first.
Args:
tf_timeout: rospy.Duration. How long to wait for a TF message. Only
used with pose goals.
Returns: moveit_msgs/MoveGroupGoal
"""
goal = MoveGroupGoal()
# Set start state
goal.request.start_state = copy.deepcopy(self.start_state)
# Set goal constraint
if self._pose_goal is not None:
self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
self.fixed_frame,
rospy.Time.now(), tf_timeout)
try:
pose_transformed = self._tf_listener.transformPose(
self.fixed_frame, self._pose_goal)
except (tf.LookupException, tf.ConnectivityException):
return None
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self.fixed_frame
c1.position_constraints[0].link_name = self.gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [self.tolerance * self.tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(self._pose_goal.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self.fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = self.gripper_frame
c1.orientation_constraints[
0].absolute_x_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_y_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_z_axis_tolerance = self.tolerance
c1.orientation_constraints[0].weight = 1.0
goal.request.goal_constraints.append(c1)
elif self._joint_names is not None:
c1 = Constraints()
for i in range(len(self._joint_names)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = self._joint_names[i]
c1.joint_constraints[i].position = self._joint_positions[i]
c1.joint_constraints[i].tolerance_above = self.tolerance
c1.joint_constraints[i].tolerance_below = self.tolerance
c1.joint_constraints[i].weight = 1.0
goal.request.goal_constraints.append(c1)
# Set path constraints
goal.request.path_constraints.orientation_constraints = self._orientation_contraints
# Set trajectory constraints
# Set planner ID (name of motion planner to use)
goal.request.planner_id = self.planner_id
# Set group name
goal.request.group_name = self.group_name
# Set planning attempts
goal.request.num_planning_attempts = self.num_planning_attempts
# Set planning time
goal.request.allowed_planning_time = self.allowed_planning_time
# Set scaling factors
goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor
# Set planning scene diff
goal.planning_options.planning_scene_diff = copy.deepcopy(
self.planning_scene_diff)
# Set is plan only
goal.planning_options.plan_only = self.plan_only
# Set look around
goal.planning_options.look_around = self.look_around
# Set replanning options
goal.planning_options.replan = self.replan
goal.planning_options.replan_attempts = self.replan_attempts
goal.planning_options.replan_delay = self.replan_delay
return goal
class ClosestPersonDetector(object):
"""
Merges the outputs from the leg_tracker package with the face_detector
package from WillowGarage. Then from the combined data, it publishes the
closest person.
"""
def __init__(self):
# Internal parameters
self.publish_rate = rospy.get_param("~publish_rate", 10.0)
self.listener = TransformListener()
self.desired_pose_frame = rospy.get_param("~desired_pose_frame",
"base_link")
self.position_match_threshold = rospy.get_param(
"~position_match_threshold", 1.0)
# Variables to keep track of state
self.closest_person = None
self.leg_detections = []
self.closest_person_lock = Lock()
self.leg_detections_lock = Lock()
# Internal helpers
self.person_face_distance_func = lambda A, B: np.sqrt(
(A.pose.position.x - B.pos.x)**2 +
(A.pose.position.y - B.pos.y)**2 +
(1.2 - B.pos.z)**2 # A person's face is about 4ft from the floor
)
self.leg_detection_is_closest_face = lambda detected_person: (
self.closest_person is not None and self.closest_person.
detection_context.pose_source == DetectionContext.POSE_FROM_FACE
and self.closest_person.id == detected_person.id)
# The subscribers and publishers
self.face_sub = rospy.Subscriber(
"face_detector/people_tracker_measurements_array",
PositionMeasurementArray, self.face_callback)
self.leg_sub = rospy.Subscriber("people_tracked", PersonArray,
self.leg_callback)
self.closest_person_pub = rospy.Publisher('~closest_person',
Person,
queue_size=10)
# Debug
self.debug_enabled = rospy.get_param("~debug", False)
if self.debug_enabled:
self.debug_pub1 = rospy.Publisher("~debug/1", Marker, queue_size=1)
self.debug_pub2 = rospy.Publisher("~debug/2", Marker, queue_size=1)
def leg_callback(self, msg):
closest_distance = np.inf
closest_person = None
# Iterate over the people and find the closest
with self.leg_detections_lock:
self.leg_detections = []
for detected_person in msg.people:
detected_pose = PoseStamped(header=msg.header,
pose=detected_person.pose)
try:
detected_pose = self.listener.transformPose(
self.desired_pose_frame, detected_pose)
except ExtrapolationException as e:
continue
# Add the person to the list of leg_detections
detected_person = Person(header=detected_pose.header,
id=str(detected_person.id),
pose=detected_pose.pose)
detected_person.detection_context.pose_source = DetectionContext.POSE_FROM_LEGS
with self.leg_detections_lock:
self.leg_detections.append(detected_person)
# If this is the closest person, save them. However, if this person
# has the same ID as the person with a face, prefer that
person_distance = np.sqrt(detected_pose.pose.position.x**2 +
detected_pose.pose.position.y**2)
if self.leg_detection_is_closest_face(
detected_person) or person_distance < closest_distance:
closest_distance = person_distance
closest_person = detected_person
# If the leg detection is the closest face, then disable new
# closest detections associations
if self.leg_detection_is_closest_face(detected_person):
closest_distance = -np.inf
# Debug
if self.debug_enabled and closest_person is not None:
marker = Marker(header=closest_person.header,
ns="debug",
id=1,
type=Marker.SPHERE,
action=Marker.ADD)
marker.pose = closest_person.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.debug_pub2.publish(marker)
# Acquire a lock to the people and update the closest person's position
# We don't want to be staring at feet...
with self.closest_person_lock:
if closest_person is None:
self.closest_person = None
elif self.closest_person is None or self.closest_person.id != closest_person.id:
self.closest_person = closest_person
else: # self.closest_person.id == closest_person.id
self.closest_person.pose.position.x = closest_person.pose.position.x
self.closest_person.pose.position.y = closest_person.pose.position.y
def face_callback(self, msg):
# Iterate through the message and find the closest face
closest_face = None
closest_distance = np.inf
for face in msg.people:
pos = PointStamped(header=face.header, point=face.pos)
try:
pos = self.listener.transformPoint(self.desired_pose_frame,
pos)
except ExtrapolationException as e:
continue
# Find the closest face
distance = np.sqrt(pos.point.x**2 + pos.point.y**2 +
pos.point.z**2)
if distance < closest_distance:
closest_distance = distance
closest_face = face
closest_face.header = pos.header
closest_face.pos = pos.point
# Debug
if self.debug_enabled and closest_face is not None:
marker = Marker(header=closest_face.header,
ns="debug",
id=0,
type=Marker.SPHERE,
action=Marker.ADD)
marker.pose.position = closest_face.pos
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
self.debug_pub1.publish(marker)
# Now associate the closest face to the leg detection that it is closest
# to
closest_person = None
with self.leg_detections_lock:
for detected_person in self.leg_detections:
if closest_face is not None and self.person_face_distance_func(
detected_person,
closest_face) < self.position_match_threshold:
closest_person = detected_person
closest_person.pose.position = closest_face.pos
break
# Now check the distance between the closest face and the closest leg.
# If the distance exceeds the threshold, then don't associate the face
# with the leg
with self.closest_person_lock:
if closest_person is None or self.closest_person is None:
pass
else:
self.closest_person = closest_person
self.closest_person.detection_context.pose_source = DetectionContext.POSE_FROM_FACE
def spin(self):
"""
Run the detector
"""
# Publish the detected closest person at the desired frequency
rate = rospy.Rate(self.publish_rate)
while not rospy.is_shutdown():
# Otherwise, check to see if we should publish the latest detection
with self.closest_person_lock:
if self.closest_person is not None:
self.closest_person_pub.publish(self.closest_person)
# Sleep
rate.sleep()
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True)
self.move_group = MoveGroupInterface("left_arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server(rospy.Duration(15.0))
self.cubes = []
self.tf_listener = TransformListener()
# self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction)
# self.gripper_client.wait_for_server()
# rospy.loginfo("...connected.")
rospy.sleep(2.0)
def updateScene(self, remove_collision = False):
if remove_collision:
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
return
# find objects
self.cubes = []
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(15.0))
find_result = self.find_client.get_result()
# rospy.loginfo(find_result)
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
# insert objects to scene
idx = -1
print(find_result.objects)
# print(find_result.support_surfaces)
# for obj in find_result.objects:
# idx += 1
# print(idx)
# obj.object.name = "object%d" % idx
# self.scene.addSolidPrimitive(obj.object.name,
# obj.object.primitives[0],
# obj.object.primitive_poses[0],
# wait = False)
# self.cubes.append(obj.object.primitive_poses[0])
#
# for obj in find_result.support_surfaces:
# # extend surface to floor, and make wider since we have narrow field of view
# height = obj.primitive_poses[0].position.z
# obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
# 1.5, # wider
# obj.primitives[0].dimensions[2] + height]
# obj.primitive_poses[0].position.z += -height / 2.0
#
# # add to scene
# self.scene.addSolidPrimitive(obj.name,
# obj.primitives[0],
# obj.primitive_poses[0],
# wait = False)
#
# self.scene.waitForSync()
#
# # store for grasping
# self.objects = find_result.objects
# self.surfaces = find_result.support_surfaces
def getGraspableCube(self):
graspable = None
for obj in self.objects:
# need grasps
if len(obj.grasps) < 1:
continue
# check size
if obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07 or \
obj.object.primitives[0].dimensions[0] < 0.05 or \
obj.object.primitives[0].dimensions[0] > 0.07:
continue
# has to be on table
if obj.object.primitive_poses[0].position.z < 0.5:
continue
return obj.object, obj.grasps
# nothing detected
return None, None
def get_sequence(self, seq_list):
start = [self.objects[seq_list[0]].object, self.objects[seq_list[0]].grasps] or [None, None]
target = [self.objects[seq_list[1]].object, self.objects[seq_list[1]].grasps] or [None, None]
res = {'start': start, 'target': target}
return res
def get_transform_pose(self, pose):
point = PoseStamped()
point.header.frame_id = 'base_link'
point.header.stamp = rospy.Time()
point.pose = pose
return self.tf_listener.transformPose('/map', point).pose.position
def getPlaceLocation(self):
pass
def pick(self, block, grasps):
success, pick_result = self.pickplace.pick_with_retry(block.name,
grasps,
retries = 5,
support_name = block.support_surface,
scene = self.scene)
self.pick_result = pick_result
return success
def place(self, block, pose_stamped):
places = list()
l = PlaceLocation()
l.place_pose.pose = pose_stamped.pose
l.place_pose.header.frame_id = pose_stamped.header.frame_id
# copy the posture, approach and retreat from the grasp used
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 360/m degrees in yaw direction
m = 16 # number of possible place poses
pi = 3.141592653589
for i in range(0, m - 1):
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m)
places.append(copy.deepcopy(l))
success, place_result = self.pickplace.place_with_retry(block.name,
places,
scene = self.scene,
retries = 5)
# print(success)
return success
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
# pose =[ 1.58, -0.056, -0.01, -1.31, -0.178, -0.13, 0.16]
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 0.0
gripper_goal.command.position = 0.09
self.gripper_client.send_goal(gripper_goal)
self.gripper_client.wait_for_result(rospy.Duration(5))
start = rospy.Time.now()
while rospy.Time.now() - start <= rospy.Duration(10.0): # rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
return
def gripper_opening(self, opening = 0.09):
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 0.0
gripper_goal.command.position = opening
self.gripper_client.send_goal(gripper_goal)
self.gripper_client.wait_for_result(rospy.Duration(5))
class Arm(object):
"""Arm controls the robot's arm.
Joint space control:
joints = ArmJoints()
# Fill out joint states
arm = fetch_api.Arm()
arm.move_to_joints(joints)
"""
def __init__(self):
self._joint_client = actionlib.SimpleActionClient(
JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction)
self._joint_client.wait_for_server(rospy.Duration(10))
self._move_group_client = actionlib.SimpleActionClient(
MOVE_GROUP_ACTION_SERVER, MoveGroupAction)
self._move_group_client.wait_for_server(rospy.Duration(10))
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._tf_listener = TransformListener()
self.tuck_pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
def tuck(self):
"""
Uses motion-planning to tuck the arm within the footprint of the base.
:return: a string describing the error, or None if there was no error
"""
return self.move_to_joint_goal(zip(ArmJoints.names(), self.tuck_pose))
def tuck_unsafe(self):
"""
TUCKS BUT DOES NOT PREVENT SELF-COLLISIONS, WHICH ARE HIGHLY LIKELY.
Don't use this unless you have prior knowledge that the arm can safely return
to tucked from its current configuration. Most likely, you should only use this
method in simulation, where the arm can clip through the base without issue.
:return:
"""
return self.move_to_joints(ArmJoints.from_list(self.tuck_pose))
def move_to_joints(self, joint_state):
"""
Moves to an ArmJoints configuration
:param joint_state: an ArmJoints instance to move to
:return:
"""
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names.extend(ArmJoints.names())
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions.extend(joint_state.values())
point.time_from_start = rospy.Duration(TIME_FROM_START)
goal.trajectory.points.append(point)
self._joint_client.send_goal(goal)
self._joint_client.wait_for_result(rospy.Duration(10))
def move_to_joint_goal(self,
joints,
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
joints: A list of (name, value) for the arm joints.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_joint_goal(*zip(*joints))
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.planner_id = ''
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
success = self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
if not success:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def move_to_pose(self,
pose_stamped,
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
orientation_constraint=None,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
pose: geometry_msgs/PoseStamped. The goal pose for the gripper.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
orientation_constraint: moveit_msgs/OrientationConstraint. An
orientation constraint for the entire path.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_pose_goal(pose_stamped)
if orientation_constraint is not None:
goal_builder.add_path_orientation_contraint(orientation_constraint)
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def straight_move_to_pose(self,
group,
pose_stamped,
ee_step=0.025,
jump_threshold=2.0,
avoid_collisions=True):
"""Moves the end-effector to a pose in a straight line.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the
gripper.
ee_step: float. The distance in meters to interpolate the path.
jump_threshold: float. The maximum allowable distance in the arm's
configuration space allowed between two poses in the path. Used to
prevent "jumps" in the IK solution.
avoid_collisions: bool. Whether to check for obstacles or not.
Returns:
string describing the error if an error occurred, else None.
"""
# Transform pose into planning frame
self._tf_listener.waitForTransform(pose_stamped.header.frame_id,
group.get_planning_frame(),
rospy.Time.now(),
rospy.Duration(1.0))
try:
pose_transformed = self._tf_listener.transformPose(
group.get_planning_frame(), pose_stamped)
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr('Unable to transform pose from frame {} to {}'.format(
pose_stamped.header.frame_id, group.get_planning_frame()))
return moveit_error_string(
MoveItErrorCodes.FRAME_TRANSFORM_FAILURE)
# Compute path
plan, fraction = group.compute_cartesian_path(
[group.get_current_pose().pose, pose_transformed.pose], ee_step,
jump_threshold, avoid_collisions)
if fraction < 1 and fraction > 0:
rospy.logerr('Only able to compute {}% of the path'.format(
fraction * 100))
if fraction == 0:
return moveit_error_string(MoveItErrorCodes.PLANNING_FAILED)
# Execute path
result = group.execute(plan, wait=True)
if not result:
return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN)
else:
return None
def check_pose(self,
pose_stamped,
allowed_planning_time=10.0,
group_name='arm',
tolerance=0.01):
return self.move_to_pose(pose_stamped,
allowed_planning_time=allowed_planning_time,
group_name=group_name,
tolerance=tolerance,
plan_only=True)
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: A list of (name, value) for the arm joints if the IK solution
was found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
joint_state = response.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
joints.append((name, position))
return joints
def cancel_all_goals(self):
self._move_group_client.cancel_all_goals()
self._joint_client.cancel_all_goals()
class Transformer:
def __init__(self):
self.__listener = TransformListener()
def __del__(self):
pass
def transform_to(self, pose_target, target_frame="/odom_combined"):
'''
Transforms the pose_target into the target_frame.
:param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2
:param target_frame: goal frame id
:return: transformed object
'''
if pose_target.header.frame_id == target_frame:
return pose_target
odom_pose = None
i = 0
while odom_pose is None and i < 10:
try:
#now = rospy.Time.now()
#self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4))
if type(pose_target) is CollisionObject:
i = 0
new_co = deepcopy(pose_target)
for cop in pose_target.primitive_poses:
p = PoseStamped()
p.header = pose_target.header
p.pose.orientation = cop.orientation
p.pose.position = cop.position
p = self.transform_to(p, target_frame)
if p is None:
return None
new_co.primitive_poses[i].position = p.pose.position
new_co.primitive_poses[i].orientation = p.pose.orientation
i += 1
new_co.header.frame_id = target_frame
return new_co
if type(pose_target) is PoseStamped:
odom_pose = self.__listener.transformPose(target_frame, pose_target)
break
if type(pose_target) is Vector3Stamped:
odom_pose = self.__listener.transformVector3(target_frame, pose_target)
break
if type(pose_target) is PointStamped:
odom_pose = self.__listener.transformPoint(target_frame, pose_target)
break
if type(pose_target) is PointCloud2:
odom_pose = self.__listener.transformPointCloud(target_frame, pose_target)
break
except Exception, e:
print "tf error:::", e
rospy.sleep(0.5)
i += 1
rospy.logdebug("tf fail nr. " + str(i))
if odom_pose is None:
rospy.logerr("FUUUUUUUUUUUUUU!!!! f*****g tf shit!!!!")
return odom_pose
class MoveGroupInterface(object):
'''
This class lets you interface with the movegroup node. It provides
the ability to execute the specified trajectory on the robot by
communicating to the movegroup node using services.
'''
def __init__(self,
group,
fixed_frame,
gripper_frame,
cart_srv,
listener=None,
plan_only=False):
self._group = group
self._fixed_frame = fixed_frame
self._gripper_frame = gripper_frame # a.k.a end-effector frame
self._action = actionlib.SimpleActionClient('move_group',
MoveGroupAction)
self._traj_action = actionlib.SimpleActionClient(
'execute_trajectory', ExecuteTrajectoryAction)
self._cart_service = rospy.ServiceProxy(cart_srv, GetCartesianPath)
try:
self._cart_service.wait_for_service(timeout=3)
except:
rospy.logerr("Timeout waiting for Cartesian Planning Service!!")
self._action.wait_for_server()
if listener == None:
self._listener = TransformListener()
else:
self._listener = listener
self.plan_only = plan_only
self.planner_id = None
self.planning_time = 15.0
def get_move_action(self):
return self._action
def moveToJointPosition(self,
joints,
positions,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm to set of joint position goals
:param joints: joint names for which the target position
is specified.
:param positions: target joint positions
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type joints: list of string element type
:type positions: list of float element type
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_scene_diff", "planning_time", "plan_only",
"start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToJointPosition: unsupported argument: %s",
arg)
# Create goal
g = MoveGroupGoal()
# 1. fill in workspace_parameters
# 2. fill in start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in goal_constraints
c1 = Constraints()
for i in range(len(joints)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = joints[i]
c1.joint_constraints[i].position = positions[i]
c1.joint_constraints[i].tolerance_above = tolerance
c1.joint_constraints[i].tolerance_below = tolerance
c1.joint_constraints[i].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in path constraints
# 5. fill in trajectory constraints
# 6. fill in planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in group name
g.request.group_name = self._group
# 8. fill in number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
try:
g.planning_options.planning_scene_diff = kwargs[
"planning_scene_diff"]
except KeyError:
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
def moveToPose(self,
pose_stamped,
gripper_frame,
tolerance=0.01,
wait=True,
**kwargs):
'''
Move the arm, based on a goal pose_stamped for the end effector.
:param pose_stamped: target pose to which we want to move
specified link to
:param gripper_frame: frame/link which we want to move
to the specified target.
:param tolerance: allowed tolerance in the final joint positions.
:param wait: if enabled, makes the fuctions wait until the
target joint position is reached
:type pose_stamped: ros message object of type PoseStamped
:type gripper_frame: string
:type tolerance: float
:type wait: bool
'''
# Check arguments
supported_args = ("max_velocity_scaling_factor", "planner_id",
"planning_time", "plan_only", "start_state")
for arg in kwargs.keys():
if not arg in supported_args:
rospy.loginfo("moveToPose: unsupported argument: %s", arg)
# Create goal
g = MoveGroupGoal()
pose_transformed = self._listener.transformPose(
self._fixed_frame, pose_stamped)
# 1. fill in request workspace_parameters
# 2. fill in request start_state
try:
g.request.start_state = kwargs["start_state"]
except KeyError:
g.request.start_state.is_diff = True
# 3. fill in request goal_constraints
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self._fixed_frame
c1.position_constraints[0].link_name = gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [tolerance * tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(pose_transformed.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self._fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = gripper_frame
c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance
c1.orientation_constraints[0].weight = 1.0
g.request.goal_constraints.append(c1)
# 4. fill in request path constraints
# 5. fill in request trajectory constraints
# 6. fill in request planner id
try:
g.request.planner_id = kwargs["planner_id"]
except KeyError:
if self.planner_id:
g.request.planner_id = self.planner_id
# 7. fill in request group name
g.request.group_name = self._group
# 8. fill in request number of planning attempts
try:
g.request.num_planning_attempts = kwargs["num_attempts"]
except KeyError:
g.request.num_planning_attempts = 1
# 9. fill in request allowed planning time
try:
g.request.allowed_planning_time = kwargs["planning_time"]
except KeyError:
g.request.allowed_planning_time = self.planning_time
# Fill in velocity scaling factor
try:
g.request.max_velocity_scaling_factor = kwargs[
"max_velocity_scaling_factor"]
except KeyError:
pass # do not fill in at all
# 10. fill in planning options diff
g.planning_options.planning_scene_diff.is_diff = True
g.planning_options.planning_scene_diff.robot_state.is_diff = True
# 11. fill in planning options plan only
try:
g.planning_options.plan_only = kwargs["plan_only"]
except KeyError:
g.planning_options.plan_only = self.plan_only
# 12. fill in other planning options
g.planning_options.look_around = False
g.planning_options.replan = False
# 13. send goal
self._action.send_goal(g)
if wait:
self._action.wait_for_result()
result = self._action.get_result()
return processResult(result)
else:
rospy.loginfo('Failed while waiting for action result.')
return False
def followCartesian(
self,
way_points,
way_point_frame,
max_step,
jump_threshold=0,
link_name=None, #usually it is Gripper Frame
start_state=None, #of type moveit robotstate
avoid_collisions=True):
'''
Movegroup-based cartesian path Control.
:param way_points: waypoints that the robot needs to track
:param way_point_frame: the frame in which the waypoints are given.
:param max_step: resolution (m) of the interpolation
on the cartesian path
:param jump_treshold: a distance in joint space that, if exceeded between
consecutive points, is interpreted as a jump in IK solutions.
:param link_name: frame or link name for which cartesian trajectory
should be followed
:param start_state: robot start state of cartesian trajectory
:param avoid_collisions: if enabled, produces collision free cartesian
path
:type way_points: list of ros message objests of type "Pose"
:type way_point_frame: string
:type max_step: float
:type jump_threshold: float
:type link_name: string
:type start_state: moveit_msgs/RobotState
:type avoid_collisions: bool
'''
req = GetCartesianPathRequest()
req.header.stamp = rospy.Time.now()
req.header.frame_id = way_point_frame
req.group_name = self._group
req.waypoints = way_points
req.max_step = max_step
req.jump_threshold = jump_threshold
req.avoid_collisions = avoid_collisions
if start_state is None:
req.start_state.is_diff = True
else:
req.start_state = start_state
if link_name is not None:
req.link_name = link_name
result = self._cart_service(req)
rospy.loginfo("Cartesian plan for %f fraction of path",
result.fraction)
if len(result.solution.joint_trajectory.points) < 1:
rospy.logwarn('No motion plan found. No execution attempted')
return False
rospy.loginfo('Executing Cartesian Plan...')
# 13. send Trajectory
action_req = ExecuteTrajectoryGoal()
action_req.trajectory = result.solution
self._traj_action.send_goal(action_req)
try:
self._traj_action.wait_for_result()
result = self._traj_action.get_result()
return processResult(result)
except:
rospy.logerr('Failed while waiting for action result.')
return False
def setPlannerId(self, planner_id):
'''
Sets the planner_id used for all future planning requests.
:param planner_id: The string for the planner id, set to None to clear
'''
self.planner_id = str(planner_id)
def setPlanningTime(self, time):
'''
Set default planning time to be used for future planning request.
'''
self.planning_time = time
