def main(): control = ee_cart_imped_action.EECartImpedClient('left_arm') control.addTrajectoryPoint(0.5, 0, -.35, 0, 0, 0, 1, 100, 1000, 1000, 30, 30, 30, False, False, False, False, False, False, 4, '/torso_lift_link') control.addTrajectoryPoint(0.7, 0, -.35, 0, 0, 0, 1, 100, 1000, 1000, 30, 30, 30, False, False, False, False, False, False, 8, '/torso_lift_link') control.addTrajectoryPoint(0.5, 0, -.35, 0, 0, 0, 1, 100, 1000, 1000, 30, 30, 30, False, False, False, False, False, False, 12, '/torso_lift_link') control.sendGoal()
def test3(): try: control = ee_cart_imped_action.EECartImpedClient('UR5') control.addTrajectoryPoint(0.5, 0, 0, 0, 0, 0, 1, 1000, 1000, 1000, 30, 30, 30, False, False, False, False, False, False, 4, '/ee_link'); control.addTrajectoryPoint(0.75, 0, 0, 0, 0, 0, 1, 50, 1000, 1000, 30, 30, 30, False, False, False, False, False, False, 6, '/ee_link'); control.sendGoal() except KeyboardInterrupt: #client.cancel_goal() raise except: raise
def main(): control = ee_cart_imped_action.EECartImpedClient('right_arm') control.addTrajectoryPoint(0.1, 0, 0, 0, 0, 0.5, 0.866, 1000, 50, 1000, 30, 30, 30, False, False, False, False, False, False, 4, '/r_gripper_tool_frame') control.addTrajectoryPoint(0.1, 0, 0, 0, 0, 0.5, 0.866, 1000, 50, 1000, 30, 30, 30, False, False, False, False, False, False, 20, '/r_gripper_tool_frame') control.addTrajectoryPoint(-0.1, 0, 0, 0, 0, 0, 1, 1000, 50, 1000, 30, 30, 30, False, False, False, False, False, False, 24, '/r_gripper_tool_frame') # control.addTrajectoryPoint(0.1, 0, 0, 0, 0, 0, 1, # 1000, 50, 1000, 30, 30, 30, # False, False, False, False, False, # False, 4, '/r_gripper_tool_frame'); # control.addTrajectoryPoint(0.0, 0, 0, 0, 0, 0, 1, # 1000, 50, 1000, 30, 30, 30, # False, False, False, False, False, # False, 8, '/r_gripper_tool_frame'); control.sendGoal()
def __init__(self, arm_name, manager=None): """ Creates a new arm_control objects """ rospy.loginfo("Creating arm controller...") self.arm_name = arm_name self.curr_controller = '' self.switchToForce() """ Simple action client to deal with the ee_cart_imped_controller. """ self.force_control = ee_cart_imped_action.EECartImpedClient(arm_name) self.switchToCartesian() self.move_arm_client =\ actionlib.SimpleActionClient('/move_'+self.arm_name,\ arm_navigation_msgs.msg.MoveArmAction) rospy.loginfo('Waiting for move arm client') self.move_arm_client.wait_for_server() self.move_arm_planner = rospy.ServiceProxy\ (PLANNER_NAME, arm_navigation_msgs.srv.GetMotionPlan) rospy.loginfo('Waiting for move arm planner') self.move_arm_planner.wait_for_service() self.traj_client =\ actionlib.SimpleActionClient\ ('/'+self.arm_name[0]+'_arm_controller/joint_trajectory_action',\ pr2_controllers_msgs.msg.JointTrajectoryAction) rospy.loginfo('Waiting for joint trajectory action server') self.traj_client.wait_for_server() self.interpolated_ik_planning_service =\ rospy.ServiceProxy('/'+self.arm_name[0]+\ '_interpolated_ik_motion_plan',\ arm_navigation_msgs.srv.GetMotionPlan) rospy.loginfo('Waiting for interpolated IK planning service') self.interpolated_ik_planning_service.wait_for_service() self.interpolated_ik_parameter_service =\ rospy.ServiceProxy('/'+self.arm_name[0]+\ '_interpolated_ik_motion_plan_set_params',\ interpolated_ik_motion_planner.srv.\ SetInterpolatedIKMotionPlanParams) rospy.loginfo('Waiting for interpolated IK parameter setting service') self.interpolated_ik_parameter_service.wait_for_service() self.filter_trajectory_with_constraints_service = rospy.ServiceProxy\ ('/trajectory_filter_server/filter_trajectory_with_constraints',\ arm_navigation_msgs.srv.FilterJointTrajectoryWithConstraints) rospy.loginfo('Waiting for trajectory filter with constraints service') self.filter_trajectory_with_constraints_service.wait_for_service() self.kinematics_info_service = rospy.ServiceProxy\ ('/pr2_'+self.arm_name+ '_kinematics/get_ik_solver_info',\ kinematics_msgs.srv.GetKinematicSolverInfo) rospy.loginfo('Waiting for kinematics solver info service') self.kinematics_info_service.wait_for_service() self.get_state_validity_service = rospy.ServiceProxy\ ('/planning_scene_validity_server/get_state_validity',\ arm_navigation_msgs.srv.GetStateValidity) rospy.loginfo('Waiting for state validity service.') self.get_state_validity_service.wait_for_service() self.collision_aware_ik_service = rospy.ServiceProxy\ ('/pr2_'+self.arm_name+'_kinematics/get_constraint_aware_ik',\ kinematics_msgs.srv.GetConstraintAwarePositionIK) rospy.loginfo('Waiting for collision aware IK service') self.collision_aware_ik_service.wait_for_service() self.ik_service = rospy.ServiceProxy\ ('/pr2_'+self.arm_name+'_kinematics/get_ik',\ kinematics_msgs.srv.GetPositionIK) rospy.loginfo('Waiting for IK service') self.collision_aware_ik_service.wait_for_service() self.set_planning_scene_service = rospy.ServiceProxy\ ('/environment_server/set_planning_scene_diff',\ arm_navigation_msgs.srv.SetPlanningSceneDiff) rospy.loginfo('Waiting for set planning scene service') self.set_planning_scene_service.wait_for_service() self.get_planning_scene_service = rospy.ServiceProxy\ ('/environment_server/get_planning_scene',\ arm_navigation_msgs.srv.GetPlanningScene) rospy.loginfo('Waiting for get planning scene service') self.get_planning_scene_service.wait_for_service() self.gripper_client = actionlib.SimpleActionClient\ (self.arm_name[0]+'_gripper_controller/gripper_action',\ pr2_controllers_msgs.msg.Pr2GripperCommandAction) rospy.loginfo('Waiting for gripper action') self.gripper_client.wait_for_server() self.manager = manager if not self.manager: self.manager =\ pr2_pick_and_place_demos.pick_and_place_manager.\ PickAndPlaceManager() """ Transform listener that allows the arm control to get the location of the hand in the torso_lift_link frame. """ self.tf_listener = self.manager.tf_listener rospy.loginfo("Arm controller created")
def main(): rospy.loginfo('The ee_cart_imped_action does no collision checking. Please place robot away from any obstacles and press enter to continue') ans = raw_input() right_arm = ee_cart_imped_action.EECartImpedClient('right_arm') left_arm = ee_cart_imped_action.EECartImpedClient('left_arm') rospy.loginfo('Moving to initial positions') pose_stamped = geometry_msgs.msg.PoseStamped() pose_stamped.header.frame_id = '/torso_lift_link' pose_stamped.pose.position.y = -0.4 pose_stamped.pose.position.z = -0.2 pose_stamped.pose.orientation.y = 0.707 pose_stamped.pose.orientation.w = 0.707 right_arm.moveToPoseStamped(pose_stamped, 4.0) pose_stamped.pose.position.y = 0.4 left_arm.moveToPoseStamped(pose_stamped, 4.0) #do this a couple times to make sure #everything continues to work rospy.loginfo('Beginning trajectory tests') for i in range(2): right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0.707, 0, 0, 0.707, 8, 1000, 5, 100, 100, 100, True, False, True, False, False, False, 4.0, frame_id='/base_link') right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0.707, 0, 0, 0.707, -3, 1000, 5, 100, 100, 100, True, False, True, False, False, False, 6.0, frame_id='/base_link') rospy.loginfo('Press enter for a trajectory of 2 force points on the right arm') ans = raw_input() right_arm.sendGoal() right_arm.resetGoal() right_arm.addTrajectoryPoint(0.6, 0, 2.0, 0.707, 0, 0, 0.707, 1000, 1000, 1000, 1.0, 100, 100, False, False, False, True, False, False, 3.0, frame_id='/base_link') right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0.5, 0.5, 0.5, 0.5, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 5.0, frame_id='/base_link') right_arm.addTrajectoryPoint(0.2, -0.6, 1.0, -0.5, 0.5, 0.5, 0.5, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 9.0, frame_id='/base_link') rospy.loginfo('Press enter for a torque to stiffness trajectory on the right arm') ans = raw_input() right_arm.sendGoal() right_arm.resetGoal() left_arm.addTrajectoryPoint(0, 0.4, 0, 0.707, 0, 0, 0.707, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 2.0, frame_id='/torso_lift_link') left_arm.addTrajectoryPoint(0, 0.4, 0, 0.707, 0, 0, 0.707, -5, 1000, 1000, 100, 100, 100, True, False, False, False, False, False, 4.0, frame_id='/torso_lift_link') left_arm.addTrajectoryPoint(0.1, 0.4, -0.2, 0.707, 0, 0, 0.707, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 6.0, frame_id='/torso_lift_link') rospy.loginfo('Press enter for a trajectory on the left arm') ans = raw_input() left_arm.sendGoal() left_arm.resetGoal() pose_stamped = geometry_msgs.msg.PoseStamped() pose_stamped.header.frame_id = '/base_link' pose_stamped.pose.position.x = 0.6 pose_stamped.pose.position.y = 0 pose_stamped.pose.position.z = 1.0 pose_stamped.pose.orientation.w = 1.0 rospy.loginfo('Press enter to test move to pose on the right arm') ans = raw_input() right_arm.moveToPoseStamped(pose_stamped, 2.0) rospy.loginfo('Moving to another pose') pose_stamped.pose.position.y = -0.2 pose_stamped.pose.orientation.z = -0.707 pose_stamped.pose.orientation.w = 0.707 right_arm.moveToPoseStamped(pose_stamped, 4.0) right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0.707, 0, 0, 0.707, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 2.0, frame_id='/base_link') right_arm.addTrajectoryPoint(0.6, 0, 0.0, 0.707, 0, 0, 0.707, 1000, -10, 1000, 100, 100, 100, False, True, False, False, False, False, 4.0, frame_id='/torso_lift_link') right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0.707, 0, 0, 0.707, 1000, 1000, -5, 100, 100, 100, False, False, True, False, False, False, 6.0, frame_id='/base_link') right_arm.addTrajectoryPoint(0.1, -0.4, 1.0, 0, 0, 0, 1.0, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 8.0, frame_id='/base_link') rospy.loginfo('Press enter for a mixed trajectory on right arm') ans = raw_input() right_arm.sendGoal() right_arm.resetGoal() pose_stamped.pose.position.y = 0.1 rospy.loginfo('Press enter to test move to pose stamped on left arm') ans = raw_input() left_arm.moveToPoseStamped(pose_stamped, 3.0) pose_stamped.pose.position.x = 0 pose_stamped.pose.position.y = 0.4 pose_stamped.pose.position.z = 0.0 pose_stamped.header.frame_id = '/torso_lift_link' pose_stamped.pose.orientation.y = 0.707 pose_stamped.pose.orientation.w = 0.707 rospy.loginfo('Testing another move to pose stamped on left arm') left_arm.moveToPoseStamped(pose_stamped, 4.0) rospy.loginfo('Beginning canceling tests') pose_stamped = geometry_msgs.msg.PoseStamped() pose_stamped.header.frame_id = '/base_link' pose_stamped.pose.position.x = 0.6 pose_stamped.pose.position.y = 0 pose_stamped.pose.position.z = 1.0 pose_stamped.pose.orientation.w = 1.0 right_arm.moveToPoseStamped(pose_stamped, 4.0) right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0.707, 0, 0, 0.707, 1000, 1000, -5, 100, 100, 100, False, False, True, False, False, False, 2.0, frame_id='/base_link') right_arm.addTrajectoryPoint(0.6, 0, 1.0, 0, 0, 0, 1.0, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 4.0, frame_id='/base_link') rospy.loginfo('Press enter to test canceling goal') ans = raw_input() right_arm.sendGoal(wait=False) right_arm.cancelGoal() rospy.loginfo('Press enter to test canceling goal on second trajectory point') ans = raw_input() right_arm.sendGoal(wait=False) rospy.sleep(2.5) right_arm.cancelGoal() rospy.loginfo('Returning to initial position') right_arm.sendGoal() rospy.loginfo('Press enter to test goal preempt') ans = raw_input() right_arm.sendGoal(wait=False) rospy.sleep(2.5) rospy.loginfo('Returning to intial position') right_arm.sendGoal() rospy.loginfo('Press enter to test quick succession of goals.') ans = raw_input() for i in range(50): right_arm.sendGoal(wait=False) rospy.sleep(0.1) rospy.loginfo('Returning to initial position') right_arm.sendGoal() right_arm.resetGoal() right_arm.addTrajectoryPoint(0, -0.4, -0.2, 0, 0.707, 0, 0.707, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 4.0, frame_id='/torso_lift_link') left_arm.addTrajectoryPoint(0, 0.4, -0.2, 0, 0.707, 0, 0.707, 1000, 1000, 1000, 100, 100, 100, False, False, False, False, False, False, 4.0, frame_id='/torso_lift_link') rospy.loginfo('Press enter to test simultaneous arm movement') ans = raw_input() left_arm.sendGoal(wait=False) right_arm.sendGoal() rospy.loginfo('Tests completed')