00001
00002 import roslib; roslib.load_manifest('cob_mmcontroller')
00003 import rospy
00004 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00005 def dual_arm_test():
00006 pub = rospy.Publisher('/lwr/lbr_controller/command', JointTrajectory)
00007 cob_pub = rospy.Publisher('/cob3_1/lbr_controller/command', JointTrajectory)
00008 rospy.init_node('dual_arm_test')
00009 rospy.sleep(1.0)
00010 while not rospy.is_shutdown():
00011 traj = JointTrajectory()
00012 traj.joint_names = ['lbr_1_joint', 'lbr_2_joint', 'lbr_3_joint', 'lbr_4_joint', 'lbr_5_joint', 'lbr_6_joint', 'lbr_7_joint']
00013 ptn = JointTrajectoryPoint()
00014 ptn.positions = [0.5, 0.8, 0.8, -0.8, 0.8, 0.8, -0.3]
00015 ptn.velocities = [0.4, 0.4, 0.8, 0.1, 0.8, 0.8, 0.1]
00016 ptn.time_from_start = rospy.Duration(2.0)
00017 traj.header.stamp = rospy.Time.now()
00018 traj.points.append(ptn)
00019 pub.publish(traj)
00020 cob_pub.publish(traj)
00021 rospy.sleep(3.0)
00022 ptn.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
00023 ptn.velocities = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
00024 ptn.time_from_start = rospy.Duration(2.0)
00025 traj.points = []
00026 traj.header.stamp = rospy.Time.now()
00027 traj.points.append(ptn)
00028 pub.publish(traj)
00029 cob_pub.publish(traj)
00030 rospy.sleep(3.0)
00031
00032 if __name__ == '__main__':
00033 try:
00034 dual_arm_test()
00035 except rospy.ROSInterruptException: pass
