37 from control_msgs.msg
import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
38 from trajectory_msgs.msg
import JointTrajectoryPoint
40 rospy.init_node(
'send_motion')
45 r_client.wait_for_server()
46 l_client.wait_for_server()
47 h_client.wait_for_server()
48 c_client.wait_for_server()
51 r_msg = FollowJointTrajectoryGoal()
52 r_msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
53 r_msg.trajectory.joint_names = [
'RARM_JOINT0',
'RARM_JOINT1',
'RARM_JOINT2',
'RARM_JOINT3',
'RARM_JOINT4',
'RARM_JOINT5']
54 r_msg.trajectory.points.append(JointTrajectoryPoint(positions=[0,0,-2.1,0,0,0], time_from_start = rospy.Duration(0.1)))
55 r_msg.trajectory.points.append(JointTrajectoryPoint(positions=[0,0,-2.1,0,0,0], time_from_start = rospy.Duration(2)))
56 r_msg.trajectory.points.append(JointTrajectoryPoint(positions=[0,0,-1.6,0,0,0], time_from_start = rospy.Duration(3)))
58 l_msg = FollowJointTrajectoryGoal()
59 l_msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
60 l_msg.trajectory.joint_names = [
'LARM_JOINT0',
'LARM_JOINT1',
'LARM_JOINT2',
'LARM_JOINT3',
'LARM_JOINT4',
'LARM_JOINT5']
61 l_msg.trajectory.points.append(JointTrajectoryPoint(positions=[0,0,-2.1,0,0,0], time_from_start = rospy.Duration(0.1)))
62 l_msg.trajectory.points.append(JointTrajectoryPoint(positions=[0,0,-2.1,0,0,0], time_from_start = rospy.Duration(2)))
63 l_msg.trajectory.points.append(JointTrajectoryPoint(positions=[0,0,-1.6,0,0,0], time_from_start = rospy.Duration(3)))
66 r_client.send_goal(r_msg)
67 l_client.send_goal(l_msg)
69 r_client.wait_for_result()
70 l_client.wait_for_result()
73 h_msg = FollowJointTrajectoryGoal()
74 h_msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
75 h_msg.trajectory.joint_names = [
'HEAD_JOINT0',
'HEAD_JOINT1']
76 h_msg.trajectory.points.append(JointTrajectoryPoint(positions=[ 1.0, 0.0], time_from_start = rospy.Duration(1)))
77 h_msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.0,-0.2], time_from_start = rospy.Duration(2)))
78 h_msg.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.0, 0.2], time_from_start = rospy.Duration(3)))
79 h_msg.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.0, 0.0], time_from_start = rospy.Duration(4)))
81 h_client.send_goal(h_msg)
82 h_client.wait_for_result()
85 c_msg = FollowJointTrajectoryGoal()
86 c_msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
87 c_msg.trajectory.joint_names = [
'CHEST_JOINT0']
88 c_msg.trajectory.points.append(JointTrajectoryPoint(positions=[ 1.0], time_from_start = rospy.Duration(1)))
89 c_msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.0], time_from_start = rospy.Duration(2)))
90 c_msg.trajectory.points.append(JointTrajectoryPoint(positions=[ 0.0], time_from_start = rospy.Duration(3)))
92 c_client.send_goal(c_msg)
93 c_client.wait_for_result()