pose_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 naoqi_msgs.msg import(
00040     JointTrajectoryResult,
00041     JointTrajectoryAction,
00042     JointAnglesWithSpeed,
00043     JointAnglesWithSpeedResult,
00044     JointAnglesWithSpeedAction,
00045     BodyPoseWithSpeedAction,
00046     BodyPoseWithSpeedGoal,
00047     BodyPoseWithSpeedResult)
00048 
00049 from naoqi_driver.naoqi_node import NaoqiNode
00050 
00051 from std_srvs.srv import Empty, EmptyResponse
00052 from sensor_msgs.msg import JointState
00053 
00054 class PoseController(NaoqiNode):
00055     def __init__(self):
00056         NaoqiNode.__init__(self, 'pose_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         self.wakeUpSrv = rospy.Service("wakeup", Empty, self.handleWakeUpSrv)
00086         self.restSrv = rospy.Service("rest", Empty, self.handleRestSrv)
00087 
00088 
00089         #Start simple action servers
00090         self.jointTrajectoryServer = actionlib.SimpleActionServer("joint_trajectory", JointTrajectoryAction,
00091                                                                   execute_cb=self.executeJointTrajectoryAction,
00092                                                                   auto_start=False)
00093 
00094         self.jointStiffnessServer = actionlib.SimpleActionServer("joint_stiffness_trajectory", JointTrajectoryAction,
00095                                                                   execute_cb=self.executeJointStiffnessAction,
00096                                                                   auto_start=False)
00097 
00098         self.jointAnglesServer = actionlib.SimpleActionServer("joint_angles_action", JointAnglesWithSpeedAction,
00099                                                                   execute_cb=self.executeJointAnglesWithSpeedAction,
00100                                                                   auto_start=False)
00101 
00102         #Start action servers
00103         self.jointTrajectoryServer.start()
00104         self.jointStiffnessServer.start()
00105         self.jointAnglesServer.start()
00106 
00107         # only start when ALRobotPosture proxy is available
00108         if not (self.robotPostureProxy is None):
00109             self.bodyPoseWithSpeedServer = actionlib.SimpleActionServer("body_pose_naoqi", BodyPoseWithSpeedAction,
00110                                               execute_cb=self.executeBodyPoseWithSpeed,
00111                                               auto_start=False)
00112             self.bodyPoseWithSpeedServer.start()
00113         else:
00114             rospy.logwarn("Proxy to ALRobotPosture not available, requests to body_pose_naoqi will be ignored.")
00115 
00116         # subscribers last:
00117         rospy.Subscriber("joint_angles", JointAnglesWithSpeed, self.handleJointAngles, queue_size=10)
00118         rospy.Subscriber("joint_stiffness", JointState, self.handleJointStiffness, queue_size=10)
00119 
00120         rospy.loginfo("nao_controller initialized")
00121 
00122     def connectNaoQi(self):
00123         '''(re-) connect to NaoQI'''
00124         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00125 
00126         self.motionProxy = self.get_proxy("ALMotion")
00127         if self.motionProxy is None:
00128             exit(1)
00129 
00130         # optional, newly introduced in 1.14
00131         self.robotPostureProxy = self.get_proxy("ALRobotPosture")
00132 
00133     def handleJointAngles(self, msg):
00134         rospy.logdebug("Received a joint angle target")
00135         try:
00136             # Note: changeAngles() and setAngles() are non-blocking functions.
00137             if (msg.relative==0):
00138                 self.motionProxy.setAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed)
00139             else:
00140                 self.motionProxy.changeAngles(list(msg.joint_names), list(msg.joint_angles), msg.speed)
00141         except RuntimeError,e:
00142             rospy.logerr("Exception caught:\n%s", e)
00143 
00144     def handleJointStiffness(self, msg):
00145         rospy.logdebug("Received a joint angle stiffness")
00146         try:
00147             self.motionProxy.setStiffnesses(list(msg.name), list(msg.effort))
00148         except RuntimeError,e:
00149             rospy.logerr("Exception caught:\n%s", e)
00150 
00151     def handleStiffnessSrv(self, req):
00152         try:
00153             self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)
00154             rospy.loginfo("Body stiffness enabled")
00155             return EmptyResponse()
00156         except RuntimeError,e:
00157             rospy.logerr("Exception caught:\n%s", e)
00158             return None
00159 
00160     def handleStiffnessOffSrv(self, req):
00161         try:
00162             self.motionProxy.stiffnessInterpolation("Body", 0.0, 0.5)
00163             rospy.loginfo("Body stiffness removed")
00164             return EmptyResponse()
00165         except RuntimeError,e:
00166             rospy.logerr("Exception caught:\n%s", e)
00167             return None
00168 
00169     def handleWakeUpSrv(self, req):
00170         try:
00171             self.motionProxy.wakeUp()
00172             rospy.loginfo("Wake Up")
00173             return EmptyResponse()
00174         except RuntimeError,e:
00175             rospy.logerr("Exception caught:\n%s", e)
00176             return None
00177 
00178     def handleRestSrv(self, req):
00179         try:
00180             self.motionProxy.rest()
00181             rospy.loginfo("Rest")
00182             return EmptyResponse()
00183         except RuntimeError,e:
00184             rospy.logerr("Exception caught:\n%s", e)
00185             return None
00186 
00187     def jointTrajectoryGoalMsgToAL(self, goal):
00188         """Helper, convert action goal msg to Aldebraran-style arrays for NaoQI"""
00189         names = list(goal.trajectory.joint_names)
00190 #        if goal.trajectory.joint_names == ["Body"]:
00191 #            names = self.motionProxy.getJointNames('Body')
00192 
00193         if len(goal.trajectory.points) == 1 and len(goal.trajectory.points[0].positions) == 1:
00194             angles = goal.trajectory.points[0].positions[0]
00195         else:
00196             angles = list(list(p.positions[i] for p in goal.trajectory.points) for i in range(0,len(goal.trajectory.points[0].positions)))
00197 
00198         #strip 6,7 and last 2 from angles if the pose was for H25 but we're running an H21
00199         if not isinstance(angles, float) and len(angles) > self.collectionSize["Body"]:
00200             rospy.loginfo("Stripping angles from %d down to %d", len(angles), self.collectionSize["Body"])
00201             angles.pop(6)
00202             angles.pop(7)
00203             angles.pop()
00204             angles.pop()
00205 
00206         if len(names) > self.collectionSize["Body"]:
00207             rospy.loginfo("Stripping names from %d down to %d", len(names), self.collectionSize["Body"])
00208             names.pop(6)
00209             names.pop(7)
00210             names.pop()
00211             names.pop()
00212 
00213         times = list(p.time_from_start.to_sec() for p in goal.trajectory.points)
00214         if len(times) == 1 and len(names) == 1:
00215             times = times[0]
00216         if (len(names) > 1):
00217             times = [times]*len(names)
00218 
00219         return (names, angles, times)
00220 
00221 
00222     def executeJointTrajectoryAction(self, goal):
00223         rospy.loginfo("JointTrajectory action executing");
00224 
00225         names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)
00226 
00227         rospy.logdebug("Received trajectory for joints: %s times: %s", str(names), str(times))
00228         rospy.logdebug("Trajectory angles: %s", str(angles))
00229 
00230         task_id = None
00231         running = True
00232         #Wait for task to complete
00233         while running and not self.jointTrajectoryServer.is_preempt_requested() and not rospy.is_shutdown():
00234             #If we haven't started the task already...
00235             if task_id is None:
00236                 # ...Start it in another thread (thanks to motionProxy.post)
00237                 task_id = self.motionProxy.post.angleInterpolation(names, angles, times, (goal.relative==0))
00238 
00239             #Wait for a bit to complete, otherwise check we can keep running
00240             running = self.motionProxy.wait(task_id, self.poll_rate)
00241 
00242         # If still running at this point, stop the task
00243         if running and task_id:
00244             self.motionProxy.stop( task_id )
00245 
00246         jointTrajectoryResult = JointTrajectoryResult()
00247         jointTrajectoryResult.goal_position.header.stamp = rospy.Time.now()
00248         jointTrajectoryResult.goal_position.position = self.motionProxy.getAngles(names, True)
00249         jointTrajectoryResult.goal_position.name = names
00250 
00251         if not self.checkJointsLen(jointTrajectoryResult.goal_position):
00252             self.jointTrajectoryServer.set_aborted(jointTrajectoryResult)
00253             rospy.logerr("JointTrajectory action error in result: sizes mismatch")
00254 
00255         elif running:
00256             self.jointTrajectoryServer.set_preempted(jointTrajectoryResult)
00257             rospy.logdebug("JointTrajectory preempted")
00258 
00259         else:
00260             self.jointTrajectoryServer.set_succeeded(jointTrajectoryResult)
00261             rospy.loginfo("JointTrajectory action done")
00262 
00263 
00264     def executeJointStiffnessAction(self, goal):
00265         rospy.loginfo("JointStiffness action executing");
00266 
00267         names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)
00268 
00269         rospy.logdebug("Received stiffness trajectory for joints: %s times: %s", str(names), str(times))
00270         rospy.logdebug("stiffness values: %s", str(angles))
00271 
00272         task_id = None
00273         running = True
00274         #Wait for task to complete
00275         while running and not self.jointStiffnessServer.is_preempt_requested() and not rospy.is_shutdown():
00276             #If we haven't started the task already...
00277             if task_id is None:
00278                 # ...Start it in another thread (thanks to motionProxy.post)
00279                 task_id = self.motionProxy.post.stiffnessInterpolation(names, angles, times)
00280 
00281             #Wait for a bit to complete, otherwise check we can keep running
00282             running = self.motionProxy.wait(task_id, self.poll_rate)
00283 
00284         # If still running at this point, stop the task
00285         if running and task_id:
00286             self.motionProxy.stop( task_id )
00287 
00288         jointStiffnessResult = JointTrajectoryResult()
00289         jointStiffnessResult.goal_position.header.stamp = rospy.Time.now()
00290         jointStiffnessResult.goal_position.position = self.motionProxy.getStiffnesses(names)
00291         jointStiffnessResult.goal_position.name = names
00292 
00293         if not self.checkJointsLen(jointStiffnessResult.goal_position):
00294             self.jointStiffnessServer.set_aborted(jointStiffnessResult)
00295             rospy.logerr("JointStiffness action error in result: sizes mismatch")
00296 
00297         elif running:
00298             self.jointStiffnessServer.set_preempted(jointStiffnessResult)
00299             rospy.logdebug("JointStiffness preempted")
00300 
00301         else:
00302             self.jointStiffnessServer.set_succeeded(jointStiffnessResult)
00303             rospy.loginfo("JointStiffness action done")
00304 
00305 
00306     def executeJointAnglesWithSpeedAction(self, goal):
00307 
00308         names = list(goal.joint_angles.joint_names)
00309         angles = list(goal.joint_angles.joint_angles)
00310         rospy.logdebug("Received JointAnglesWithSpeed for joints: %s angles: %s", str(names), str(angles))
00311 
00312         if goal.joint_angles.relative == 1:
00313             # TODO: this uses the current angles instead of the angles at the given timestamp
00314             currentAngles = self.motionProxy.getAngles(names, True)
00315             angles = list(map(lambda x,y: x+y, angles, currentAngles))
00316 
00317         task_id = None
00318         running = True
00319         #Wait for task to complete
00320         while running and not self.jointAnglesServer.is_preempt_requested() and not rospy.is_shutdown():
00321             #If we haven't started the task already...
00322             if task_id is None:
00323                 # ...Start it in another thread (thanks to motionProxy.post)
00324                 task_id = self.motionProxy.post.angleInterpolationWithSpeed(names, angles, goal.joint_angles.speed)
00325 
00326             #Wait for a bit to complete, otherwise check we can keep running
00327             running = self.motionProxy.wait(task_id, self.poll_rate)
00328 
00329         # If still running at this point, stop the task
00330         if running and task_id:
00331             self.motionProxy.stop( task_id )
00332 
00333         jointAnglesWithSpeedResult = JointAnglesWithSpeedResult()
00334         jointAnglesWithSpeedResult.goal_position.header.stamp = rospy.Time.now()
00335         jointAnglesWithSpeedResult.goal_position.position = self.motionProxy.getAngles(names, True)
00336         jointAnglesWithSpeedResult.goal_position.name = names
00337 
00338         if not self.checkJointsLen(jointAnglesWithSpeedResult.goal_position):
00339             self.jointAnglesServer.set_aborted(jointAnglesWithSpeedResult)
00340             rospy.logerr("JointAnglesWithSpeed action error in result: sizes mismatch")
00341 
00342         elif running:
00343             self.jointAnglesServer.set_preempted(jointAnglesWithSpeedResult)
00344             rospy.logdebug("JointAnglesWithSpeed preempted")
00345 
00346         else:
00347             self.jointAnglesServer.set_succeeded(jointAnglesWithSpeedResult)
00348             rospy.loginfo("JointAnglesWithSpeed action done")
00349 
00350     def checkJointsLen(self, goal_position):
00351         if len(goal_position.name) == 1 and self.collectionSize.has_key(goal_position.name[0]):
00352             return len(goal_position.position) == self.collectionSize[goal_position.name[0]]
00353         else:
00354             return len(goal_position.position) ==  len(goal_position.name)
00355 
00356     def executeBodyPoseWithSpeed(self, goal):
00357 
00358         #~ Sanity checks
00359         if (goal.speed < 0.0) or (goal.speed > 1.0):
00360             bodyPoseWithSpeedResult = BodyPoseWithSpeedResult()
00361             self.bodyPoseWithSpeedServer.set_aborted(bodyPoseWithSpeedResult)
00362             rospy.logerr("Body pose setter: Not a valid speed value.")
00363             return
00364 
00365         valid_postures = self.robotPostureProxy.getPostureList()
00366 
00367         if goal.posture_name not in valid_postures:
00368             bodyPoseWithSpeedResult = BodyPoseWithSpeedResult()
00369             self.bodyPoseWithSpeedServer.set_aborted(bodyPoseWithSpeedResult)
00370             rospy.logerr("Body pose setter: Not a valid posture.")
00371             return
00372 
00373         #~ Must set stiffness on
00374         try:
00375             self.motionProxy.stiffnessInterpolation("Body", 1.0, 0.5)
00376             rospy.loginfo("Body stiffness enabled")
00377         except RuntimeError,e:
00378             rospy.logerr("Exception caught:\n%s", e)
00379             return
00380 
00381         #~ Go to posture. This is blocking
00382         self.robotPostureProxy.goToPosture(goal.posture_name, goal.speed)
00383         #~ Return success
00384         self.bodyPoseWithSpeedServer.set_succeeded()
00385 
00386 if __name__ == '__main__':
00387 
00388     controller = PoseController()
00389     rospy.loginfo("nao pose_controller running...")
00390     rospy.spin()
00391 
00392     rospy.loginfo("nao pose_controller stopped.")
00393     exit(0)


nao_pose
Author(s): Armin Hornung, Séverin Lemaignan
autogenerated on Sat Jun 27 2015 11:22:34