nao_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to provide joint angle control to Nao by wrapping NaoQI
00005 # This code is currently compatible to NaoQI version 1.6 or newer (latest 
00006 # tested: 1.12)
00007 #
00008 # Copyright 2011 Armin Hornung, University of Freiburg
00009 # http://www.ros.org/wiki/nao
00010 #
00011 # Redistribution and use in source and binary forms, with or without
00012 # modification, are permitted provided that the following conditions are met:
00013 #
00014 #    # Redistributions of source code must retain the above copyright
00015 #       notice, this list of conditions and the following disclaimer.
00016 #    # Redistributions in binary form must reproduce the above copyright
00017 #       notice, this list of conditions and the following disclaimer in the
00018 #       documentation and/or other materials provided with the distribution.
00019 #    # Neither the name of the University of Freiburg nor the names of its
00020 #       contributors may be used to endorse or promote products derived from
00021 #       this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00027 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00030 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 
00037 import rospy
00038 import actionlib
00039 from nao_msgs.msg import(
00040     JointTrajectoryResult,
00041     JointTrajectoryAction,
00042     JointAnglesWithSpeed,
00043     JointAnglesWithSpeedResult,
00044     JointAnglesWithSpeedAction,
00045     BodyPoseWithSpeedAction,
00046     BodyPoseWithSpeedGoal,
00047     BodyPoseWithSpeedResult)
00048  
00049 from nao_driver import NaoNode
00050 
00051 from std_srvs.srv import Empty, EmptyResponse
00052 from sensor_msgs.msg import JointState
00053 
00054 class NaoController(NaoNode):
00055     def __init__(self): 
00056         NaoNode.__init__(self, 'nao_controller')
00057         
00058         self.connectNaoQi()
00059         
00060         # store the number of joints in each motion chain and collection, used for sanity checks
00061         self.collectionSize = {}
00062         for collectionName in ['Head', 'LArm', 'LLeg', 'RLeg', 'RArm', 'Body', 'BodyJoints', 'BodyActuators']:
00063             try:
00064                 self.collectionSize[collectionName] = len(self.motionProxy.getJointNames(collectionName));
00065             except RuntimeError:
00066                 # the following is useful for old NAOs with no legs/arms
00067                 rospy.logwarn('Collection %s not found on your robot.' % collectionName)
00068 
00069         # Get poll rate for actionlib (ie. how often to check whether currently running task has been preempted)
00070         # Defaults to 200ms
00071         self.poll_rate = int(rospy.get_param("~poll_rate", 0.2)*1000)
00072         
00073         # initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
00074         # set to 1.0 if you want to control the robot immediately
00075         initStiffness = rospy.get_param('~init_stiffness', 0.0)
00076         
00077         # TODO: parameterize
00078         if initStiffness > 0.0 and initStiffness <= 1.0:
00079             self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
00080 
00081 
00082         # start services / actions:
00083         self.enableStiffnessSrv = rospy.Service("body_stiffness/enable", Empty, self.handleStiffnessSrv)
00084         self.disableStiffnessSrv = rospy.Service("body_stiffness/disable", Empty, self.handleStiffnessOffSrv)
00085 
00086 
00087         #Start simple action servers
00088         self.jointTrajectoryServer = actionlib.SimpleActionServer("joint_trajectory", JointTrajectoryAction, 
00089                                                                   execute_cb=self.executeJointTrajectoryAction,
00090                                                                   auto_start=False)
00091         
00092         self.jointStiffnessServer = actionlib.SimpleActionServer("joint_stiffness_trajectory", JointTrajectoryAction, 
00093                                                                   execute_cb=self.executeJointStiffnessAction,
00094                                                                   auto_start=False)
00095         
00096         self.jointAnglesServer = actionlib.SimpleActionServer("joint_angles_action", JointAnglesWithSpeedAction, 
00097                                                                   execute_cb=self.executeJointAnglesWithSpeedAction,
00098                                                                   auto_start=False)
00099 
00100         #Start action servers
00101         self.jointTrajectoryServer.start()
00102         self.jointStiffnessServer.start()
00103         self.jointAnglesServer.start()
00104 
00105         # only start when ALRobotPosture proxy is available
00106         if not (self.robotPostureProxy is None):
00107             self.bodyPoseWithSpeedServer = actionlib.SimpleActionServer("body_pose_naoqi", BodyPoseWithSpeedAction,
00108                                               execute_cb=self.executeBodyPoseWithSpeed,
00109                                               auto_start=False)
00110             self.bodyPoseWithSpeedServer.start()
00111         else:
00112             rospy.logwarn("Proxy to ALRobotPosture not available, requests to body_pose_naoqi will be ignored.")
00113 
00114         # subscribers last:
00115         rospy.Subscriber("joint_angles", JointAnglesWithSpeed, self.handleJointAngles, queue_size=10)
00116         rospy.Subscriber("joint_stiffness", JointState, self.handleJointStiffness, queue_size=10)
00117 
00118         rospy.loginfo("nao_controller initialized")
00119 
00120     def connectNaoQi(self):
00121         '''(re-) connect to NaoQI'''
00122         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00123 
00124         self.motionProxy = self.get_proxy("ALMotion")
00125         if self.motionProxy is None:
00126             exit(1)
00127 
00128         # optional, newly introduced in 1.14
00129         self.robotPostureProxy = self.get_proxy("ALRobotPosture")
00130             
00131             
00132     def handleJointAngles(self, msg):
00133         rospy.logdebug("Received a joint angle target")
00134         try:
00135             # Note: changeAngles() and setAngles() are non-blocking functions.
00136             if (msg.relative==0):
00137                 self.motionProxy.setAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed)
00138             else:
00139                 self.motionProxy.changeAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed)
00140         except RuntimeError,e:
00141             rospy.logerr("Exception caught:\n%s", e)
00142             
00143     def handleJointStiffness(self, msg):
00144         rospy.logdebug("Received a joint angle stiffness")
00145         try:
00146             self.motionProxy.setStiffnesses(list(msg.name), list(msg.effort))
00147         except RuntimeError,e:
00148             rospy.logerr("Exception caught:\n%s", e)
00149 
00150     def handleStiffnessSrv(self, req):
00151         try:
00152             self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)
00153             rospy.loginfo("Body stiffness enabled")
00154             return EmptyResponse()
00155         except RuntimeError,e:
00156             rospy.logerr("Exception caught:\n%s", e)
00157             return None
00158         
00159     def handleStiffnessOffSrv(self, req):
00160         try:
00161             self.motionProxy.stiffnessInterpolation("Body", 0.0, 0.5)
00162             rospy.loginfo("Body stiffness removed")
00163             return EmptyResponse()
00164         except RuntimeError,e:
00165             rospy.logerr("Exception caught:\n%s", e)
00166             return None
00167             
00168 
00169     def jointTrajectoryGoalMsgToAL(self, goal):
00170         """Helper, convert action goal msg to Aldebraran-style arrays for NaoQI"""
00171         names = list(goal.trajectory.joint_names)
00172 #        if goal.trajectory.joint_names == ["Body"]:
00173 #            names = self.motionProxy.getJointNames('Body')
00174        
00175         if len(goal.trajectory.points) == 1 and len(goal.trajectory.points[0].positions) == 1:
00176             angles = goal.trajectory.points[0].positions[0]
00177         else:
00178             angles = list(list(p.positions[i] for p in goal.trajectory.points) for i in range(0,len(goal.trajectory.points[0].positions)))
00179         
00180         #strip 6,7 and last 2 from angles if the pose was for H25 but we're running an H21
00181         if not isinstance(angles, float) and len(angles) > self.collectionSize["Body"]:
00182             rospy.loginfo("Stripping angles from %d down to %d", len(angles), self.collectionSize["Body"])
00183             angles.pop(6)
00184             angles.pop(7)
00185             angles.pop()
00186             angles.pop()
00187             
00188         if len(names) > self.collectionSize["Body"]:
00189             rospy.loginfo("Stripping names from %d down to %d", len(names), self.collectionSize["Body"])
00190             names.pop(6)
00191             names.pop(7)
00192             names.pop()
00193             names.pop()
00194         
00195         times = list(p.time_from_start.to_sec() for p in goal.trajectory.points)
00196         if len(times) == 1 and len(names) == 1:
00197             times = times[0]
00198         if (len(names) > 1):
00199             times = [times]*len(names)
00200             
00201         return (names, angles, times)
00202     
00203 
00204     def executeJointTrajectoryAction(self, goal):
00205         rospy.loginfo("JointTrajectory action executing");
00206                     
00207         names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)
00208                 
00209         rospy.logdebug("Received trajectory for joints: %s times: %s", str(names), str(times))     
00210         rospy.logdebug("Trajectory angles: %s", str(angles))
00211 
00212         task_id = None
00213         running = True
00214         #Wait for task to complete
00215         while running and not self.jointTrajectoryServer.is_preempt_requested() and not rospy.is_shutdown():
00216             #If we haven't started the task already...
00217             if task_id is None:
00218                 # ...Start it in another thread (thanks to motionProxy.post)
00219                 task_id = self.motionProxy.post.angleInterpolation(names, angles, times, (goal.relative==0))
00220             
00221             #Wait for a bit to complete, otherwise check we can keep running
00222             running = self.motionProxy.wait(task_id, self.poll_rate)
00223         
00224         # If still running at this point, stop the task
00225         if running and task_id:
00226             self.motionProxy.stop( task_id )
00227         
00228         jointTrajectoryResult = JointTrajectoryResult()
00229         jointTrajectoryResult.goal_position.header.stamp = rospy.Time.now()
00230         jointTrajectoryResult.goal_position.position = self.motionProxy.getAngles(names, True)
00231         jointTrajectoryResult.goal_position.name = names
00232 
00233         if not self.checkJointsLen(jointTrajectoryResult.goal_position):
00234             self.jointTrajectoryServer.set_aborted(jointTrajectoryResult)
00235             rospy.logerr("JointTrajectory action error in result: sizes mismatch")
00236         
00237         elif running:
00238             self.jointTrajectoryServer.set_preempted(jointTrajectoryResult)
00239             rospy.logdebug("JointTrajectory preempted")
00240         
00241         else:
00242             self.jointTrajectoryServer.set_succeeded(jointTrajectoryResult)
00243             rospy.loginfo("JointTrajectory action done")
00244 
00245 
00246     def executeJointStiffnessAction(self, goal):
00247         rospy.loginfo("JointStiffness action executing");
00248                     
00249         names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)
00250         
00251         rospy.logdebug("Received stiffness trajectory for joints: %s times: %s", str(names), str(times))     
00252         rospy.logdebug("stiffness values: %s", str(angles))
00253 
00254         task_id = None
00255         running = True
00256         #Wait for task to complete
00257         while running and not self.jointStiffnessServer.is_preempt_requested() and not rospy.is_shutdown():
00258             #If we haven't started the task already...
00259             if task_id is None:
00260                 # ...Start it in another thread (thanks to motionProxy.post)
00261                 task_id = self.motionProxy.post.stiffnessInterpolation(names, angles, times)
00262             
00263             #Wait for a bit to complete, otherwise check we can keep running
00264             running = self.motionProxy.wait(task_id, self.poll_rate)
00265 
00266         # If still running at this point, stop the task
00267         if running and task_id:
00268             self.motionProxy.stop( task_id )
00269             
00270         jointStiffnessResult = JointTrajectoryResult()
00271         jointStiffnessResult.goal_position.header.stamp = rospy.Time.now()
00272         jointStiffnessResult.goal_position.position = self.motionProxy.getStiffnesses(names)
00273         jointStiffnessResult.goal_position.name = names
00274 
00275         if not self.checkJointsLen(jointStiffnessResult.goal_position):
00276             self.jointStiffnessServer.set_aborted(jointStiffnessResult)
00277             rospy.logerr("JointStiffness action error in result: sizes mismatch")
00278         
00279         elif running:
00280             self.jointStiffnessServer.set_preempted(jointStiffnessResult)
00281             rospy.logdebug("JointStiffness preempted")
00282         
00283         else:
00284             self.jointStiffnessServer.set_succeeded(jointStiffnessResult)
00285             rospy.loginfo("JointStiffness action done")
00286             
00287 
00288     def executeJointAnglesWithSpeedAction(self, goal):           
00289         
00290         names = list(goal.joint_angles.joint_names)
00291         angles = list(goal.joint_angles.joint_angles)
00292         rospy.logdebug("Received JointAnglesWithSpeed for joints: %s angles: %s", str(names), str(angles))
00293             
00294         if goal.joint_angles.relative == 1:
00295             # TODO: this uses the current angles instead of the angles at the given timestamp
00296             currentAngles = self.motionProxy.getAngles(names, True)
00297             angles = list(map(lambda x,y: x+y, angles, currentAngles))            
00298 
00299         task_id = None
00300         running = True
00301         #Wait for task to complete
00302         while running and not self.jointAnglesServer.is_preempt_requested() and not rospy.is_shutdown():
00303             #If we haven't started the task already...
00304             if task_id is None:
00305                 # ...Start it in another thread (thanks to motionProxy.post)
00306                 task_id = self.motionProxy.post.angleInterpolationWithSpeed(names, angles, goal.joint_angles.speed)
00307             
00308             #Wait for a bit to complete, otherwise check we can keep running
00309             running = self.motionProxy.wait(task_id, self.poll_rate)
00310         
00311         # If still running at this point, stop the task
00312         if running and task_id:
00313             self.motionProxy.stop( task_id )
00314             
00315         jointAnglesWithSpeedResult = JointAnglesWithSpeedResult()
00316         jointAnglesWithSpeedResult.goal_position.header.stamp = rospy.Time.now()
00317         jointAnglesWithSpeedResult.goal_position.position = self.motionProxy.getAngles(names, True)
00318         jointAnglesWithSpeedResult.goal_position.name = names
00319 
00320         if not self.checkJointsLen(jointAnglesWithSpeedResult.goal_position):
00321             self.jointAnglesServer.set_aborted(jointAnglesWithSpeedResult)
00322             rospy.logerr("JointAnglesWithSpeed action error in result: sizes mismatch")
00323 
00324         elif running:
00325             self.jointAnglesServer.set_preempted(jointAnglesWithSpeedResult)
00326             rospy.logdebug("JointAnglesWithSpeed preempted")
00327 
00328         else:
00329             self.jointAnglesServer.set_succeeded(jointAnglesWithSpeedResult)
00330             rospy.loginfo("JointAnglesWithSpeed action done")
00331 
00332     def checkJointsLen(self, goal_position):      
00333         if len(goal_position.name) == 1 and self.collectionSize.has_key(goal_position.name[0]):
00334             return len(goal_position.position) == self.collectionSize[goal_position.name[0]] 
00335         else:
00336             return len(goal_position.position) ==  len(goal_position.name)
00337             
00338     def executeBodyPoseWithSpeed(self, goal):
00339       
00340         #~ Sanity checks
00341         if (goal.speed < 0.0) or (goal.speed > 1.0):
00342             bodyPoseWithSpeedResult = BodyPoseWithSpeedResult()
00343             self.bodyPoseWithSpeedServer.set_aborted(bodyPoseWithSpeedResult)
00344             rospy.logerr("Body pose setter: Not a valid speed value.")
00345             return
00346       
00347         valid_postures = self.robotPostureProxy.getPostureList()
00348 
00349         if goal.posture_name not in valid_postures:
00350             bodyPoseWithSpeedResult = BodyPoseWithSpeedResult()
00351             self.bodyPoseWithSpeedServer.set_aborted(bodyPoseWithSpeedResult)  
00352             rospy.logerr("Body pose setter: Not a valid posture.")
00353             return
00354 
00355         #~ Must set stiffness on
00356         try:
00357             self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)
00358             rospy.loginfo("Body stiffness enabled")
00359         except RuntimeError,e:
00360             rospy.logerr("Exception caught:\n%s", e)
00361             return
00362           
00363         #~ Go to posture. This is blocking
00364         self.robotPostureProxy.goToPosture(goal.posture_name, goal.speed)
00365         #~ Return success
00366         self.bodyPoseWithSpeedServer.set_succeeded()
00367 
00368 if __name__ == '__main__':
00369 
00370     controller = NaoController()
00371     rospy.loginfo("nao_controller running...")
00372     rospy.spin()
00373     
00374     rospy.loginfo("nao_controller stopped.")
00375     exit(0)


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Thu Oct 30 2014 09:20:04