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