pi_tracker: tracker_joint_controller.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004     Control the joints of a robot using a skeleton tracker such as the
00005     OpenNI tracker package in junction with a Kinect RGB-D camera.
00006     
00007     Based on Taylor Veltrop's C++ work (http://www.ros.org/wiki/veltrop-ros-pkg)
00008     
00009     Created for the Pi Robot Project: http://www.pirobot.org
00010     Copyright (c) 2011 Patrick Goebel.  All rights reserved.
00011 
00012     This program is free software; you can redistribute it and/or modify
00013     it under the terms of the GNU General Public License as published by
00014     the Free Software Foundation; either version 2 of the License, or
00015     (at your option) any later version.
00016     
00017     This program is distributed in the hope that it will be useful,
00018     but WITHOUT ANY WARRANTY; without even the implied warranty of
00019     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020     GNU General Public License for more details at:
00021     
00022     http://www.gnu.org/licenses/gpl.html
00023 """
00024 
00025 import rospy
00026 import pi_tracker_lib as PTL
00027 from sensor_msgs.msg import JointState
00028 from skeleton_markers.msg import Skeleton
00029 from dynamixel_controllers.srv import TorqueEnable, SetTorqueLimit, SetSpeed
00030 from std_msgs.msg import Float64
00031 from pi_tracker.srv import *
00032 import PyKDL as KDL
00033 from math import acos, asin, pi
00034 
00035 class TrackerJointController():
00036     def __init__(self):
00037         rospy.init_node('tracker_joint_controller')
00038         rospy.on_shutdown(self.shutdown)
00039         
00040         rospy.loginfo("Initializing Joint Controller Node...")
00041         
00042         self.rate = rospy.get_param('~joint_controller_rate', 5)
00043         
00044         rate = rospy.Rate(self.rate)
00045         
00046         # The namespace may be set by the servo controller (usually null)
00047         namespace = rospy.get_namespace()
00048         
00049         self.joints = rospy.get_param(namespace + '/joints', '')
00050         
00051         self.skel_to_joint_map = rospy.get_param("~skel_to_joint_map", dict())
00052         
00053         self.use_real_robot = rospy.get_param('~use_real_robot', False)
00054         
00055         self.default_joint_speed = rospy.get_param('~default_joint_speed', 0.5)
00056         
00057         self.tracker_commands = rospy.get_param('~tracker_commands', ['STOP', 'TELEOP_JOINTS'])
00058          
00059         self.HALF_PI = pi / 2.0
00060 
00061         # Subscribe to the skeleton topic.
00062         rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00063         
00064         # Store the current skeleton configuration in a local dictionary.
00065         self.skeleton = dict()
00066         self.skeleton['confidence'] = dict()
00067         self.skeleton['position'] = dict()
00068         self.skeleton['orientation'] = dict()
00069          
00070         # Set up the tracker command service for this node.
00071         self.set_command = rospy.Service('~set_command', SetCommand, self.set_command_callback)
00072 
00073         # Use the joint_state publisher for a fake robot
00074         if not self.use_real_robot:
00075             self.joint_state_pub = rospy.Publisher('/joint_states', JointState, queue_size=5)      
00076         else:
00077             # Initialize servo controllers for a real robot
00078             self.init_servos()
00079         
00080         # The get_joints command parses a URDF description of the robot to get all the non-fixed joints.
00081         self.cmd_joints = PTL.get_joints()
00082         
00083         # Store the last joint command so we can stop and hold a given posture.
00084         self.last_cmd_joints = PTL.get_joints()
00085         
00086         # Initialize the robot in the stopped state.
00087         self.tracker_command = "STOP"
00088         
00089         while not rospy.is_shutdown():              
00090             # Execute the behavior appropriate for the current command.
00091             if self.tracker_command in self.tracker_commands:
00092                 if self.tracker_command == 'STOP':
00093                     self.stop_joints()
00094                 else:
00095                     self.teleop_joints()
00096                 
00097                 self.cmd_joints.header.stamp = rospy.Time.now()
00098                 
00099                 if self.use_real_robot:
00100                     try:
00101                         self.pub_joint_cmds(self.cmd_joints)
00102                     except:
00103                         pass
00104                 else:
00105                      self.joint_state = self.cmd_joints
00106                      self.joint_state_pub.publish(self.joint_state)
00107             else:
00108                 pass
00109                 
00110             self.last_cmd_joints = self.cmd_joints
00111             
00112             rate.sleep()
00113             
00114     def stop_joints(self):
00115         self.cmd_joints = self.last_cmd_joints
00116         
00117     def relax_joints(self):
00118         pass
00119         
00120     def reset_joints(self):
00121         pass
00122         
00123     def enable_joints(self):
00124         pass
00125             
00126     def teleop_joints(self):
00127         # Fixed Joints: Set position to 0 radians.
00128         try:
00129             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_shoulder_roll_joint')] = 0
00130             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_shoulder_roll_joint')] = 2
00131             
00132             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_wrist_flex_joint')] = 0
00133             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_wrist_flex_joint')] = 2
00134         except KeyError:
00135             pass
00136         
00137         try:
00138             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_shoulder_roll_joint')] = 0
00139             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_shoulder_roll_joint')] = 2
00140             
00141             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_wrist_flex_joint')] = 0
00142             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_wrist_flex_joint')] = 2
00143         except KeyError:
00144             pass
00145                     
00146         # Torso Rotation          
00147         try:
00148             torso_quaternion = self.skeleton['orientation']['torso']
00149             torso_rpy = torso_quaternion.GetRPY()
00150             self.cmd_joints.position[self.cmd_joints.name.index(self.skel_to_joint_map['torso'])] = torso_rpy[1] / 2.0
00151             self.cmd_joints.velocity[self.cmd_joints.name.index('torso_joint')] = self.default_joint_speed
00152         except:
00153             pass
00154         
00155         # Head Pan           
00156         try:
00157             head_quaternion = self.skeleton['orientation']['head']
00158             head_rpy = head_quaternion.GetRPY()
00159             self.cmd_joints.position[self.cmd_joints.name.index('head_pan_joint')] = head_rpy[1]
00160             self.cmd_joints.velocity[self.cmd_joints.name.index('head_pan_joint')] = self.default_joint_speed
00161         except:
00162             pass
00163         
00164         # Head Tilt
00165         try:
00166             self.cmd_joints.position[self.cmd_joints.name.index('head_tilt_joint')] = -head_rpy[0]
00167             self.cmd_joints.velocity[self.cmd_joints.name.index('head_tilt_joint')] = self.default_joint_speed
00168         except:
00169             pass
00170         
00171         # Left Arm
00172         try:
00173             left_shoulder_neck = self.skeleton['position']['neck'] - self.skeleton['position']['left_shoulder']
00174             left_shoulder_elbow = self.skeleton['position']['left_elbow'] - self.skeleton['position']['left_shoulder']
00175             left_arm_elbow_flex_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_elbow']
00176             left_shoulder_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_shoulder']
00177             
00178             left_shoulder_neck.Normalize()
00179             left_shoulder_elbow.Normalize()
00180             lh = left_arm_elbow_flex_hand.Normalize()
00181             left_shoulder_hand.Normalize()                           
00182 
00183             left_arm_shoulder_lift_angle = asin(left_shoulder_elbow.y()) + self.HALF_PI           
00184             left_arm_shoulder_pan_angle = asin(left_arm_elbow_flex_hand.x())
00185             left_arm_elbow_flex_angle = -acos(KDL.dot(left_shoulder_elbow, left_arm_elbow_flex_hand))
00186             left_arm_wrist_flex_angle = -left_arm_elbow_flex_angle / 2.0
00187             left_arm_elbow_flex_angle = left_arm_elbow_flex_angle / 4.0
00188             
00189             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_shoulder_lift_joint')] = left_arm_shoulder_lift_angle
00190             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_shoulder_lift_joint')] = self.default_joint_speed
00191             
00192             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_shoulder_pan_joint')] = -left_arm_shoulder_pan_angle
00193             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_shoulder_pan_joint')] = self.default_joint_speed
00194             
00195             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_elbow_flex_joint')] = left_arm_elbow_flex_angle
00196             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_elbow_flex_joint')] = self.default_joint_speed                                   
00197         
00198             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_wrist_flex_joint')] = left_arm_wrist_flex_angle
00199             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_wrist_flex_joint')] = self.default_joint_speed
00200             
00201 #                left_arm_elbow_flex_rpy = [0]*3
00202 #                left_arm_elbow_flex_rpy = euler_from_quaternion(self.skeleton['orientation']['left_elbow'])
00203 #                left_arm_shoulder_roll_angle = -left_arm_elbow_flex_rpy[2]
00204 
00205             left_arm_shoulder_roll_angle = acos(KDL.dot(left_shoulder_elbow, left_shoulder_neck))
00206             #left_arm_shoulder_roll_angle = -asin(left_shoulder_hand.x())
00207             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_shoulder_roll_joint')] = 0
00208             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_shoulder_roll_joint')] = self.default_joint_speed
00209         
00210 #                left_arm_wrist_flex_angle = -acos(min(1, abs((lh - 0.265) / (0.30 - 0.265)))) + QUARTER_PI
00211 #                self.cmd_joints.position[self.cmd_joints.name.index('left_arm_wrist_flex_joint')] = 0
00212 #                self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_wrist_flex_joint')] = self.default_joint_speed
00213         
00214         except KeyError:
00215             pass      
00216             
00217         # Right Arm
00218         try:
00219             right_shoulder_neck = self.skeleton['position']['neck'] - self.skeleton['position']['right_shoulder']
00220             right_shoulder_elbow = self.skeleton['position']['right_elbow'] - self.skeleton['position']['right_shoulder']
00221             right_arm_elbow_flex_hand = self.skeleton['position']['right_hand'] - self.skeleton['position']['right_elbow']
00222             right_shoulder_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_shoulder']
00223             
00224             right_shoulder_neck.Normalize()
00225             right_shoulder_elbow.Normalize()
00226             rh = right_arm_elbow_flex_hand.Normalize()
00227             right_shoulder_hand.Normalize()                
00228 
00229             right_arm_shoulder_lift_angle = -(asin(right_shoulder_elbow.y()) + self.HALF_PI)
00230             right_arm_shoulder_pan_angle = -asin(right_arm_elbow_flex_hand.x())
00231             right_arm_elbow_flex_angle = acos(KDL.dot(right_shoulder_elbow, right_arm_elbow_flex_hand))
00232             right_arm_wrist_flex_angle = -right_arm_elbow_flex_angle / 2.0
00233             right_arm_elbow_flex_angle = right_arm_elbow_flex_angle / 4.0
00234                             
00235             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_shoulder_lift_joint')] = right_arm_shoulder_lift_angle
00236             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_shoulder_lift_joint')] = self.default_joint_speed
00237             
00238             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_shoulder_pan_joint')] = right_arm_shoulder_pan_angle
00239             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_shoulder_pan_joint')] = self.default_joint_speed
00240             
00241             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_elbow_flex_joint')] = right_arm_elbow_flex_angle
00242             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_elbow_flex_joint')] = self.default_joint_speed
00243             
00244             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_wrist_flex_joint')] = right_arm_wrist_flex_angle
00245             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_wrist_flex_joint')] = self.default_joint_speed
00246  
00247             right_arm_shoulder_roll_angle = -asin(right_shoulder_hand.x())
00248             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_shoulder_roll_joint')] = 0
00249             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_shoulder_roll_joint')] = self.default_joint_speed
00250             
00251 #                right_arm_wrist_flex_angle = acos(min(1, abs((rh - 0.265) / (0.30 - 0.265)))) - QUARTER_PI
00252 #                self.cmd_joints.position[self.cmd_joints.name.index('right_arm_wrist_flex_joint')] = 0
00253 #                self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_wrist_flex_joint')] = self.default_joint_speed
00254             
00255         except KeyError:
00256             pass
00257     
00258     def pub_joint_cmds(self, cmd_joints):
00259         # First set the joint speeds
00260         for joint in sorted(self.joints):
00261             self.servo_speed[joint](cmd_joints.velocity[self.cmd_joints.name.index(joint)])
00262             
00263         # Then set the joint positions
00264         for joint in sorted(self.joints):
00265             self.servo_position[joint].publish(cmd_joints.position[self.cmd_joints.name.index(joint)])
00266             
00267     def skeleton_handler(self, msg):
00268         for joint in msg.name:  
00269             self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
00270             self.skeleton['position'][joint] = KDL.Vector(msg.position[msg.name.index(joint)].x, msg.position[msg.name.index(joint)].y, msg.position[msg.name.index(joint)].z)
00271             self.skeleton['orientation'][joint] = KDL.Rotation.Quaternion(msg.orientation[msg.name.index(joint)].x, msg.orientation[msg.name.index(joint)].y, msg.orientation[msg.name.index(joint)].z, msg.orientation[msg.name.index(joint)].w)
00272             
00273     def joint_state_handler(self, msg):
00274         for joint in msg.name:  
00275             self.joint_state = msg
00276             
00277     def set_command_callback(self, req):
00278         self.tracker_command = req.command
00279         return SetCommandResponse()
00280     
00281     def init_servos(self):
00282         # Create dictionaries to hold the speed, position and torque controllers
00283         self.servo_speed = dict()
00284         self.servo_position = dict()
00285         self.relax_servo = dict()
00286 
00287         # Connect to the set_speed services and define a position publisher for each servo
00288         rospy.loginfo("Waiting for joint controllers services...")
00289                 
00290         for joint in sorted(self.joints):
00291             # The set_speed services
00292             set_speed_service = '/' + joint + '/set_speed'
00293             rospy.wait_for_service(set_speed_service)
00294             self.servo_speed[joint] = rospy.ServiceProxy(set_speed_service, SetSpeed, persistent=True)
00295 
00296             # Initialize the servo speed to the default_joint_speed
00297             self.servo_speed[joint](self.default_joint_speed)
00298 
00299             # The position controllers
00300             self.servo_position[joint] = rospy.Publisher('/' + joint + '/command', Float64, queue_size=5)
00301             
00302             # A service to relax (disable torque) a servo
00303             relax_service = '/' + joint + '/torque_enable'
00304             rospy.wait_for_service(relax_service) 
00305             self.relax_servo[joint] = rospy.ServiceProxy(relax_service, TorqueEnable)
00306             self.relax_servo[joint](False)
00307             
00308         rospy.loginfo("Servos are ready.")
00309 
00310     def shutdown(self):
00311         rospy.loginfo('Shutting down Tracker Joint Controller Node.')
00312         
00313 if __name__ == '__main__':
00314     try:
00315         TrackerJointController()
00316     except rospy.ROSInterruptException:
00317         pass