00001
00002 import roslib; roslib.load_manifest('cob_mmcontroller')
00003 import rospy
00004 import actionlib
00005 import time
00006 from pr2_controllers_msgs.msg import *
00007 from cob_srvs.srv import *
00008 from cob_actions.msg import *
00009 from cob_msgs.msg import *
00010 from trajectory_msgs.msg import *
00011
00012
00013 rospy.init_node('dual_arm_script')
00014 rospy.sleep(1.0)
00015
00016 cob_lbr_client = actionlib.SimpleActionClient('/lbr_controller/joint_trajectory_action', JointTrajectoryAction)
00017 lwr_client = actionlib.SimpleActionClient('/lwr/lbr_controller/joint_trajectory_action', JointTrajectoryAction)
00018
00019 cob_sdh_client = actionlib.SimpleActionClient('/sdh_controller/joint_trajectory_action', JointTrajectoryAction)
00020 lwr_sdh_client = actionlib.SimpleActionClient('/lwr/sdh_controller/joint_trajectory_action', JointTrajectoryAction)
00021
00022 rospy.sleep(2.0)
00023
00024 lbr_joint_names = ["lbr_1_joint","lbr_2_joint","lbr_3_joint","lbr_4_joint","lbr_5_joint","lbr_6_joint","lbr_7_joint"]
00025 sdh_joint_names = ["sdh_thumb_2_joint", "sdh_thumb_3_joint", "sdh_finger_11_joint", "sdh_finger_12_joint", "sdh_finger_13_joint", "sdh_finger_21_joint", "sdh_finger_22_joint", "sdh_finger_23_joint"]
00026
00027 home = JointTrajectory()
00028 home.joint_names = lbr_joint_names
00029 point=JointTrajectoryPoint()
00030 point.positions=[0,0,0,0,0,0,0]
00031 point.time_from_start=rospy.Duration(3)
00032 home.points.append(point)
00033
00034 grasp = JointTrajectory()
00035 grasp.joint_names = lbr_joint_names
00036 point=JointTrajectoryPoint()
00037 point.positions=[0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
00038 point.time_from_start=rospy.Duration(3)
00039 grasp.points.append(point)
00040
00041 cylOpen = JointTrajectory()
00042 cylOpen.joint_names = sdh_joint_names
00043 point=JointTrajectoryPoint()
00044 point.positions=[-0.7854,1.0472,0.0,-0.7854,1.0472,0.0,-0.7854,1.0472]
00045 point.time_from_start=rospy.Duration(2)
00046 cylOpen.points.append(point)
00047
00048 cylClose = JointTrajectory()
00049 cylClose.joint_names = sdh_joint_names
00050 point=JointTrajectoryPoint()
00051 point.positions=[0.0,1.0472,0.0,0.0,1.0472,0.0,0.0,1.0472]
00052 point.time_from_start=rospy.Duration(2)
00053 cylClose.points.append(point)
00054
00055 goal = JointTrajectoryGoal()
00056 sdhgoal = JointTrajectoryGoal()
00057
00058 while not rospy.is_shutdown():
00059 goal.trajectory = home
00060 goal.trajectory.header.stamp = rospy.Time.now()
00061 cob_lbr_client.send_goal(goal)
00062 lwr_client.send_goal(goal)
00063 cob_lbr_client.wait_for_result()
00064 lwr_client.wait_for_result()
00065
00066 sdhgoal.trajectory = cylOpen
00067 sdhgoal.trajectory.header.stamp = rospy.Time.now()
00068 cob_sdh_client.send_goal(sdhgoal)
00069 lwr_sdh_client.send_goal(sdhgoal)
00070 lwr_sdh_client.wait_for_result()
00071 cob_sdh_client.wait_for_result()
00072
00073
00074 goal.trajectory = grasp
00075 goal.trajectory.header.stamp = rospy.Time.now()
00076 cob_lbr_client.send_goal(goal)
00077 lwr_client.send_goal(goal)
00078 cob_lbr_client.wait_for_result()
00079 lwr_client.wait_for_result()
00080
00081 sdhgoal.trajectory = cylClose
00082 sdhgoal.trajectory.header.stamp = rospy.Time.now()
00083 cob_sdh_client.send_goal(sdhgoal)
00084 lwr_sdh_client.send_goal(sdhgoal)
00085 lwr_sdh_client.wait_for_result()
00086 cob_sdh_client.wait_for_result()
