test_joint_angles.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import roslib
4 #from rospy.exceptions import ROSException
5 roslib.load_manifest('naoqi_driver')
6 import rospy
7 from rospy import Duration
8 
9 
10 import actionlib
11 from actionlib_msgs.msg import GoalStatus
12 import naoqi_bridge_msgs.msg
13 import trajectory_msgs.msg
14 from trajectory_msgs.msg import JointTrajectoryPoint
15 import std_srvs.srv
16 
17 # go to crouching position
18 def joint_angle_client():
19  #inhibitWalkSrv = rospy.ServiceProxy("inhibit_walk", std_srvs.srv.Empty)
20  #uninhibitWalkSrv = rospy.ServiceProxy("uninhibit_walk", std_srvs.srv.Empty)
21 
22  client = actionlib.SimpleActionClient("joint_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction)
23  stiffness_client = actionlib.SimpleActionClient("joint_stiffness_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction)
24  angle_client = actionlib.SimpleActionClient("joint_angles_action", naoqi_bridge_msgs.msg.JointAnglesWithSpeedAction)
25 
26  rospy.loginfo("Waiting for joint_trajectory and joint_stiffness servers...")
27  client.wait_for_server()
28  stiffness_client.wait_for_server()
29  angle_client.wait_for_server()
30  rospy.loginfo("Done.")
31 
32  #inhibitWalkSrv()
33  try:
34  goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
35 
36  # move head: single joint, multiple keypoints
37  goal.trajectory.joint_names = ["HeadYaw"]
38  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0]))
39  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.0), positions = [-1.0]))
40  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.5), positions = [0.0]))
41 
42 
43  rospy.loginfo("Sending goal...")
44  client.send_goal(goal)
45  client.wait_for_result()
46  result = client.get_result()
47  rospy.loginfo("Results: %s", str(result.goal_position.position))
48 
49  # Test for preemption
50  rospy.loginfo("Sending goal again...")
51  client.send_goal(goal)
52  rospy.sleep(0.5)
53  rospy.loginfo("Preempting goal...")
54  client.cancel_goal()
55  client.wait_for_result()
56  if client.get_state() != GoalStatus.PREEMPTED or client.get_result() == result:
57  rospy.logwarn("Preemption does not seem to be working")
58  else:
59  rospy.loginfo("Preemption seems okay")
60 
61  # crouching pose: single keypoint, multiple joints:
62  goal.trajectory.joint_names = ["Body"]
63  point = JointTrajectoryPoint()
64  point.time_from_start = Duration(1.5)
65  point.positions = [0.0,0.0, # head
66  1.545, 0.33, -1.57, -0.486, 0.0, 0.0, # left arm
67  -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # left leg
68  -0.3, 0.057, -0.744, 2.192, -1.122, -0.035, # right leg
69  1.545, -0.33, 1.57, 0.486, 0.0, 0.0] # right arm
70  goal.trajectory.points = [point]
71 
72  rospy.loginfo("Sending goal...")
73  client.send_goal(goal)
74  client.wait_for_result()
75  rospy.loginfo("Getting results...")
76  result = client.get_result()
77  print "Result:", ', '.join([str(n) for n in result.goal_position.position])
78 
79 
80  # multiple joints, multiple keypoints:
81  goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"]
82  goal.trajectory.points = []
83  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5),
84  positions = [1.0, 1.0]))
85  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0),
86  positions = [1.0, 0.0]))
87  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.5),
88  positions = [0.0, 0.0]))
89 
90  rospy.loginfo("Sending goal...")
91  client.send_goal(goal)
92  client.wait_for_result()
93  rospy.loginfo("Getting results...")
94  result = client.get_result()
95  print "Result:", ', '.join([str(n) for n in result.goal_position.position])
96 
97 
98  # multiple joints, single keypoint:
99  goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"]
100  goal.trajectory.points = []
101  goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5),
102  positions = [0.5, 0.5]))
103 
104  rospy.loginfo("Sending goal...")
105  client.send_goal(goal)
106  client.wait_for_result()
107  rospy.loginfo("Getting results...")
108  result = client.get_result()
109  print "Result:", ', '.join([str(n) for n in result.goal_position.position])
110 
111 
112  # Control of joints with relative speed
113  angle_goal = naoqi_bridge_msgs.msg.JointAnglesWithSpeedGoal()
114  angle_goal.joint_angles.relative = False
115  angle_goal.joint_angles.joint_names = ["HeadYaw", "HeadPitch"]
116  angle_goal.joint_angles.joint_angles = [1.0, 0.0]
117  angle_goal.joint_angles.speed = 0.2
118  rospy.loginfo("Sending joint angles goal...")
119  angle_client.send_goal_and_wait(angle_goal)
120  result = angle_client.get_result()
121  rospy.loginfo("Angle results: %s", str(result.goal_position.position))
122 
123  # Test for preemption
124  angle_goal.joint_angles.joint_angles = [-1.0, 0.0]
125  angle_goal.joint_angles.speed = 0.05
126  rospy.loginfo("Sending goal again...")
127  angle_client.send_goal(angle_goal)
128  rospy.sleep(0.5)
129  rospy.loginfo("Preempting goal...")
130  angle_client.cancel_goal()
131  angle_client.wait_for_result()
132  if angle_client.get_state() != GoalStatus.PREEMPTED or angle_client.get_result() == result:
133  rospy.logwarn("Preemption does not seem to be working")
134  else:
135  rospy.loginfo("Preemption seems okay")
136 
137  # Test stiffness actionlib
138  stiffness_goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
139  stiffness_goal.trajectory.joint_names = ["Body"]
140  stiffness_goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0]))
141  rospy.loginfo("Sending stiffness goal...")
142  stiffness_client.send_goal(stiffness_goal)
143  stiffness_client.wait_for_result()
144  result = stiffness_client.get_result()
145  rospy.loginfo("Stiffness results: %s", str(result.goal_position.position))
146 
147  # Test for preemption
148  stiffness_goal.trajectory.points = [JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.0])]
149  rospy.loginfo("Sending goal again...")
150  stiffness_client.send_goal(stiffness_goal)
151  rospy.sleep(0.25)
152  rospy.loginfo("Preempting goal...")
153  stiffness_client.cancel_goal()
154  stiffness_client.wait_for_result()
155  if stiffness_client.get_state() != GoalStatus.PREEMPTED or stiffness_client.get_result() == result:
156  rospy.logwarn("Preemption does not seem to be working")
157  else:
158  rospy.loginfo("Preemption seems okay")
159 
160  finally:
161  pass #uninhibitWalkSrv()
162 
163 
164 
165 if __name__ == '__main__':
166  try:
167  rospy.init_node('joint_angle_client')
168  joint_angle_client()
169 
170  except rospy.ROSInterruptException:
171  print "program interrupted before completion"
172 
test_joint_angles::joint_angle_client
def joint_angle_client()
Definition: test_joint_angles.py:18

nao_apps
Author(s):
autogenerated on Mon Jun 10 2019 14:04:55