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


naoqi_pose
Author(s): Armin Hornung, Séverin Lemaignan
autogenerated on Thu Aug 27 2015 14:05:40