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')