test_joint_angles.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 #from rospy.exceptions import ROSException
00005 roslib.load_manifest('naoqi_driver')
00006 import rospy
00007 from rospy import Duration
00008 
00009 
00010 import actionlib
00011 from actionlib_msgs.msg import GoalStatus
00012 import naoqi_bridge_msgs.msg
00013 import trajectory_msgs.msg
00014 from trajectory_msgs.msg import JointTrajectoryPoint
00015 import std_srvs.srv
00016 
00017 # go to crouching position
00018 def joint_angle_client():
00019         #inhibitWalkSrv = rospy.ServiceProxy("inhibit_walk", std_srvs.srv.Empty)
00020         #uninhibitWalkSrv = rospy.ServiceProxy("uninhibit_walk", std_srvs.srv.Empty)
00021         
00022         client = actionlib.SimpleActionClient("joint_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction)
00023         stiffness_client = actionlib.SimpleActionClient("joint_stiffness_trajectory", naoqi_bridge_msgs.msg.JointTrajectoryAction)
00024         angle_client = actionlib.SimpleActionClient("joint_angles_action", naoqi_bridge_msgs.msg.JointAnglesWithSpeedAction)
00025         
00026         rospy.loginfo("Waiting for joint_trajectory and joint_stiffness servers...")
00027         client.wait_for_server()
00028         stiffness_client.wait_for_server()
00029         angle_client.wait_for_server()
00030         rospy.loginfo("Done.")
00031         
00032         #inhibitWalkSrv()
00033         try:    
00034                 goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
00035                 
00036                 # move head: single joint, multiple keypoints
00037                 goal.trajectory.joint_names = ["HeadYaw"]
00038                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), positions = [1.0]))
00039                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.0), positions = [-1.0]))
00040                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(2.5), positions = [0.0]))
00041         
00042                 
00043                 rospy.loginfo("Sending goal...")
00044                 client.send_goal(goal)
00045                 client.wait_for_result()
00046                 result = client.get_result()
00047                 rospy.loginfo("Results: %s", str(result.goal_position.position))
00048                 
00049                 # Test for preemption
00050                 rospy.loginfo("Sending goal again...")
00051                 client.send_goal(goal)
00052                 rospy.sleep(0.5)
00053                 rospy.loginfo("Preempting goal...")
00054                 client.cancel_goal()
00055                 client.wait_for_result()
00056                 if client.get_state() != GoalStatus.PREEMPTED or client.get_result() == result:
00057                     rospy.logwarn("Preemption does not seem to be working")
00058                 else:
00059                     rospy.loginfo("Preemption seems okay")
00060 
00061                 # crouching pose: single keypoint, multiple joints:
00062                 goal.trajectory.joint_names = ["Body"]
00063                 point = JointTrajectoryPoint()
00064                 point.time_from_start = Duration(1.5)
00065                 point.positions = [0.0,0.0,                                     # head
00066                         1.545, 0.33, -1.57, -0.486, 0.0, 0.0,           # left arm
00067                         -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,     # left leg
00068                         -0.3, 0.057, -0.744, 2.192, -1.122, -0.035,     # right leg
00069                         1.545, -0.33, 1.57, 0.486, 0.0, 0.0]            # right arm
00070                 goal.trajectory.points = [point]
00071         
00072                 rospy.loginfo("Sending goal...")
00073                 client.send_goal(goal)
00074                 client.wait_for_result()
00075                 rospy.loginfo("Getting results...")
00076                 result = client.get_result()    
00077                 print "Result:", ', '.join([str(n) for n in result.goal_position.position])
00078                 
00079                         
00080                 # multiple joints, multiple keypoints:
00081                 goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"]
00082                 goal.trajectory.points = []
00083                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), 
00084                                                                                                                 positions = [1.0, 1.0]))
00085                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.0), 
00086                                                                                                                 positions = [1.0, 0.0]))
00087                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(1.5), 
00088                                                                                                                 positions = [0.0, 0.0]))
00089         
00090                 rospy.loginfo("Sending goal...")
00091                 client.send_goal(goal)
00092                 client.wait_for_result()
00093                 rospy.loginfo("Getting results...")
00094                 result = client.get_result()
00095                 print "Result:", ', '.join([str(n) for n in result.goal_position.position])
00096                 
00097                 
00098                 # multiple joints, single keypoint:
00099                 goal.trajectory.joint_names = ["HeadYaw", "HeadPitch"]
00100                 goal.trajectory.points = []
00101                 goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), 
00102                                                                                                                 positions = [0.5, 0.5]))
00103         
00104                 rospy.loginfo("Sending goal...")
00105                 client.send_goal(goal)
00106                 client.wait_for_result()
00107                 rospy.loginfo("Getting results...")
00108                 result = client.get_result()
00109                 print "Result:", ', '.join([str(n) for n in result.goal_position.position])
00110                 
00111                 
00112                 # Control of joints with relative speed
00113                 angle_goal = naoqi_bridge_msgs.msg.JointAnglesWithSpeedGoal()
00114                 angle_goal.joint_angles.relative = False
00115                 angle_goal.joint_angles.joint_names = ["HeadYaw", "HeadPitch"]
00116                 angle_goal.joint_angles.joint_angles = [1.0, 0.0]
00117                 angle_goal.joint_angles.speed = 0.2
00118                 rospy.loginfo("Sending joint angles goal...")
00119                 angle_client.send_goal_and_wait(angle_goal)
00120                 result = angle_client.get_result()
00121                 rospy.loginfo("Angle results: %s", str(result.goal_position.position))
00122                 
00123                 # Test for preemption
00124                 angle_goal.joint_angles.joint_angles = [-1.0, 0.0]
00125                 angle_goal.joint_angles.speed = 0.05
00126                 rospy.loginfo("Sending goal again...")
00127                 angle_client.send_goal(angle_goal)
00128                 rospy.sleep(0.5)
00129                 rospy.loginfo("Preempting goal...")
00130                 angle_client.cancel_goal()
00131                 angle_client.wait_for_result()
00132                 if angle_client.get_state() != GoalStatus.PREEMPTED or angle_client.get_result() == result:
00133                     rospy.logwarn("Preemption does not seem to be working")
00134                 else:
00135                     rospy.loginfo("Preemption seems okay")
00136 
00137                 # Test stiffness actionlib
00138                 stiffness_goal = naoqi_bridge_msgs.msg.JointTrajectoryGoal()
00139                 stiffness_goal.trajectory.joint_names = ["Body"]
00140                 stiffness_goal.trajectory.points.append(JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [1.0]))
00141                 rospy.loginfo("Sending stiffness goal...")
00142                 stiffness_client.send_goal(stiffness_goal)
00143                 stiffness_client.wait_for_result()
00144                 result = stiffness_client.get_result()
00145                 rospy.loginfo("Stiffness results: %s", str(result.goal_position.position))
00146                 
00147                 # Test for preemption
00148                 stiffness_goal.trajectory.points = [JointTrajectoryPoint(time_from_start = Duration(0.5), positions = [0.0])]
00149                 rospy.loginfo("Sending goal again...")
00150                 stiffness_client.send_goal(stiffness_goal)
00151                 rospy.sleep(0.25)
00152                 rospy.loginfo("Preempting goal...")
00153                 stiffness_client.cancel_goal()
00154                 stiffness_client.wait_for_result()
00155                 if stiffness_client.get_state() != GoalStatus.PREEMPTED or stiffness_client.get_result() == result:
00156                     rospy.logwarn("Preemption does not seem to be working")
00157                 else:
00158                     rospy.loginfo("Preemption seems okay")
00159 
00160         finally:
00161                 pass #uninhibitWalkSrv()
00162         
00163         
00164         
00165 if __name__ == '__main__':
00166         try:
00167                 rospy.init_node('joint_angle_client')
00168                 joint_angle_client()
00169                 
00170         except rospy.ROSInterruptException:
00171                 print "program interrupted before completion"
00172 


nao_apps
Author(s):
autogenerated on Sat Jun 8 2019 20:38:35