class MoveitPlanner: def __init__(self, manipulator_side = 'left'): # Enable the robot at start and disable at the end BaxterRobot().EnableBaxterRobot() rospy.on_shutdown(BaxterRobot().DisableBaxterRobot) # Test Open Both Grippers and Close Them # baxter_manipulator = BaxterManipulatorAndGripper() # baxter_manipulator.SetupBaxterManipualtor() # baxter_manipulator.MoveBaxterManipulatorToNeutralPose() # baxter_manipulator.SetupBaxterGrippers() # baxter_manipulator.OpenGripper('left') # baxter_manipulator.OpenGripper('right') # rospy.sleep(3.0) # baxter_manipulator.CloseGripper('left') # baxter_manipulator.CloseGripper('right') # Referenceing baxter arm self.__manipulator_name = manipulator_side self.limb = MoveGroupCommander("{}_arm".format(manipulator_side)) self.limb.set_end_effector_link("{}_gripper".format(manipulator_side)) # Baxter Model configuration with MoveIt not configured to control its gripper, # thus using Baxter SDK API gripper class baxter_manipulator = BaxterManipulatorAndGripper() baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side) self.limb.set_planner_id("RRTConnectKConfigDefualt") self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01)
class GenericDualArmClient(): _safe_kinematicsolver = "RRTConnectkConfigDefault" def __init__(self, *args, **kwargs): larm_name = args[0] rarm_name = args[1] # "arm_right" for Motoman SDA10F self.r_arm = MoveGroupCommander(rarm_name) self.r_arm.set_planner_id(self._safe_kinematicsolver) def move_rarm_shift_forward(self, axis, value): ''' Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f ''' self.r_arm.shift_pose_target(axis, value) self.r_arm.plan() self.r_arm.go() def move_rarm_fixedpose_forward(self): tpose = Pose() #TODO Currently the following position may work with SDA10F but not with NXO tpose.position.x = 0.599 tpose.position.y = -0.379 tpose.position.z = 1.11 self.r_arm.set_pose_target(tpose) self.r_arm.plan() self.r_arm.go()
def callback(states): scene = PlanningSceneInterface() robot = RobotCommander() arm = MoveGroupCommander("manipulator") arm.set_planner_id("RRTConnectkConfigDefault") arm.set_num_planning_attempts(20) arm.allow_replanning(True) pose = copy.deepcopy(states.pose[-1]) temp = tf.transformations.euler_from_quaternion( (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) quaternion = tf.transformations.quaternion_from_euler( math.pi, temp[1], temp[2]) pose.position.z += 0.22 pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] print pose arm.set_pose_target(pose) move_plan = arm.plan() i = 0 while (not move_plan.joint_trajectory.joint_names): move_plan = arm.plan() i += 1 if (i == 5): break state = arm.execute(move_plan)
class MoveItArm: """ Represents Baxter's Arm in MoveIt! world. Represents a hand of Baxter through MoveIt! Group. Some reusable code has been packed together into this class to make the code more readable. """ def __init__(self, side_name): """Will configure and initialise Baxter's hand.""" self._side_name = side_name # This is the reference to Baxter's arm. self.limb = MoveGroupCommander("{}_arm".format(side_name)) self.limb.set_end_effector_link("{}_gripper".format(side_name)) # Unfortunetly, MoveIt was not able to make the gripper work, neither # did was able to find a very good pose using Forward Kinematics, # so instead the Baxter SDK is used here to do these two jobs. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self._limb = Limb(side_name) # This solver seems to be better for finding solution among obstacles self.limb.set_planner_id("RRTConnectkConfigDefault") # Error tollerance should be as low as possible for better accuracy. self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01) def __str__(self): """ String representation of the arm. String representation of the arm, either 'left' or 'right' string will be returned. Useful when the string representation of the arm is needed, like when accessing the IK solver. """ return self._side_name def is_left(self): """Will return True if this is the left arm, false otherwise.""" return True if self._side_name == "left" else False def is_right(self): """Will return True if this is the right arm, false otherwise.""" return True if self._side_name == "right" else False def open_gripper(self): """Will open Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.open(block=True) def close_gripper(self): """Will close Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.close(block=True)
def get_move_group_commander(group): ''' Gets the move_group_commander for this process. ''' global _mgc_dict with _mgc_dict_creation_lock: if not group in _mgc_dict: _mgc_group = MoveGroupCommander(group) _mgc_group.set_planner_id('RRTConnectkConfigDefault') _mgc_dict[group] = _mgc_group add_ground() return _mgc_dict[group]
def get_move_group_commander(group): ''' Gets the move_group_commander for this process. ''' global _mgc_dict with _mgc_dict_creation_lock: if not group in _mgc_dict: _mgc_group = MoveGroupCommander(group) _mgc_group.set_planner_id('RRTConnectkConfigDefault') _mgc_dict[group] = _mgc_group add_ground() return _mgc_dict[group]
def setUpClass(self): rospy.init_node('test_moveit') rospy.sleep(5) # intentinally wait for starting up hrpsys self.robot = RobotCommander() # TODO: Read groups from SRDF file ideally. for mg_attr in self._MOVEGROUP_ATTRS: mg = MoveGroupCommander(mg_attr[0]) # Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170 mg.set_planner_id(self._KINEMATICSOLVER_SAFE) # Append MoveGroup instance to the MoveGroup attribute list. mg_attr.append(mg)
class MoveitWorkshopExercise(object): def __init__(self): rospy.init_node('commander_example', anonymous=True) self.group_l = MoveGroupCommander("left_arm") self.group_r = MoveGroupCommander("right_arm") self.group_l.set_planner_id('RRTConnectkConfigDefault') self.group_r.set_planner_id('RRTConnectkConfigDefault') def exercise1(self): pass def exercise2(self): pass def exercise3(self): pass
def main(): rospy.init_node('arm_start_up', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") left_arm = MoveGroupCommander("left_arm") right_arm.set_planner_id("RRTConnect:"); left_arm.set_planner_id("RRTConnect:") # move to a random target #group.set_named_target("ahead") #group.go() #rospy.sleep(1) right_arm.set_named_target("right_start") right_arm.go() rospy.sleep(1) left_arm.set_named_target("left_start") left_arm.go() rospy.sleep(1)
class CalibrationMovements: def __init__(self, move_group_name, max_velocity_scaling=0.5, max_acceleration_scaling=0.5): #self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic self.mgc = MoveGroupCommander(move_group_name) self.mgc.set_planner_id("RRTConnectkConfigDefault") self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling) self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling) self.start_pose = self.mgc.get_current_pose() self.poses = [] self.current_pose_index = -1 self.fallback_joint_limits = [math.radians(90)] * 4 + [ math.radians(90) ] + [math.radians(180)] + [math.radians(350)] if len(self.mgc.get_active_joints()) == 6: self.fallback_joint_limits = self.fallback_joint_limits[1:] def compute_poses_around_current_state(self, angle_delta, translation_delta): self.start_pose = self.mgc.get_current_pose() basis = np.eye(3) pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave final_rots = [] for qd in quaternion_deltas: final_rots.append(list(qd)) # TODO: clean up pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave for qd in quaternion_deltas: final_rots.append(list(qd)) final_poses = [] for rot in final_rots: fp = deepcopy(self.start_pose) ori = fp.pose.orientation combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot) fp.pose.orientation = Quaternion(*combined_rot) final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.x += translation_delta / 2 final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.x -= translation_delta / 2 final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.y += translation_delta final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.y -= translation_delta final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.z += translation_delta / 3 final_poses.append(fp) self.poses = final_poses self.current_pose_index = -1 def check_poses(self, joint_limits): if len(self.fallback_joint_limits) == 6: joint_limits = joint_limits[1:] for fp in self.poses: self.mgc.set_pose_target(fp) plan = self.mgc.plan() if len(plan.joint_trajectory.points ) == 0 or CalibrationMovements.is_crazy_plan( plan, joint_limits): return False return True def plan_to_start_pose(self): return self.plan_to_pose(self.start_pose) def plan_to_pose(self, pose): self.mgc.set_start_state_to_current_state() self.mgc.set_pose_target(pose) plan = self.mgc.plan() return plan def execute_plan(self, plan): if CalibrationMovements.is_crazy_plan(plan, self.fallback_joint_limits): raise RuntimeError("got crazy plan!") self.mgc.execute(plan) @staticmethod def rot_per_joint(plan, degrees=False): np_traj = np.array([p.positions for p in plan.joint_trajectory.points]) if len(np_traj) == 0: raise ValueError np_traj_max_per_joint = np_traj.max(axis=0) np_traj_min_per_joint = np_traj.min(axis=0) ret = abs(np_traj_max_per_joint - np_traj_min_per_joint) if degrees: ret = [math.degrees(j) for j in ret] return ret @staticmethod def is_crazy_plan(plan, max_rotation_per_joint): abs_rot_per_joint = CalibrationMovements.rot_per_joint(plan) if (abs_rot_per_joint > max_rotation_per_joint).any(): return True else: return False
#!/usr/bin/env python import rospy import geometry_msgs.msg from moveit_commander import MoveGroupCommander if __name__ == '__main__': rospy.init_node('commander_example', anonymous=True) group = MoveGroupCommander("right_arm") group.set_planner_id('RRTConnectkConfigDefault') pose_rarm_curr = group.get_current_pose('RARM_JOINT5_Link') rospy.loginfo('Current pose: {}'.format(pose_rarm_curr)) # right hand move to approx. 2cm ahead pose_target = geometry_msgs.msg.Pose() pose_target.position.x = 0.392 pose_target.position.y = -0.182 pose_target.position.z = 0.0748 pose_target.orientation.y = -0.708 pose_target.orientation.w = 0.706 rospy.loginfo("set target to {}".format(pose_target)) group.set_pose_target(pose_target) plan = group.plan() rospy.loginfo("plan is {}".format(plan)) ret = group.go() rospy.loginfo("Command ran ... {}".format(ret)) pose_rarm_curr = group.get_current_pose('RARM_JOINT5_Link') rospy.loginfo('Pose after: {}'.format(pose_rarm_curr))
class TestMoveit(unittest.TestCase): _MOVEGROUP_MAIN = 'manipulator' _KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault' @classmethod def setUpClass(self): rospy.init_node('test_moveit_vs060') self.robot = RobotCommander() self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN) # Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170 self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE) self._movegroups = sorted(['manipulator', 'manipulator_flange']) self._joints_movegroup_main = sorted( ['j1', 'j2', 'j3', 'j4', 'j5', 'flange']) @classmethod def tearDownClass(self): True # TODO impl something meaningful def _set_sample_pose(self): ''' @return: Pose() with some values populated in. ''' pose_target = Pose() pose_target.orientation.x = 0.00 pose_target.orientation.y = 0.00 pose_target.orientation.z = -0.20 pose_target.orientation.w = 0.98 pose_target.position.x = 0.18 pose_target.position.y = 0.18 pose_target.position.z = 0.94 return pose_target def _plan(self): ''' Run `clear_pose_targets` at the beginning. @rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html ''' self._mvgroup.clear_pose_targets() pose_target = self._set_sample_pose() self._mvgroup.set_pose_target(pose_target) plan = self._mvgroup.plan() rospy.loginfo(' plan: '.format(plan)) return plan def test_list_movegroups(self): '''Check if the defined move groups are loaded.''' self.assertEqual(self._movegroups, sorted(self.robot.get_group_names())) def test_list_activejoints(self): '''Check if the defined joints in a move group are loaded.''' self.assertEqual(self._joints_movegroup_main, sorted(self._mvgroup.get_active_joints())) def test_plan(self): '''Evaluate plan (RobotTrajectory)''' plan = self._plan() # TODO Better way to check the plan is valid. # Currently the following checks if the number of waypoints is not zero, # which (hopefully) indicates that a path is computed. self.assertNotEqual(0, plan.joint_trajectory.points) def test_planandexecute(self): ''' Evaluate Plan and Execute works. Currently no value checking is done (, which is better to be implemented) ''' self._plan() # TODO Better way to check the plan is valid. # The following checks if plan execution was successful or not. self.assertTrue(self._mvgroup.go()) def test_set_pose_target(self): ''' Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py ''' self._plan() p = [0.1, -0.35, 0.4] pose = PoseStamped( header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'), pose=Pose(position=Point(*p), orientation=Quaternion( *quaternion_from_euler(1.57, 0, 1.57, 'sxyz')))) self._mvgroup.set_pose_target(pose) self.assertTrue(self._mvgroup.go() or self._mvgroup.go()) def test_planning_scene(self): ''' Check if publish_simple_scene.py is working ''' rospy.wait_for_service('/get_planning_scene', timeout=20) get_planning_scene = rospy.ServiceProxy("/get_planning_scene", GetPlanningScene) collision_objects = [] # wait for 10 sec time_start = rospy.Time.now() while not collision_objects and (rospy.Time.now() - time_start).to_sec() < 10.0: world = get_planning_scene( PlanningSceneComponents(components=PlanningSceneComponents. WORLD_OBJECT_NAMES)).scene.world collision_objects = world.collision_objects rospy.loginfo("collision_objects = " + str(world.collision_objects)) rospy.sleep(1) self.assertTrue(world.collision_objects != [], world)
class RobotArm: """ A class used to represent the arm of a robot using both "Moveit" API and Baxter SDK API. """ def __init__(self, side_name): """ Constructor. @param side_name: which arm side we are refering to 'left' or 'right'. """ self.__side_name = side_name # MoveIt interface to a group of arm joints. # Either left arm joint group or right arm joint group. self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name)) # MoveIt limb setting. self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name)) self.moveit_limb.set_planner_id('RRTConnectKConfigDefault') self.moveit_limb.set_goal_position_tolerance(0.01) self.moveit_limb.set_goal_orientation_tolerance(0.01) # MoveIt does not provide support for Baxter gripper. # Thus we use Baxter SDK gripper instead. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self.baxter_sdk_limb = Limb(side_name) def __str__(self): """ Built-in function to return a printable string representing the arm used in either print(object) or str(object). """ return str(self.__side_name) def open_gripper(self): """ Command the robot arm end-effector i.e. the gripper to open. """ self.gripper.open(block=True) def close_gripper(self): """ Command the robot arm end-effector i.e. the gripper to close. """ self.gripper.close(block=True) def is_left_arm(self): """ Return true if the arm is corresponding to the left arm. """ return self.__side_name == 'left' def is_right_arm(self): """ Return true if the arm is corresponding to the right arm. """ return self.__side_name == 'right'
class MoveIt(object): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.add_table() # self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.arm = MoveGroupCommander("arm") # self.arm.set_goal_joint_tolerance(0.1) self.gripper = MoveGroupCommander("gripper") # already default self.arm.set_planner_id("RRTConnectkConfigDefault") self.end_effector_link = self.arm.get_end_effector_link() self.arm.allow_replanning(True) self.arm.set_planning_time(5) self.transformer = tf.TransformListener() rospy.sleep(2) # allow some time for initialization of moveit def __del__(self): moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def _open_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _set_gripper_width(self, width): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _close_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [1.2, 1.2] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory # Template function for creating the Grasps def _create_grasp(self, x, y, z, roll, pitch, yaw): grasp = Grasp() # pre_grasp grasp.pre_grasp_posture = self._open_gripper() grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link grasp.pre_grasp_approach.direction.vector.z = 1.0 grasp.pre_grasp_approach.direction.vector.y = 0.0 grasp.pre_grasp_approach.direction.vector.x = 0.0 grasp.pre_grasp_approach.min_distance = 0.05 grasp.pre_grasp_approach.desired_distance = 0.1 # grasp grasp.grasp_posture = self._close_gripper() grasp.grasp_pose.pose.position.x = x grasp.grasp_pose.pose.position.y = y grasp.grasp_pose.pose.position.z = z q = quaternion_from_euler(roll, pitch, yaw) grasp.grasp_pose.pose.orientation.x = q[0] grasp.grasp_pose.pose.orientation.y = q[1] grasp.grasp_pose.pose.orientation.z = q[2] grasp.grasp_pose.pose.orientation.w = q[3] grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # post_grasp grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link grasp.post_grasp_retreat.direction.vector.z = -1.0 grasp.post_grasp_retreat.direction.vector.x = 0.0 grasp.post_grasp_retreat.direction.vector.y = 0.0 grasp.post_grasp_retreat.min_distance = 0.05 grasp.post_grasp_retreat.desired_distance = 0.25 return [grasp] # Template function, you can add parameters if needed! def grasp(self, x, y, z, roll, pitch, yaw, width): self.add_object('object', [0.37, -0.24, 0.1, math.pi, 0., 0.], [0.1, 0.1, 0.1]) grasps = self._create_grasp(x, y, z, roll, pitch, yaw) result = self.arm.pick('object', grasps) self.remove_object() if result == MoveItErrorCodes.SUCCESS: print 'Success grasp' return True else: print 'Failed grasp' return False def open_fingers(self): self.gripper.set_joint_value_target([0.0, 0.0]) self.gripper.go(wait=True) rospy.sleep(2.0) def close_fingers(self): self.gripper.set_joint_value_target([1.3, 1.3]) self.gripper.go(wait=True) rospy.sleep(2.0) def move_to(self, x, y, z, roll, pitch, yaw, frame_id="m1n6s200_link_base"): q = quaternion_from_euler(roll, pitch, yaw) pose = PoseStamped() pose.header.frame_id = frame_id pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = True success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def remove_object(self, object='object'): self.scene.remove_attached_object(self.end_effector_link, object) self.scene.remove_world_object(object) rospy.loginfo("Object removed") def add_object(self, name, pose, size): object_pose = PoseStamped() object_pose.header.frame_id = "m1n6s200_link_base" object_pose.pose.position.x = pose[0] object_pose.pose.position.y = pose[1] object_pose.pose.position.z = pose[2] q = quaternion_from_euler(*pose[3:]) object_pose.pose.orientation.x = q[0] object_pose.pose.orientation.y = q[1] object_pose.pose.orientation.z = q[2] object_pose.pose.orientation.w = q[3] self.scene.add_box(name, object_pose, size) def add_table(self): self.add_object('table', [0, 0, -0.005, 0, 0, 0], (2, 2, 0.01))
class MoveIt(object): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.arm = MoveGroupCommander("arm") # self.arm.set_goal_joint_tolerance(0.1) self.gripper = MoveGroupCommander("gripper") # already default self.arm.set_planner_id("RRTConnectkConfigDefault") self.end_effector_link = self.arm.get_end_effector_link() self.arm.allow_replanning(True) self.arm.set_planning_time(5) self.transformer = tf.TransformListener() rospy.sleep(2) # allow some time for initialization of moveit def __del__(self): moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def _open_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _close_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [1.2, 1.2] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory # Template function for creating the Grasps def _create_grasps(self, x, y, z, z_max, rotation): grasps = [] # You can create multiple grasps and add them to the grasps list grasp = Grasp() # create a new grasp # Set the pre grasp posture (the fingers) grasp.pre_grasp_posture = self._open_gripper() # Set the grasp posture (the fingers) grasp.grasp_posture = self._close_gripper() # Set the position of where to grasp grasp.grasp_pose.pose.position.x = x grasp.grasp_pose.pose.position.y = y grasp.grasp_pose.pose.position.z = z # Set the orientation of the end effector q = quaternion_from_euler(math.pi, 0.0, rotation) grasp.grasp_pose.pose.orientation.x = q[0] grasp.grasp_pose.pose.orientation.y = q[1] grasp.grasp_pose.pose.orientation.z = q[2] grasp.grasp_pose.pose.orientation.w = q[3] grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # Set the pre_grasp_approach grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link grasp.pre_grasp_approach.direction.vector.z = 1.0 grasp.pre_grasp_approach.direction.vector.y = 0.0 grasp.pre_grasp_approach.direction.vector.x = 0.0 grasp.pre_grasp_approach.min_distance = 0.05 grasp.pre_grasp_approach.desired_distance = 0.1 # # Set the post_grasp_approach grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link grasp.post_grasp_retreat.direction.vector.z = -1.0 grasp.post_grasp_retreat.direction.vector.x = 0.0 grasp.post_grasp_retreat.direction.vector.y = 0.0 grasp.post_grasp_retreat.min_distance = 0.05 grasp.post_grasp_retreat.desired_distance = 0.25 grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm) grasps.append( grasp ) # add all your grasps in the grasps list, MoveIT will pick the best one for z_offset in np.arange(z + 0.02, z_max, 0.01): new_grasp = copy.deepcopy(grasp) new_grasp.grasp_pose.pose.position.z = z_offset grasps.append(new_grasp) return grasps # Template function, you can add parameters if needed! def grasp(self, x, y, z, z_max, rotation, size): print '******************* grasp' # Object distance: obj_dist = np.linalg.norm(np.asarray((x, y, z))) if obj_dist > 0.5: rospy.loginfo( "Object too far appart ({} m), skipping pick".format(obj_dist)) return False # Add collision object, easiest to name the object, "object" object_pose = PoseStamped() object_pose.header.frame_id = "m1n6s200_link_base" object_pose.pose.position.x = x object_pose.pose.position.y = y object_pose.pose.position.z = z q = quaternion_from_euler(math.pi, 0.0, rotation) object_pose.pose.orientation.x = q[0] object_pose.pose.orientation.y = q[1] object_pose.pose.orientation.z = q[2] object_pose.pose.orientation.w = q[3] self.scene.add_box("object", object_pose, size) rospy.sleep(0.5) self.clear_octomap() rospy.sleep(1.0) # Create and return grasps # z += size[2]/2 # Focus on the top of the object only # z += size[2]/2 + 0.02 # Focus on the top of the object only grasps = self._create_grasps(x, y, z, z_max, rotation) print '******************************************************************************' result = self.arm.pick( 'object', grasps) # Perform pick on "object", returns result print '******************************************************************************' # self.move_to(x, y, z + 0.15, rotation) if result == MoveItErrorCodes.SUCCESS: print 'Success grasp' return True else: print 'Failed grasp' return False def clear_object(self, x, y, z, z_max, rotation, size): print '******************* clear_object' self.move_to_waypoint() success = self.grasp(x, y, z, z_max, rotation, size) if success: self.move_to_waypoint() success = self.move_to_drop_zone() if success: print 'success move to drop zone' else: print 'failed move to drop zone' self.open_fingers() self.remove_object() rospy.sleep(1.0) self.close_fingers() return success def open_fingers(self): print '******************* open_fingers' self.gripper.set_joint_value_target([0.0, 0.0]) self.gripper.go(wait=True) rospy.sleep(2.0) def close_fingers(self): print '******************* close_fingers' self.gripper.set_joint_value_target([1.3, 1.3]) self.gripper.go(wait=True) rospy.sleep(2.0) def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"): print '******************* move_to' q = quaternion_from_euler(math.pi, 0.0, rotation) pose = PoseStamped() pose.header.frame_id = frame_id pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def move_to_waypoint(self): print '******************* move_to_waypoint' return self.move_to(0.35, 0, 0.25, 1.57) def rtb(self): print '******************* rtb' self.move_to_drop_zone() # pose = PoseStamped() # pose.header.frame_id = 'base_footprint' # pose.pose.position.x = -0.191258927921 # pose.pose.position.y = 0.1849306168113 # pose.pose.position.z = 0.813729734732 # pose.pose.orientation.x = -0.934842026356 # pose.pose.orientation.y = 0.350652799078 # pose.pose.orientation.z = -0.00168532388516 # pose.pose.orientation.w = 0.0557688079539 # # self.arm.set_pose_target(pose, self.end_effector_link) # plan = self.arm.plan() # self.arm.go(wait=True) # self.arm.stop() # self.arm.clear_pose_targets() def move_to_drop_zone(self): print '******************* move_to_drop_zone' pose = PoseStamped() pose.header.frame_id = "m1n6s200_link_base" pose.pose.position.x = 0.2175546259709541 pose.pose.position.y = 0.18347985269448372 pose.pose.position.z = 0.16757751444136426 pose.pose.orientation.x = 0.6934210704552356 pose.pose.orientation.y = 0.6589390059796749 pose.pose.orientation.z = -0.23223137602833943 pose.pose.orientation.w = -0.17616808290725341 self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def print_position(self): pose = self.arm.get_current_pose() self.transformer.waitForTransform("m1n6s200_link_base", "base_footprint", rospy.Time.now(), rospy.Duration(10)) eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose) orientation = eef_pose.pose.orientation orientation = [ orientation.x, orientation.y, orientation.z, orientation.w ] euler = euler_from_quaternion(orientation) print "z:", eef_pose.pose.position.x print "y:", eef_pose.pose.position.y print "z:", eef_pose.pose.position.z print "yaw (degrees):", math.degrees(euler[2]) def remove_object(self): self.scene.remove_attached_object(self.end_effector_link, "object") self.scene.remove_world_object("object") rospy.loginfo("Object removed")
from moveit_commander import RobotCommander, MoveGroupCommander from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown from geometry_msgs.msg import PoseStamped from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation from trajectory_msgs.msg import JointTrajectoryPoint if __name__=='__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) GRIPPER_FRAME = 'right_gripper_link' scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") right_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") scene.remove_attached_object(GRIPPER_FRAME, "part") #rospy.logwarn("cleaning world") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("open") #right_gripper.go() rospy.sleep(3)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault") left_arm.set_planner_id("KPIECEkConfigDefault") rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class UR5_Gripped_Manipulator: """ Class Attributes Robot : Robot Commander Object Scene : Planning Scene Interface Object (current scene) Man : MoveGroupCommander object to control manipulator EEF : MoveGroupCommander object to control endeffector Target_poses : Pose list of created target objects Picked : List of part indexes to ensure to picked non-picked ones """ def __init__(self, manip_name="manipulator", eef_name="endeffector"): self.robot = RobotCommander() self.man = MoveGroupCommander(manip_name) self.eef = MoveGroupCommander(eef_name) self.target_poses = [] self.picked = [] """ ----------------Function Name: clean_scene---------------- Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method """ def clear_poses(self): for i in xrange(len(self.target_poses)): self.target_poses.pop() for i in xrange(len(self.picked)): self.picked.pop() """ ----------------Function Name: clean_scene---------------- Definition : Clears all non-attached objects from planning scene Group : MoveGroupCommander object to control given group """ def default_state_gripper(self, grp): rospy.loginfo("Openning Gripper") joint_vals = grp.get_current_joint_values() joint_vals[0] = 0.0 grp.set_joint_value_target(joint_vals) init_plan = grp.plan() return grp.execute(init_plan) """ ----------------Function Name: closed_state_gripper---------------- Definition : Function that opens gripper and detachs the gripped object Obj : Name of the Object that is needed to detach """ def closed_state_gripper(self, curr_scene, obj): rospy.loginfo("Closing Gripper") def convert(width): return 0.77 - width / 0.15316 * 0.77 width = curr_scene.get_objects([obj])[obj].primitives[0].dimensions[0] width = convert(width) now = self.robot.endeffector.get_current_joint_values()[0] cnt = 0 while abs(now - width) > 0.05: now = self.robot.endeffector.get_current_joint_values()[0] cnt = cnt + 1 tmp = width - abs(now - width) / 2.0 self.robot.endeffector.set_joint_value_target('finger_joint', tmp) self.robot.endeffector.go() rospy.sleep(0.05) if cnt == 7: break rospy.sleep(1.0) ret = self.robot.manipulator.attach_object(obj) return ret """ ----------------Function Name: set_mid_state---------------- Definition : Motion planning function for defined mid state for manipulator, can be used for enhancing the performance if needed Group : MoveGroupCommander object to control given group """ def set_mid_state(self, group): rospy.loginfo("Going to Mid-State") pos = PoseStamped() pos.header.frame_id = self.robot.get_planning_frame() pos.pose.position.x = 0.45358 pos.pose.position.y = 0.188518 pos.pose.position.z = 0.473063 pos.pose.orientation.x = 0.0004738 pos.pose.orientation.y = 1 pos.pose.orientation.z = 0 pos.pose.orientation.w = 0.000155894 group.set_pose_target(pos) move_plan = group.plan() while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() return group.execute(move_plan) """ ----------------Function Name: add_object---------------- Name : Object Name Pose : Pose of the Object (x,y,z,xo,yo,zo,wo) Dimension : Dimensions of the Obhect (Tuple) (d1,d2,d3), Type : Box(0),Sphere(1) d1 is radius for sphere i.e typ==1, """ def add_object(self, name, x, y, z, xo=0.0, yo=0.0, zo=0.0, wo=0.0, d1=0.1, d2=0.1, d3=0.1, curr_scene=None, typ=0): pos = PoseStamped() pos.header.frame_id = self.robot.get_planning_frame() pos.pose.position.x = x pos.pose.position.y = y pos.pose.position.z = z pos.pose.orientation.x = xo pos.pose.orientation.y = yo pos.pose.orientation.z = zo pos.pose.orientation.w = wo rospy.loginfo("ADDING OBJECT") if (typ == 0): curr_scene.add_box(name, pos, (d1, d2, d3)) elif (typ == 1): curr_scene.add_sphere(name, pos, d1) else: print("ERROR in Type") return pos """ ----------------Function Name: clean_scene---------------- Definition: Clears all non-attached objects from planning scene """ def clean_scene(self, curr_scene): rospy.loginfo("Clearing the Scene") curr_scene.remove_world_object() """ ----------------Function Name: create_environment---------------- Definition : Creates the simulation environment with non-target collision objects, can be edited to create different default environment """ def create_environment(self, curr_scene): self.add_object(name="wall", x=0.0, y=0.8, z=0.5, d1=0.1, d2=0.35, d3=1, curr_scene=curr_scene, typ=0) self.add_object(name="wall_2", x=0.0, y=-0.8, z=0.5, d1=0.1, d2=0.35, d3=1, curr_scene=curr_scene, typ=0) self.add_object(name="table", x=0.0, y=0.0, z=-0.05, d1=2, d2=2, d3=0.0001, curr_scene=curr_scene, typ=0) """ ----------------Function Name: check_targets---------------- Definition : Creates given number of objects within the distance provided. It prevents collision object overlapping. It returns the pose list of the objects Number : Number of collision objects required to spawn Distance : Required minimum distance between each collision objects """ def check_targets(self, curr_scene, number, distance): rospy.loginfo("Creating Boxes!") def al(typ, x=0.0): if typ == 'x': return random.uniform(0.35, 0.65) elif typ == "y": rang = math.sqrt(0.7**2 - x**2) return random.uniform(-rang, rang) hemme = [] dims = [] cnt = 0 while len(hemme) != number: if cnt == 200: hemme = [] cnt = 0 cnt = cnt + 1 dim = (random.uniform(0.08, 0.12), random.uniform(0.08, 0.10), random.uniform(0.05, 0.2)) quaternion = tf.transformations.quaternion_from_euler( 0.0, 0.0, random.uniform(-math.pi, math.pi)) box = PoseStamped() box.header.frame_id = self.robot.get_planning_frame() box.pose.position.z = (-0.04 + (dim[2] / 2)) box.pose.position.x = al('x') box.pose.position.y = al('y', box.pose.position.x) box.pose.orientation.x = quaternion[0] box.pose.orientation.y = quaternion[1] box.pose.orientation.z = quaternion[2] box.pose.orientation.w = quaternion[3] flag = 1 for i in hemme: if abs(i.pose.position.x - box.pose.position.x) < distance or abs( i.pose.position.y - box.pose.position.y) < distance: flag = 0 if flag == 0: continue hemme.append(box) dims.append(dim) cnt = 0 names = [] for i in xrange(len(hemme)): now = "part" + str(cnt) cnt = cnt + 1 names.append(now) self.add_object(name=now, x=hemme[i].pose.position.x, y=hemme[i].pose.position.y, z=hemme[i].pose.position.z, xo=hemme[i].pose.orientation.x, yo=hemme[i].pose.orientation.y, zo=hemme[i].pose.orientation.z, wo=hemme[i].pose.orientation.w, d1=dims[i][0], d2=dims[i][1], d3=dims[i][2], curr_scene=curr_scene, typ=0) print("End Check!") return hemme """ ----------------Function Name: pick_object---------------- Definition : It moves the given group i.e robot to the collision object whose index is given and picks that object. Group : MoveGroupCommander object to control given group Part_index : Index of the target object to obtain pose """ def pick_object(self, curr_scene, group, part_index): rospy.loginfo("Pick Operation starts!") gripped_object = curr_scene.get_objects(["part" + str(part_index) ])["part" + str(part_index)] pos = copy.deepcopy(self.target_poses[part_index]) temp = tf.transformations.euler_from_quaternion( (pos.pose.orientation.x, pos.pose.orientation.y, pos.pose.orientation.z, pos.pose.orientation.w)) quaternion = tf.transformations.quaternion_from_euler( math.pi, temp[1], temp[2]) pos.pose.position.z += 0.17 + ( gripped_object.primitives[0].dimensions[2] / 2.0) pos.pose.orientation.y = 1 group.set_pose_target(pos) move_plan = group.plan() i = 0 while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() i += 1 if (i == 500): break state = group.execute(move_plan) rospy.loginfo("Execute operation for Object is %s" % str(part_index)) if (state): self.closed_state_gripper(curr_scene, "part" + str(part_index)) rospy.sleep(1.0) self.place_object(curr_scene, group, part_index) return else: return """ ----------------Function Name: place_object---------------- Definition : It places the gripped object to the target location Group : MoveGroupCommander object to control given group Part_index : Index of the target object to obtain pose """ def place_object(self, curr_scene, group, part_index): pos = PoseStamped() def al(typ, x=0.0): if typ == 'x': return random.uniform(-0.35, -0.6) elif typ == 'y': rang = math.sqrt(0.75**2 - x**2) x = random.uniform(-rang, rang) while abs(x) < 0.15: x = random.uniform(-rang, rang) return x pos = PoseStamped() pos.header.frame_id = robot.get_planning_frame() pos.pose.position.x = al("x") pos.pose.position.y = al("y", pos.pose.position.x) #This line makes placing possible by setting a valid z position for gripped object pos.pose.position.z = -0.04 + 0.17 + ((curr_scene.get_attached_objects( ["part" + str(part_index) ])["part" + str(part_index)]).object.primitives[0].dimensions[2]) pos.pose.orientation.y = 1.0 group.set_pose_target(pos) move_plan = group.plan() while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() state = group.execute(move_plan) if (state): detached = group.detach_object("part" + str(part_index)) rospy.sleep(1.5) if (detached): curr_scene.remove_world_object("part" + str(part_index)) rospy.sleep(1.5) self.default_state_gripper(self.eef) self.picked.append(part_index) else: self.default_state_gripper(robot.endeffector) group.detach_object("part" + str(part_index)) rospy.sleep(2) """ ----------------Function Name: execute_simulation---------------- Definition : Creates the specified environment , generates motion plans and does the pick and place operation based on these plans. To visualize RViz is needed. Planner ID can be changed based on MoveIT's supported OMPL. """ def execute_simulation(self, curr_scene=None, num_of_objects=3): is_success = False #Reset State of the Gripper self.default_state_gripper(self.eef) #### #Reset the position of the Arm # Will be implemented if needed #### #Clean the scene self.clean_scene(curr_scene=curr_scene) #Create environment self.create_environment(curr_scene=curr_scene) #Create targets self.target_poses = self.check_targets(curr_scene, num_of_objects, 0.10) rospy.sleep(5) #Planner setup self.man.set_planner_id("RRTConnectkConfigDefault") self.man.set_num_planning_attempts(20) self.man.allow_replanning(True) #Pick and place every object for i in xrange(len(self.target_poses)): if i not in self.picked: self.pick_object(curr_scene=curr_scene, group=self.man, part_index=i) rospy.loginfo("END OF PICK AND PLACE") if (len(self.picked) == len(self.target_poses)): is_success = True self.clear_poses() return is_success """ ----------------Function Name: execute_simulation---------------- Definition : Executes the simulation num_attempts times. Stores the success rate and writes into the specified document Num_Attempts : Limiting number for executing simulation. (Default Value: 10) File_Name : File name (with path if needed) to write test results """ def test_simulation(self, curr_scene=None, num_attempts=10, num_of_objects=3, file_name="ur5_pick_place_test.txt"): success_rate = 0 file = open(file_name, "a+") file.write("Start Test \n") for case in xrange(1, num_attempts + 1): state = self.execute_simulation(curr_scene=curr_scene, num_of_objects=num_of_objects) if (state): success_rate += 1 rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(case)) file.write("Test Case %s is successfull" % str(case)) else: rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(case)) file.write("Test Case %s is failedl" % str(case)) file.write("Success: %" + str(((success_rate / (case - 1) * 100.0)))) file.write("----END OF TEST----") file.close() return
def graspobject(): # Connect to the right_arm move group right_arm = MoveGroupCommander('manipulator') right_arm.set_planner_id('RRTConnectkConfigDefault') # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame('base_link') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() #right_arm.set_end_effector_link(end_effector_link) #start_pose = Pose() #start_pose.position.x = 0.587 #start_pose.position.y = 0.034 #start_pose.position.z = -0.010 #start_pose.orientation.x =0.508 #start_pose.orientation.y = 0.508 #start_pose.orientation.z = -0.491 #start_pose.orientation.w = 0.491 #right_arm.set_pose_target(start_pose) #traj = right_arm.plan() #rospy.sleep(2) #right_arm.execute(traj) #rospy.sleep(2) global getkey print '回到初始位置结束' startclient = rospy.ServiceProxy('/detect_grasps/actionstart', npy) startclient(0) print 'call start over' sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspcallback) rate = rospy.Rate(200) while not rospy.is_shutdown(): if getkey==1: break rate.sleep() getkey=0 global grasps print '检测到抓取位置,开始执行' global ser ser.write(('0\n').encode()) rospy.sleep(2) print "-start movingA------------------------------------" pose = [] currentcamera=current_cameralinkpose() grasppose1,grasppose2= transformgrasp(currentcamera,grasps,0.24,0.15) start_pose = current_pose() print 'start_pose',start_pose pose.append(start_pose) pose.append(grasppose1) pose.append(grasppose2) #end_pose = copy.deepcopy(start_pose) # end_pose.position.x = grasps.approach.x # end_pose.position.y = grasps.approach.y # end_pose.position.z = grasps.approach.z catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态 print "-start movingB------------------------------------" print '抓' ser.write(('1\n').encode()) rospy.sleep(2) ############################################################## group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里 group_variable_values[1] = -1.5415833632098597 group_variable_values[2] = -1.2562201658831995 group_variable_values[3] = -1.8575199286090296 group_variable_values[4] = 1.572489857673645 group_variable_values[5] = 1.5713907480239868 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(1) right_arm.execute(plan2)#先移到观测姿态 group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =2.6649599075317383#机器人的观测姿态定义在这里 group_variable_values[1] = -1.493981663380758 group_variable_values[2] = -1.7679532209979456 group_variable_values[3] =-1.3932693640338343 group_variable_values[4] = 1.5719022750854492 group_variable_values[5] = 1.5657243728637695 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(2) right_arm.execute(plan2)#先移到观测姿态 print "-start movingB------------------------------------" print '笛卡尔插值到放置位置完成' ser.write(('0\n').encode()) rospy.sleep(2) # pose = [] #posecurrent=current_pose()#获取现有的机器人末端姿态 # pose.append(posecurrent) #start_pose = Pose()#似乎这里不能接受相对的移动 #start_pose= objectpose#获取目标末端姿态 # posecurrent.position.z =posecurrent.position.z -0.1 # pose.append(posecurrent) # print start_pose print "-------------------------------------" # print pose print "-------------------------------------" # catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态 #回到初始位置 group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里 group_variable_values[1] = -1.5415833632098597 group_variable_values[2] = -1.2562201658831995 group_variable_values[3] = -1.8575199286090296 group_variable_values[4] = 1.572489857673645 group_variable_values[5] = 1.5713907480239868 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(1) right_arm.execute(plan2)#先移到观测姿态 rospy.sleep(3) print '回到初始位置结束' startclient2 = rospy.ServiceProxy('/detect_grasps/actionover', npy) startclient2(0) print 'call start over'
# modify the pose p.pose.position.x = 0.72 p.pose.position.z = 0.05 # add the pen scene.add_mesh("pen",p,resourcepath+'objects/pen.dae') rospy.sleep(1) # print the existing groups robot = RobotCommander() print "Available groups: ",robot.get_group_names() # setup the arm group and its planner arm = MoveGroupCommander("arm") arm.set_start_state_to_current_state() arm.set_planner_id("RRTstarkConfigDefault") arm.set_planning_time(5.0) arm.detach_object("pen") # set the arm to a safe target arm.set_named_target("gamma") # plan and execute the motion arm.go() # setup the hand group and its planner hand = MoveGroupCommander("hand") hand.set_start_state_to_current_state() hand.set_planner_id("LBKPIECEkConfigDefault") hand.set_planning_time(10.0) # set the hand to a safe target
class SingleArmROS(SingleArmReal): """ A class for single arms with ROS-related functions. Args: cfgs (YACS CfgNode): configurations for the arm. moveit_planner (str): motion planning algorithm. eetool_cfg (dict): arguments to pass in the constructor of the end effector tool class. Attributes: max_vel (float): Maximum joint velocity, read in from MoveIt config file. Can be scaled with self.scale_motion(). max_acc (float): Maximum joint acceleration, read in from MoveIt config file. Can be scaled with self.scale_motion(). tf_listner (tf.TransformListener): ROS subscriber to the /tf topic. moveit_group (MoveGroupCommander): Internal interface to MoveIt! move_group, used to call motion planning functions. moveit_scene (MoveitScene): Internal interface to MoveIt! planning scene, used to add objects, remove objects, etc. from the motion planning environment. moveit_planner (str): The name of the motion planning algorithm being used for all motion planning requests. """ def __init__(self, cfgs, moveit_planner='RRTstarkConfigDefault', eetool_cfg=None): super(SingleArmROS, self).__init__(cfgs=cfgs, eetool_cfg=eetool_cfg) self.moveit_planner = moveit_planner self._gazebo_sim = rospy.get_param('sim') self._init_ros_consts() def set_jpos(self, position, joint_name=None, wait=True, *args, **kwargs): """ Method to send a joint position command to the robot (units in rad) Args: position (float or list or flattened np.ndarray): desired joint position(s) (shape: :math:`[DOF,]` if list, otherwise a single value). joint_name (str): If not provided, position should be a list and all actuated joints will be moved to specified positions. If provided, only specified joint will move. Defaults to None. wait (bool): whether position command should be blocking or non blocking. Defaults to True. Returns: bool: True if command was completed successfully, returns False if wait flag is set to False. """ position = copy.deepcopy(position) if joint_name is None: if len(position) != self.arm_dof: raise ValueError('position should contain %d elements if ' 'joint_name is not provided' % self.arm_dof) tgt_pos = position else: if not isinstance(position, numbers.Number): raise TypeError('position should be individual float value' ' if joint_name is provided') if joint_name not in self.arm_jnt_names_set: raise TypeError('Joint name [%s] is not in the arm' ' joint list!' % joint_name) else: tgt_pos = self.get_jpos() arm_jnt_idx = self.arm_jnt_names.index(joint_name) tgt_pos[arm_jnt_idx] = position self.moveit_group.set_joint_value_target(tgt_pos) success = self.moveit_group.go(tgt_pos, wait=wait) return success def set_jvel(self, velocity, joint_name=None, wait=False, *args, **kwargs): """ Set joint velocity command to the robot (units in rad/s). Args: velocity (float or list or flattened np.ndarray): list of target joint velocity value(s) (shape: :math:`[6,]` if list, otherwise a single value). joint_name (str, optional): If not provided, velocity should be list and all joints will be turned on at specified velocity. Defaults to None. wait (bool, optional): If True, block until robot reaches desired joint velocity value(s). Defaults to False. Returns: bool: True if command was completed successfully, returns False if wait flag is set to False. """ raise NotImplementedError def set_ee_pose(self, pos=None, ori=None, wait=True, *args, **kwargs): """ Set cartesian space pose of end effector. Args: pos (list or np.ndarray): Desired x, y, z positions in the robot's base frame to move to (shape: :math:`[3,]`). ori (list or np.ndarray, optional): It can be euler angles ([roll, pitch, yaw], shape: :math:`[4,]`), or quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`), or rotation matrix (shape: :math:`[3, 3]`). If it's None, the solver will use the current end effector orientation as the target orientation. wait (bool): wait until the motion completes. Returns: bool: Returns True is robot successfully moves to goal pose. """ if ori is None and pos is None: return True if ori is None: pose = self.get_ee_pose() # last index is euler angles quat = pose[1] else: quat = arutil.to_quat(ori) if pos is None: pose = self.get_ee_pose() pos = pose[0] pose = self.moveit_group.get_current_pose() pose.pose.position.x = pos[0] pose.pose.position.y = pos[1] pose.pose.position.z = pos[2] pose.pose.orientation.x = quat[0] pose.pose.orientation.y = quat[1] pose.pose.orientation.z = quat[2] pose.pose.orientation.w = quat[3] self.moveit_group.set_pose_target(pose) success = self.moveit_group.go(wait=wait) return success def move_ee_xyz(self, delta_xyz, eef_step=0.005, wait=True, *args, **kwargs): """ Move end effector in straight line while maintaining orientation. Args: delta_xyz (list or np.ndarray): Goal change in x, y, z position of end effector. eef_step (float, optional): Discretization step in cartesian space for computing waypoints along the path. Defaults to 0.005 (m). wait (bool, optional): True if robot should not do anything else until this goal is reached, or the robot times out. Defaults to True. Returns: bool: True if robot successfully reached the goal pose. """ ee_pos, ee_quat, ee_rot_mat, ee_euler = self.get_ee_pose() plan = moveit_cartesian_path(ee_pos, ee_quat, delta_xyz, self.moveit_group, eef_step) success = self.moveit_group.execute(plan, wait=wait) return success def get_jpos(self, joint_name=None): """ Gets the current joint position of the robot. Gets the value from the internally updated dictionary that subscribes to the ROS topic /joint_states. Args: joint_name (str, optional): If it's None, it will return joint positions of all the actuated joints. Otherwise, it will return the joint position of the specified joint. Returns: One of the following - float: joint position given joint_name. - list: joint positions if joint_name is None (shape: :math:`[DOF]`). """ self._j_state_lock.acquire() if joint_name is not None: if joint_name not in self.arm_jnt_names: raise TypeError('Joint name [%s] ' 'not recognized!' % joint_name) jpos = self._j_pos[joint_name] else: jpos = [] for joint in self.arm_jnt_names: jpos.append(self._j_pos[joint]) self._j_state_lock.release() return jpos def get_jvel(self, joint_name=None): """ Gets the current joint angular velocities of the robot. Gets the value from the internally updated dictionary that subscribes to the ROS topic /joint_states. Args: joint_name (str, optional): If it's None, it will return joint velocities of all the actuated joints. Otherwise, it will return the joint position of the specified joint. Returns: One of the following - float: joint velocity given joint_name. - list: joint velocities if joint_name is None (shape: :math:`[DOF]`). """ self._j_state_lock.acquire() if joint_name is not None: if joint_name not in self.arm_jnt_names: raise TypeError('Joint name [%s] not recognized!' % joint_name) jvel = self._j_vel[joint_name] else: jvel = [] for joint in self.arm_jnt_names: jvel.append(self._j_vel[joint]) self._j_state_lock.release() return jvel def get_ee_pose(self): """ Get current cartesian pose of the EE, in the robot's base frame, using ROS subscriber to the tf tree topic. Returns: 4-element tuple containing - np.ndarray: x, y, z position of the EE (shape: :math:`[3]`). - np.ndarray: quaternion representation ([x, y, z, w]) of the EE orientation (shape: :math:`[4]`). - np.ndarray: rotation matrix representation of the EE orientation (shape: :math:`[3, 3]`). - np.ndarray: euler angle representation of the EE orientation (roll, pitch, yaw with static reference frame) (shape: :math:`[3]`). """ pos, quat = get_tf_transform(self.tf_listener, self.cfgs.ARM.ROBOT_BASE_FRAME, self.cfgs.ARM.ROBOT_EE_FRAME) rot_mat = arutil.quat2rot(quat) euler_ori = arutil.quat2euler(quat) return np.array(pos), np.array(quat), rot_mat, euler_ori def get_ee_vel(self): """ Return the end effector's velocity. Returns: 2-element tuple containing - np.ndarray: translational velocity (vx, vy, vz) (shape: :math:`[3,]`). - np.ndarray: rotational velocity (wx, wy, wz) (shape: :math:`[3,]`). """ jpos = self.get_jpos() jvel = self.get_jvel() ee_vel = self.compute_fk_velocity(jpos, jvel, self.cfgs.ARM.ROBOT_EE_FRAME) return ee_vel[:3], ee_vel[3:] def scale_motion(self, vel_scale=1.0, acc_scale=1.0): """ Sets the maximum velocity and acceleration for the robot motion. Specified as a fraction from 0.0 - 1.0 of the maximum velocity and acceleration specified in the MoveIt joint limits configuration file. Args: vel_scale (float): velocity scale, Defaults to 1.0. acc_scale (float): acceleration scale, Defaults to 1.0. """ vel_scale = arutil.clamp(vel_scale, 0.0, 1.0) acc_scale = arutil.clamp(acc_scale, 0.0, 1.0) self.moveit_group.set_max_velocity_scaling_factor(vel_scale) self.moveit_group.set_max_acceleration_scaling_factor(acc_scale) self._motion_vel = self.max_vel * vel_scale self._motion_acc = self.max_acc * acc_scale def _init_ros_consts(self): """ Initialize constants """ moveit_commander.roscpp_initialize(sys.argv) self.moveit_group = MoveGroupCommander(self.cfgs.ARM.MOVEGROUP_NAME) self.moveit_group.set_planner_id(self.moveit_planner) self.moveit_group.set_planning_time(1.0) self.moveit_scene = MoveitScene() # read the joint limit (max velocity and acceleration) from the # moveit configuration file jnt_params = [] max_vels = [] max_accs = [] for arm_jnt in self.arm_jnt_names: jnt_param = self.cfgs.ROBOT_DESCRIPTION + \ '_planning/joint_limits/' + arm_jnt jnt_params.append(copy.deepcopy(jnt_param)) max_vels.append(rospy.get_param(jnt_param + '/max_velocity')) max_accs.append(rospy.get_param(jnt_param + '/max_acceleration')) self.max_vel = np.min(max_vels) self.max_acc = np.min(max_accs) self._j_pos = dict() self._j_vel = dict() self._j_torq = dict() self._j_state_lock = threading.RLock() self.tf_listener = tf.TransformListener() rospy.Subscriber(self.cfgs.ARM.ROSTOPIC_JOINT_STATES, JointState, self._callback_joint_states) def _callback_joint_states(self, msg): """ ROS subscriber callback for arm joint states Args: msg (sensor_msgs/JointState): Contains message published in topic. """ self._j_state_lock.acquire() for idx, name in enumerate(msg.name): if name in self.arm_jnt_names: if idx < len(msg.position): self._j_pos[name] = msg.position[idx] if idx < len(msg.velocity): self._j_vel[name] = msg.velocity[idx] if idx < len(msg.effort): self._j_torq[name] = msg.effort[idx] self._j_state_lock.release()
class Arm(): def __init__(self): self.p = Pose() self.gripper = MoveGroupCommander("onine_gripper") self.arm = MoveGroupCommander("onine_arm") self.arm.set_goal_tolerance(0.004) self.arm.allow_replanning(True) # self.arm.set_goal_position_tolerance(0.005) # self.arm.set_goal_orientation_tolerance(0.1) self.arm.set_num_planning_attempts(10) self.arm.set_planning_time(5) self.arm.set_planner_id("RRTkConfigDefault") def go(self, x, y, z, roll, pitch, yaw): self.p.position.x = x self.p.position.y = y self.p.position.z = z self.p.orientation = Quaternion( *quaternion_from_euler(roll, pitch, yaw)) self.arm.set_pose_target(self.p) os.system("rosservice call clear_octomap") rospy.loginfo("Moving to arm target") self.arm.go(wait=True) rospy.sleep(1) def get_valid_pose(self, x, y, z, distance): origin_translation = [0.095, 0.00, 0.00] delta_x = x - origin_translation[0] delta_y = y - origin_translation[1] theta = math.atan(delta_y / delta_x) grasp_yaw = theta grasp_x = x + (distance * math.cos(theta)) grasp_y = y + (distance * math.sin(theta)) return (grasp_x, grasp_y, z, grasp_yaw) def ready(self): self.arm.set_named_target("onine_ready") self.arm.go(wait=True) def home(self): self.arm.set_named_target("onine_home") self.arm.go(wait=True) def feed_pos(self): self.arm.set_named_target("feed_pos") self.arm.go(wait=True) def tilt_food(self): self.arm.set_named_target("tilt_food") self.arm.go(wait=True) def open_gripper(self): os.system("rostopic pub /onine_gripper std_msgs/Float64 0.085 -1") os.system("rosservice call clear_octomap") def close_gripper(self): os.system("rostopic pub /onine_gripper std_msgs/Float64 0.035 -1") os.system("rosservice call clear_octomap") def pickup_sim(self, x, y, z): # self.ready() self.open_gripper() (aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z + 0.15, 0.00) self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw) (aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z, 0.00) self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw) self.close_gripper()
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) right_arm.set_planner_id("KPIECEkConfigDefault") right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("right_gripper_open") #right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.40 p.pose.position.y = -0.0 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.37636 start_pose.pose.position.y = 0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
class MoveItObstaclesDemo: def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('manipulator') # 设置机械臂工作空间 self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100]) # 设置机械臂最大速度 self.arm.set_max_velocity_scaling_factor(value=0.1) # 获取终端link的名称 self.end_effector_link = self.arm.get_end_effector_link() rospy.loginfo('end effector link: {}'.format(self.end_effector_link)) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 # self.arm.allow_replanning(True) self.arm.set_num_planning_attempts(10) # self.arm.allow_looking(True) # 设置目标位置所使用的参考坐标系 self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(3) # 控制机械臂先回到初始化位置 self.arm.set_named_target('home') self.arm.go() rospy.sleep(2) def planning(self, start_point, end_point): """ 功能:动态避障 :param start_point: 起始点, type: dict :param end_point: 终点, type: dict :return: None """ # 移动机械臂到指定位置,并获取当前位姿数据为机械臂运动的起始位姿 if start_point: self.move_arm(p=start_point) self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose # 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中 # way_points = self.get_way_points(start_point, end_point) while True: print('set planner id: RRT') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('up') self.arm.go() rospy.sleep(5) print('set planner id: PRM') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('home') self.arm.go() rospy.sleep(5) # ------------------------------------------------------------------- def get_way_points(self, a, b): way_points = [] # plan 1 self.arm.set_named_target('up') traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") else: rospy.logerr("Trajectory is empty. Planning false!") self.arm.clear_pose_targets() # plan 2 # traj = self.moveit_planning(p=b) for i, p in enumerate(traj.joint_trajectory.points): # rospy.loginfo('waypoint: {}'.format(i)) if i%2 == 0: point = { 'x': p.positions[0], 'y': p.positions[1], 'z': p.positions[2], 'ox': p.positions[3], 'oy': p.positions[4], 'oz': p.positions[5] } way_points.append(point) rospy.loginfo('waypoint: \n {}'.format(way_points)) rospy.loginfo(len(way_points)) return way_points def moveit_planning(self, p): """ 使用moveit求解器规划路径 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start moveit planning...') target_pose = PoseStamped() target_pose.header.frame_id = self.reference_frame target_pose.pose.position.x = p['x'] target_pose.pose.position.y = p['y'] target_pose.pose.position.z = p['z'] if 'ox' in p.keys() and p['ox']: target_pose.pose.orientation.x = p['ox'] if 'oy' in p.keys() and p['oy']: target_pose.pose.orientation.y = p['oy'] if 'oz' in p.keys() and p['oz']: target_pose.pose.orientation.z = p['oz'] if 'ow' in p.keys() and p['ow']: target_pose.pose.orientation.w = p['ow'] # 传入一个PoseStamped # self.arm.set_pose_target(target_pose, self.end_effector_link) # 尝试直接传入一个列表 self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link) traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") return traj else: rospy.logerr("Trajectory is empty. Planning false!") def move_arm(self, p): """ 移动机械臂到p点 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start arm moving...') traj = self.moveit_planning(p) self.arm.execute(traj) # 设置当前位置为起始位置 self.arm.set_start_state_to_current_state() rospy.sleep(1) def stop_arm(self): """ 急停 :return: """ pass def exist_danger_obstacle(self): """ 环境中是否存在危险的障碍物 :return: True or False """ return False def get_obstacle_octomap(self): """ 获取环境的octomap信息 :return: """ pass
def __init__(self): # Initialize the move_group API print 'starting' moveit_commander.roscpp_initialize(sys.argv) # Connect to the right_arm move group right_arm = MoveGroupCommander('manipulator') right_arm.set_planner_id('RRTConnectkConfigDefault') # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame('base_link') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() #right_arm.set_end_effector_link(end_effector_link) # Get the current pose so we can add it as a waypoint group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里 group_variable_values[1] = -1.5415833632098597 group_variable_values[2] = -1.2562201658831995 group_variable_values[3] = -1.8575199286090296 group_variable_values[4] = 1.572489857673645 group_variable_values[5] = 1.5713907480239868 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(2) right_arm.execute(plan2)#先移到观测姿态 try: rate = rospy.Rate(10) while not rospy.is_shutdown(): print '.' graspobject( ) rate.sleep() except rospy.ROSInterruptException: pass
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node("moveit_py_demo", anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) right_arm.set_planner_id("KPIECEkConfigDefault") right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") # right_arm.set_named_target("r_start") # right_arm.go() # right_gripper.set_named_target("right_gripper_open") # right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table # p.pose.position.x = 0.42 # p.pose.position.y = -0.2 # p.pose.position.z = 0.3 # scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.40 p.pose.position.y = -0.0 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.37636 start_pose.pose.position.y = 0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
group.set_path_constraints(constraints) rospy.loginfo("Get Path Constraints:\n{}".format( group.get_path_constraints())) # Pose Target 2 rospy.loginfo("Start Pose Target 2") pose_target_2 = Pose() pose_target_2.position.x = 0.3 pose_target_2.position.y = -0.5 pose_target_2.position.z = 0.15 pose_target_2.orientation.x = 0.0 pose_target_2.orientation.y = -0.707 pose_target_2.orientation.z = 0.0 pose_target_2.orientation.w = 0.707 group.set_planner_id("RRTConnectkConfigDefault") group.allow_replanning(True) rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_2)) xyz = [ pose_target_2.position.x, pose_target_2.position.y, pose_target_2.position.z ] group.set_position_target(xyz) result_p = group.go() rospy.loginfo("Moving to the Position Executed... {}".format(result_p)) group.clear_path_constraints()
class TestPlanners(object): def __init__(self, group_id, planner): rospy.init_node('moveit_test_planners', anonymous=True) self.pass_list = [] self.fail_list = [] self.planner = planner self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(group_id) rospy.sleep(1) self.group.set_planner_id(self.planner) self.test_trajectories_empty_environment() self.test_waypoints() self.test_trajectories_with_walls_and_ground() for pass_test in self.pass_list: print(pass_test) for fail_test in self.fail_list: print(fail_test) def _add_walls_and_ground(self): # publish a scene p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = 0 # offset such that the box is below ground (to prevent collision with # the robot itself) p.pose.position.z = -0.11 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 self.scene.add_box("ground", p, (3, 3, 0.1)) p.pose.position.x = 0.4 p.pose.position.y = 0.85 p.pose.position.z = 0.4 p.pose.orientation.x = 0.5 p.pose.orientation.y = -0.5 p.pose.orientation.z = 0.5 p.pose.orientation.w = 0.5 self.scene.add_box("wall_front", p, (0.8, 2, 0.01)) p.pose.position.x = 1.33 p.pose.position.y = 0.4 p.pose.position.z = 0.4 p.pose.orientation.x = 0.0 p.pose.orientation.y = -0.707388 p.pose.orientation.z = 0.0 p.pose.orientation.w = 0.706825 self.scene.add_box("wall_right", p, (0.8, 2, 0.01)) p.pose.position.x = -0.5 p.pose.position.y = 0.4 p.pose.position.z = 0.4 p.pose.orientation.x = 0.0 p.pose.orientation.y = -0.707107 p.pose.orientation.z = 0.0 p.pose.orientation.w = 0.707107 self.scene.add_box("wall_left", p, (0.8, 2, 0.01)) # rospy.sleep(1) def _check_plan(self, plan): if len(plan.joint_trajectory.points) > 0: return True else: return False def _plan_joints(self, joints): self.group.clear_pose_targets() group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) plan = self.group.plan() return self._check_plan(plan) def test_trajectories_rotating_each_joint(self): # test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2] test_joint_values = [numpy.pi / 2.0] joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0] # Joint 4th is colliding with the hand # for joint in range(6): for joint in [0, 1, 2, 3, 5]: for value in test_joint_values: joints[joint] = value if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_rotating_each_joint, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_rotating_each_joint, " + self.planner + ", joints:" + str(joints)) def test_trajectories_empty_environment(self): # Up - Does not work with sbpl but it does with ompl joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # All joints up joints = [ -1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Down joints = [ -0.000348431194526, 0.397651011661, 0.0766181197394, -0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # left joints = [ 0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765, 0.146075718452, 0.00420656698366, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Front joints = [ 1.425279839, -0.110370375874, -1.52548746261, -1.50659865247, -1.42700242769, 3.1415450794, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Behind joints = [ 1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839, 0.694689321451, -0.390769365032, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Should fail because it is in self-collision joints = [ -0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181, 1.04235601797, -1.69730925867, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) def test_waypoints(self): # Start planning in a given joint position joints = [ -0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979, 0.344227622185, -3.03250264451, 0.0, 0.0 ] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:8] = joints current.joint_state.position = current_joints self.group.set_start_state(current) waypoints = [] initial_pose = self.group.get_current_pose().pose initial_pose.position.x = -0.301185959729 initial_pose.position.y = 0.517069787724 initial_pose.position.z = 1.20681710541 initial_pose.orientation.x = 0.0207499700474 initial_pose.orientation.y = -0.723943002716 initial_pose.orientation.z = -0.689528413407 initial_pose.orientation.w = 0.00515118111221 # start with a specific position waypoints.append(initial_pose) # first move it down wpose = geometry_msgs.msg.Pose() wpose.orientation = waypoints[0].orientation wpose.position.x = waypoints[0].position.x wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z - 0.20 waypoints.append(copy.deepcopy(wpose)) # second front wpose.position.y += 0.20 waypoints.append(copy.deepcopy(wpose)) # third side wpose.position.x -= 0.20 waypoints.append(copy.deepcopy(wpose)) # fourth return to initial pose wpose = waypoints[0] waypoints.append(copy.deepcopy(wpose)) (plan3, fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0) if not self._check_plan(plan3) and fraction > 0.8: self.fail_list.append("Failed: test_waypoints, " + self.planner) else: self.pass_list.append("Passed: test_waypoints, " + self.planner) def test_trajectories_with_walls_and_ground(self): self._add_walls_and_ground() # Should fail to plan: Goal is in collision with the wall_front joints = [ 0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275, 0.302143499777, 0.00130280337897, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) # Should fail to plan: Goal is in collision with the ground joints = [ 3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437, 0.000133883149666, -0.594498939239, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) # Goal close to right corner joints = [ 0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116, -1.3516255551, 2.8225061435, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed, test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) self.scene.remove_world_object("ground") self.scene.remove_world_object("wall_left") self.scene.remove_world_object("wall_right") self.scene.remove_world_object("wall_front")
def __init__(self, pickPos, placePos): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm pick_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the momitive group for the right gripper pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = pick_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) pick_arm.set_goal_position_tolerance(0.05) pick_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution pick_arm.allow_replanning(True) # Set the right arm reference frame pick_arm.set_pose_reference_frame(REFERENCE_FRAME) #pick_arm.set_planner_id("RRTConnectkConfigDefault") #pick_arm.set_planner_id("RRTstarkConfigDefault") pick_arm.set_planner_id("RRTstarkConfigDefault") # Allow 5 seconds per planning attempt pick_arm.set_planning_time(3) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name base_table_id = 'base_table' target_id = 'target' limit_table_id = 'limit_table' #tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(base_table_id) #scene.remove_world_object(table_id) #scene.remove_world_object(box1_id) #scene.remove_world_object(box2_id) scene.remove_world_object(target_id) #scene.remove_world_object(tool_id) scene.remove_world_object(limit_table_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Open the gripper to the neutral position # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # pick_gripper.go() rospy.sleep(1) # Set the height of the table off the ground # table_ground = 0.6 table_ground = 0.0 # Set the dimensions of the scene objects [l, w, h] base_table_size = [2, 2, 0.04] #table_size = [0.2, 0.7, 0.01] #box1_size = [0.1, 0.05, 0.05] #box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.055, 0.055, 0.12] target_radius = 0.03305 target_height = 0.12310 limit_table_size = [0.6, 1.8, 0.04] # Add a base table to the scene base_table_pose = PoseStamped() base_table_pose.header.frame_id = REFERENCE_FRAME base_table_pose.pose.position.x = 0.0 base_table_pose.pose.position.y = 0.0 base_table_pose.pose.position.z = -0.3 base_table_pose.pose.orientation.w = 1.0 scene.add_box(base_table_id, base_table_pose, base_table_size) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file pick_arm.set_named_target(ARM_HOME_POSE) pick_arm.go() rospy.sleep(1) # Add a table top and two boxes to the scene #table_pose = PoseStamped() #table_pose.header.frame_id = REFERENCE_FRAME #table_pose.pose.position.x = 0.5 #table_pose.pose.position.y = -0.4 #table_pose.pose.position.z = table_ground + table_size[2] / 2.0 #table_pose.pose.orientation.w = 1.0 #scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = pickPos.pose.position.x target_pose.pose.position.y = pickPos.pose.position.y target_pose.pose.position.z = pickPos.pose.position.z target_pose.pose.orientation.w = pickPos.pose.orientation.w # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) #scene.add_cylinder(target_id,target_pose,target_height,target_radius) #Add the limit_table object to the scene limit_table_pose = PoseStamped() limit_table_pose.header.frame_id = REFERENCE_FRAME limit_table_pose.pose.position.x = 0.58 limit_table_pose.pose.position.y = -0.4 limit_table_pose.pose.position.z = 0.18 limit_table_pose.pose.orientation.w = 1.0 #scene.add_box(limit_table_id, limit_table_pose, limit_table_size) self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0) # Make the table red and the boxes orange #self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) # self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object #pick_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up #指定拾取后的放置目标姿态 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = placePos.pose.position.x place_pose.pose.position.y = placePos.pose.position.y place_pose.pose.position.z = placePos.pose.position.z place_pose.pose.orientation.w = placePos.pose.orientation.w place_pose.pose.orientation.x = placePos.pose.orientation.x place_pose.pose.orientation.y = placePos.pose.orientation.y place_pose.pose.orientation.z = placePos.pose.orientation.z #设置机器人运行最大速度位百分之~ pick_arm.set_max_velocity_scaling_factor(0.5) # Initialize the grasp pose to the target pose #初始化抓取目标点位 grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it # this way is just used by PR2 robot #grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RV for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts #重复直到成功或者超出尝试的次数 #while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: while result != 1 and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) #moveit中的pick接口,target_id为moveit添加场景的id,此处为目标物体,grasps为可尝试抓取的点位序列 result = pick_arm.pick(target_id, grasps) #打印信息 rospy.logerr("pick_arm.pick: " + str(result)) rospy.sleep(0.2) # If the pick was successful, attempt the place operation #如果抓取成功,尝试放置操作 result_g = None n_attempts = 2 #if result == MoveItErrorCodes.SUCCESS: while result_g != True and n_attempts < max_pick_attempts: #and result == 1: # Generate valid place poses #places = self.make_places(place_pose) n_attempts += 1 print("-------------------") print(place_pose) #更新当前的机械臂状态 pick_arm.set_start_state_to_current_state() #设置moveit运动的目标点位 pick_arm.set_pose_target(place_pose) # Repeat until we succeed or run out of attempts #while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: # n_attempts += 1 # rospy.loginfo("Place attempt: " + str(n_attempts)) # for place in places: # result = pick_arm.place(target_id, place) # if result == MoveItErrorCodes.SUCCESS: # break # rospy.sleep(0.2) #moveit的运动接口,wait=True表示等到执行完成才返回 result_g = pick_arm.go(wait=True) rospy.logerr("pick_arm.go: " + str(result_g)) #打开夹爪 open_client(500) rospy.sleep(0.2) #if result == "False" # rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.") #else: # rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "home" pose stored in the SRDF file #将机械臂返回到SRDF中的“home”姿态 #scene.remove_world_object(target_id) #scene.remove_attached_object(GRIPPER_FRAME, target_id) #scene.remove_world_object(target_id) rospy.sleep(1) open_client(500) pick_arm.set_named_target(ARM_HOME_POSE) pick_arm.go() # Open the gripper to the neutral position # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # pick_gripper.go() rospy.sleep(1) rospy.logerr("pick_server over ")
msg = Float64() msg.data = -1.0 for i in range(30): pub.publish(msg) rospy.sleep(0.1) if __name__ == '__main__': rospy.init_node('task_2_cheat', anonymous=True) # for for 5 sec rospy.sleep(5) rospy.loginfo("start program %f" % rospy.get_time()) arm = MoveGroupCommander("ur5_arm") arm.set_planner_id('RRTConnectkConfigDefault') pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1) client = actionlib.SimpleActionClient( 'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() rospy.loginfo("init pose") msg = FollowJointTrajectoryGoal() msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) msg.trajectory.joint_names = [ 'ur5_arm_shoulder_pan_joint', 'ur5_arm_shoulder_lift_joint', 'ur5_arm_elbow_joint', 'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint' ]
def close_hand(): msg = Float64() msg.data = -1.0 for i in range(30): pub.publish(msg) rospy.sleep(0.1) if __name__ == '__main__': rospy.init_node('task_2_cheat', anonymous=True) # for for 5 sec rospy.sleep(5) rospy.loginfo("start program %f" % rospy.get_time()) arm = MoveGroupCommander("ur5_arm") arm.set_planner_id('RRTConnectkConfigDefault') pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1) client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() rospy.loginfo("init pose") msg = FollowJointTrajectoryGoal() msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2) msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint', 'ur5_arm_shoulder_lift_joint', 'ur5_arm_elbow_joint', 'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint'] msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0], time_from_start=rospy.Duration(2)))
from moveit_commander import RobotCommander, MoveGroupCommander from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown from geometry_msgs.msg import PoseStamped from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation from trajectory_msgs.msg import JointTrajectoryPoint if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) GRIPPER_FRAME = 'right_gripper_link' scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") right_arm.set_planner_id("KPIECEkConfigDefault") rospy.sleep(1) # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") scene.remove_attached_object(GRIPPER_FRAME, "part") #rospy.logwarn("cleaning world") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("open") #right_gripper.go() rospy.sleep(3)
from geometry_msgs.msg import PoseStamped import moveit_msgs.msg import geometry_msgs.msg import moveit_commander if __name__=='__main__': roscpp_initialize(sys.argv) rospy.init_node('move_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() group = MoveGroupCommander("manipulator") # specify the planner group.set_planner_id("RRTkConfigDefault") rospy.sleep(3) ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. # display_trajectory_publisher = rospy.Publisher( # '/move_group/display_planned_path', # moveit_msgs.msg.DisplayTrajectory) print ">>>>> remove scenes" # clean the scene scene.remove_world_object("pole") scene.remove_world_object("ball")
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Initialize the move group for the right arm Arm = MoveGroupCommander(GROUP_NAME_ARM) Arm.set_planner_id("RRTstarkConfigDefault") # Initialize the move group for the right gripper Hand = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = Arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) Arm.set_goal_position_tolerance(0.05) Arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution Arm.allow_replanning(True) # Set the right arm reference frame Arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt Arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 3 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name target_id = 'target' # Remove leftover objects from a previous run scene.remove_world_object(target_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Open the gripper to the neutral position #Hand.set_joint_value_target(GRIPPER_NEUTRAL) #Hand.go() #rospy.sleep(1) target_size = [0.01, 0.01, 0.01] # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.3 target_pose.pose.position.y = 0.03 target_pose.pose.position.z = -0.3 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Initialize the grasp pose to the target pose grasp_pose = PoseStamped() grasp_pose.header.frame_id = REFERENCE_FRAME grasp_pose.pose = target_pose.pose grasp_pose.pose.position.y = 0.2 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = Arm.pick(target_id, grasps) rospy.sleep(0.2) rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class CalibrationMovements: def __init__(self, move_group_name, max_velocity_scaling, max_acceleration_scaling, angle_delta, translation_delta, move_group_namespace='/'): # self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic if not move_group_namespace.endswith('/'): move_group_namespace = move_group_namespace + '/' if move_group_namespace != '/': self.mgc = MoveGroupCommander( move_group_name, robot_description=move_group_namespace + 'robot_description', ns=move_group_namespace) else: self.mgc = MoveGroupCommander(move_group_name) self.mgc.set_planner_id("RRTConnectkConfigDefault" ) # TODO: this is only needed for the UR5 self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling) self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling) self.start_pose = self.mgc.get_current_pose() self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [ math.radians(350) ] # TODO: make param self.target_poses = [] self.current_pose_index = None self.current_plan = None self.fallback_joint_limits = [math.radians(90)] * 4 + [ math.radians(90) ] + [math.radians(180)] + [math.radians(350)] if len(self.mgc.get_active_joints()) == 6: self.fallback_joint_limits = self.fallback_joint_limits[1:] self.angle_delta = angle_delta self.translation_delta = translation_delta def set_and_check_starting_position(self): # sets the starting position to the current robot cartesian EE position and checks movement in all directions # TODO: make repeatable # - save the home position and each target position as joint position # - plan to each target position and back, save plan if not crazy # - return true if can plan to each target position without going crazy self.start_pose = self.mgc.get_current_pose() self.target_poses = self._compute_poses_around_state( self.start_pose, self.angle_delta, self.translation_delta) self.current_pose_index = -1 ret = self._check_target_poses(self.joint_limits) if ret: rospy.loginfo("Set current pose as home") return True else: rospy.logerr("Can't calibrate from this position!") self.start_pose = None self.target_poses = None return False def select_target_pose(self, i): number_of_target_poses = len(self.target_poses) if 0 <= i < number_of_target_poses: rospy.loginfo("Selected pose {} for next movement".format(i)) self.current_pose_index = i return True else: rospy.logerr( "Index {} is out of bounds: there are {} target poses".format( i, number_of_target_poses)) return False def plan_to_start_pose(self): # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal rospy.loginfo("Planning to home pose") return self._plan_to_pose(self.start_pose) def plan_to_current_target_pose(self): # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal i = self.current_pose_index rospy.loginfo("Planning to target pose {}".format(i)) return self._plan_to_pose(self.target_poses[i]) def execute_plan(self): if self.plan is None: rospy.logerr("No plan found!") return False if CalibrationMovements._is_crazy_plan(self.plan, self.fallback_joint_limits): rospy.logerr("Crazy plan found, not executing!") return False self.mgc.execute(self.plan) return True def _plan_to_pose(self, pose): self.mgc.set_start_state_to_current_state() self.mgc.set_pose_target(pose) ret = self.mgc.plan() if type(ret) is tuple: # noetic success, plan, planning_time, error_code = ret else: # melodic plan = ret if CalibrationMovements._is_crazy_plan(plan, self.fallback_joint_limits): rospy.logwarn("Planning failed") self.plan = None return False else: rospy.loginfo("Planning successful") self.plan = plan return True def _check_target_poses(self, joint_limits): if len(self.fallback_joint_limits) == 6: joint_limits = joint_limits[1:] for fp in self.target_poses: self.mgc.set_pose_target(fp) ret = self.mgc.plan() if type(ret) is tuple: # noetic success, plan, planning_time, error_code = ret else: # melodic plan = ret if len(plan.joint_trajectory.points ) == 0 or CalibrationMovements._is_crazy_plan( plan, joint_limits): return False return True @staticmethod def _compute_poses_around_state(start_pose, angle_delta, translation_delta): basis = np.eye(3) pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave final_rots = [] for qd in quaternion_deltas: final_rots.append(list(qd)) # TODO: accept a list of delta values pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave for qd in quaternion_deltas: final_rots.append(list(qd)) final_poses = [] for rot in final_rots: fp = deepcopy(start_pose) ori = fp.pose.orientation combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot) fp.pose.orientation = Quaternion(*combined_rot) final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.x += translation_delta / 2 final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.x -= translation_delta / 2 final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.y += translation_delta final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.y -= translation_delta final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.z += translation_delta / 3 final_poses.append(fp) return final_poses @staticmethod def _rot_per_joint(plan, degrees=False): np_traj = np.array([p.positions for p in plan.joint_trajectory.points]) if len(np_traj) == 0: raise ValueError np_traj_max_per_joint = np_traj.max(axis=0) np_traj_min_per_joint = np_traj.min(axis=0) ret = abs(np_traj_max_per_joint - np_traj_min_per_joint) if degrees: ret = [math.degrees(j) for j in ret] return ret @staticmethod def _is_crazy_plan(plan, max_rotation_per_joint): abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan) if (abs_rot_per_joint > max_rotation_per_joint).any(): return True else: return False
rospy.Duration(1.0)) # Instantiate a robotcommander object robot = moveit_commander.RobotCommander() # Connect to the manipulator move group and get end effector link manipulator = MoveGroupCommander('manipulator') end_effector_link = manipulator.get_end_effector_link() # Set the robot reference frame manipulator.set_pose_reference_frame('rb07_stand_link') # Allow replanning to increase the odds of a solution manipulator.allow_replanning(True) # Set accuracy in position(meters) and orientation (radians) manipulator.set_goal_position_tolerance(0.00001) manipulator.set_goal_orientation_tolerance(0.0001) manipulator.set_planner_id("RRTkConfigDefault") tr = tool_tf.transform.translation print([tr.x, tr.y, tr.z]) # Get a pose to modify start_pose = manipulator.get_current_pose(end_effector_link).pose # go home manipulator.set_named_target('all-zeros') manipulator.go() rospy.sleep(1) # go to vise go_to_pose('vise', manipulator, start_pose) # go to g54 go_to_pose('G54', manipulator, start_pose)
class PlannerAnnotationParser(AnnotationParserBase): """ Parses the annotations files that contains the benchmarking information. """ def __init__(self, path_to_annotation, path_to_data): super(PlannerAnnotationParser, self).__init__(path_to_annotation, path_to_data) self.parse() self._load_scene() self._init_planning() self.benchmark() def check_results(self, results): """ Returns the results from the planner, checking them against any eventual validation data (no validation data in our case). """ return self.planner_data def _load_scene(self): """ Loads the proper scene for the planner. It can be either a python static scene or bag containing an occupancy map. """ scene = self._annotations["scene"] for element in scene: if element["type"] == "launch": self.play_launch(element["name"]) elif element["type"] == "python": self.load_python(element["name"]) elif element["type"] == "bag": self.play_bag(element["name"]) for _ in range(150): rospy.sleep(0.3) # wait for the scene to be spawned properly rospy.sleep(0.5) def _init_planning(self): """ Initialises the needed connections for the planning. """ self.group_id = self._annotations["group_id"] self.planners = self._annotations["planners"] self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(self.group_id) self._marker_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10, latch=True) self._planning_time_sub = rospy.Subscriber( '/move_group/result', MoveGroupActionResult, self._check_computation_time) rospy.sleep(1) self.group.set_num_planning_attempts( self._annotations["planning_attempts"]) self.group.set_goal_tolerance(self._annotations["goal_tolerance"]) self.group.set_planning_time(self._annotations["planning_time"]) self.group.allow_replanning(self._annotations["allow_replanning"]) self._comp_time = [] self.planner_data = [] def benchmark(self): for test_id, test in enumerate(self._annotations["tests"]): marker_position_1 = test["start_xyz"] marker_position_2 = test["goal_xyz"] self._add_markers(marker_position_1, "Start test \n sequence", marker_position_2, "Goal") # Start planning in a given joint position joints = test["start_joints"] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:6] = joints current.joint_state.position = current_joints self.group.set_start_state(current) joints = test["goal_joints"] for planner in self.planners: if planner == "stomp": planner = "STOMP" elif planner == "sbpl": planner = "AnytimeD*" self.planner_id = planner self.group.set_planner_id(planner) self._plan_joints( joints, self._annotations["name"] + "-test_" + str(test_id)) return self.planner_data def _add_markers(self, point, text1, point_2, text2): # add marker for start and goal pose of EE marker_array = MarkerArray() marker_1 = Marker() marker_1.header.frame_id = "world" marker_1.header.stamp = rospy.Time.now() marker_1.type = Marker.SPHERE marker_1.scale.x = 0.04 marker_1.scale.y = 0.04 marker_1.scale.z = 0.04 marker_1.lifetime = rospy.Duration() marker_2 = deepcopy(marker_1) marker_1.color.g = 0.5 marker_1.color.a = 1.0 marker_1.id = 0 marker_1.pose.position.x = point[0] marker_1.pose.position.y = point[1] marker_1.pose.position.z = point[2] marker_2.color.r = 0.5 marker_2.color.a = 1.0 marker_2.id = 1 marker_2.pose.position.x = point_2[0] marker_2.pose.position.y = point_2[1] marker_2.pose.position.z = point_2[2] marker_3 = Marker() marker_3.header.frame_id = "world" marker_3.header.stamp = rospy.Time.now() marker_3.type = Marker.TEXT_VIEW_FACING marker_3.scale.z = 0.10 marker_3.lifetime = rospy.Duration() marker_4 = deepcopy(marker_3) marker_3.text = text1 marker_3.id = 2 marker_3.color.g = 0.5 marker_3.color.a = 1.0 marker_3.pose.position.x = point[0] marker_3.pose.position.y = point[1] marker_3.pose.position.z = point[2] + 0.15 marker_4.text = text2 marker_4.id = 3 marker_4.color.r = 0.5 marker_4.color.a = 1.0 marker_4.pose.position.x = point_2[0] marker_4.pose.position.y = point_2[1] marker_4.pose.position.z = point_2[2] + 0.15 marker_array.markers = [marker_1, marker_2, marker_3, marker_4] self._marker_pub.publish(marker_array) rospy.sleep(1) def _plan_joints(self, joints, test_name): # plan to joint target and determine success self.group.clear_pose_targets() group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) plan = self.group.plan() plan_time = "N/A" total_joint_rotation = "N/A" comp_time = "N/A" plan_success = self._check_plan_success(plan) if plan_success: plan_time = self._check_plan_time(plan) total_joint_rotation = self._check_plan_total_rotation(plan) while not self._comp_time: rospy.sleep(0.5) comp_time = self._comp_time.pop(0) self.planner_data.append([ self.planner_id, test_name, str(plan_success), plan_time, total_joint_rotation, comp_time ]) @staticmethod def _check_plan_success(plan): if len(plan.joint_trajectory.points) > 0: return True else: return False @staticmethod def _check_plan_time(plan): # find duration of successful plan number_of_points = len(plan.joint_trajectory.points) time = plan.joint_trajectory.points[number_of_points - 1].time_from_start.to_sec() return time @staticmethod def _check_plan_total_rotation(plan): # find total joint rotation in successful trajectory angles = [0, 0, 0, 0, 0, 0] number_of_points = len(plan.joint_trajectory.points) for i in range(number_of_points - 1): angles_temp = [ abs(x - y) for x, y in zip(plan.joint_trajectory.points[i + 1].positions, plan.joint_trajectory.points[i].positions) ] angles = [x + y for x, y in zip(angles, angles_temp)] total_angle_change = sum(angles) return total_angle_change def _check_computation_time(self, msg): # get computation time for successful plan to be found if msg.status.status == 3: self._comp_time.append(msg.result.planning_time) def _check_path_length(self, plan): # find distance travelled by end effector # not yet implemented return
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
class Arm_Controller: def __init__(self): # Give the launch a chance to catch up # rospy.sleep(5) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('Arm_Controller') rospy.loginfo("Launched Arm Controller") # constants self.GROUP_NAME_ARM = 'arm' self.GRIPPER_FRAME = 'gripper_link' self.REFERENCE_FRAME = 'base_link' self.ARM_BASE_FRAME = 'arm_base_link' self.done = True self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped) rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1) self.robot_name = "gatlin" move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm) # We need a tf listener to convert poses into arm reference base self.tfl = tf.TransformListener() # Initialize the move group for the right arm self.arm = MoveGroupCommander(self.GROUP_NAME_ARM) self.gripper = Gripper() self.robot = moveit_commander.RobotCommander() # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Set the planner self.arm.set_planner_id("RRTConnectkConfigDefault") # Set the right arm reference frame self.arm.set_pose_reference_frame(self.REFERENCE_FRAME) # Give the scene a chance to catch up rospy.sleep(1) # Allow some leeway in position (meters) and orientation (radians) # USELESS; do not work on pick and place! Explained on this issue: # https://github.com/ros-planning/moveit_ros/issues/577 self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.05) # Allow 2 seconds per planning attempt self.arm.set_planning_time(2.0) # Create a quaternion from the Euler angles #q = quaternion_from_euler(0, pitch, yaw) # horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555) # deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861) down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052) # back_pos = Point(-0.03, 0.0313, 0.476) # init rest pose self.rest_pose = PoseStamped() self.rest_pose.header.frame_id = self.REFERENCE_FRAME self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35) self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981) # init place_upper pose self.place_upper_pose = PoseStamped() self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998) self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889) # init current pose self.current_pose = PoseStamped() self.current_pose.header.frame_id = self.REFERENCE_FRAME self.current_pose.pose.position = Point(0,0,0) self.current_pose.pose.orientation = down # Open the gripper rospy.loginfo("Set Gripper: open") self.gripper.set(1.0) # self.arm.set_pose_target(self.rest_pose) # self.arm.go() # rospy.sleep(1) # self.ar = ArbotixROS() # rate = rospy.Rate(30) # while not rospy.is_shutdown(): # # rospy.logerr(self.ar.getVoltage(4)) # # rospy.logerr(self.ar.getSpeed(5)) # rospy.logerr(self.ar.getPosition(1)) # rate.sleep() rospy.spin() def MoveToPoseWithIntermediate(self, ps, offsets) : success = False for offset in offsets: # interpose = getOffsetPose(hand_pose, offset) interpose = getOffsetPose(ps, offset) success = self.MoveToPose(interpose, "MoveToIntermediatePose") rospy.sleep(1) success = self.MoveToPose(ps, "MoveToPose") return success def MoveToPose(self, ps, name) : newpose = self.transform_pose(self.REFERENCE_FRAME, ps) down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052) newpose.pose.orientation = down if self.move_arm_to_pose(newpose) : rospy.loginfo("SUCCEEDED: %s" % name) return True else : rospy.logerr("FAILED %s" % name) return False def move_arm_to_pose(self, ps): arm_target_pose = deepcopy(ps) arm_target_pose.header.stamp = rospy.Time.now() self.test_pose_publisher.publish(arm_target_pose) self.arm.set_pose_target(arm_target_pose) success = self.arm.go() return success def handle_move_arm(self, req): success = True gripper = self.gripper if req.action == "OPEN_GRIPPER": rospy.loginfo("Beginning to open gripper") gripper.open(block=True) rospy.loginfo("Opened Gripper") elif req.action == "CLOSE_GRIPPER" : rospy.loginfo("Beginning to close Gripper") gripper.close(block=True) rospy.loginfo("Closed Gripper") elif req.action == "MOVE_TO_POSE_INTERMEDIATE" : rospy.loginfo("Trying to Move To Pose With Intermediate") offsets = [Vector3(0,0,.07)] success = self.MoveToPoseWithIntermediate(req.ps, offsets) elif req.action == "MOVE_TO_POSE" : rospy.loginfo("Trying to Move To Pose") success = self.MoveToPose(req.ps, "FAILED MoveToPose") elif req.action == "RESET_ARM" : rospy.loginfo("Trying to Move To Rest Pose") success = self.move_arm_to_pose(self.rest_pose) elif req.action == "PLACE_UPPER" : rospy.loginfo("Trying to Move To Place Upper Pose") success = self.move_arm_to_pose(self.place_upper_pose) # success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose") # elif req.action == "MOVE_TO_POS" : # rospy.loginfo("Trying to Move To Pos") # new_pose = Pose() # if req.limb == 'left': # try: # self.initial_left # new_pose = deepcopy(self.initial_left) # except AttributeError: # new_pose = deepcopy(self.hand_pose_left) # self.initial_left = deepcopy(self.hand_pose_left) # elif req.limb == 'right': # try: # self.initial_right # new_pose = deepcopy(self.initial_right) # except AttributeError: # new_pose = deepcopy(self.hand_pose_right) # self.initial_right = deepcopy(self.hand_pose_right) # new_pose.position = deepcopy(req.pose.position) # # success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose") # success = self.MoveToPoseWithIntermediate(req.limb, new_pose) # rospy.loginfo("Moved to pos: %r" % success) else : success = False rospy.logerr("invalid action") return MoveRobotResponse(success) def orientation_cb(self, data): if(data.x == -1.0 and data.y == -2.0): print "################################################" print "################### Orientation! #############" print "################################################" deg = data.z*15 rad = -deg * pi/180 + pi/2 print rad q = quaternion_from_euler(0,rad,0) self.current_pose.pose.orientation.x = q[0] self.current_pose.pose.orientation.y = q[1] self.current_pose.pose.orientation.z = q[2] self.current_pose.pose.orientation.w = q[3] else: return # transform the pose stamped to the new frame def transform_pose(self, new_frame, pose): if pose.header.frame_id == new_frame: return pose try: ps = deepcopy(pose) ps.header.stamp = rospy.Time(0) self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0)) new_pose = self.tfl.transformPose(new_frame, ps) new_pose.header.stamp = deepcopy(pose.header.stamp) return new_pose except Exception as e: rospy.logerr(e) rospy.logerr("no transform") return None