pi_tracker: pi_tracker: tracker_joint_controller.py Source 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 roslib; roslib.load_manifest('pi_tracker')
00026 import rospy
00027 import pi_tracker_lib as PTL
00028 from sensor_msgs.msg import JointState
00029 from pi_tracker.msg import Skeleton
00030 from pi_tracker.srv import *
00031 import PyKDL as KDL
00032 from math import acos, asin, pi
00033 
00034 class TrackerJointController():
00035     def __init__(self):
00036         rospy.init_node('tracker_joint_controller')
00037         rospy.on_shutdown(self.shutdown)
00038         
00039         rospy.loginfo("Initializing Joint Controller Node...")
00040         
00041         self.rate = rospy.get_param('~joint_controller_rate', 5)
00042         rate = rospy.Rate(self.rate)
00043         
00044         self.skel_to_joint_map = rospy.get_param("~skel_to_joint_map", dict())
00045         self.use_real_robot = rospy.get_param('~use_real_robot', False)
00046         self.default_joint_speed = rospy.get_param('~default_joint_speed', 0.5)
00047         self.tracker_commands = rospy.get_param('~tracker_commands', ['STOP', 'TELEOP_JOINTS'])
00048          
00049         self.HALF_PI = pi / 2.0
00050 
00051         # Subscribe to the skeleton topic.
00052         rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00053         
00054         # Store the current skeleton configuration in a local dictionary.
00055         self.skeleton = dict()
00056         self.skeleton['confidence'] = dict()
00057         self.skeleton['position'] = dict()
00058         self.skeleton['orientation'] = dict()
00059          
00060         # Set up the tracker command service for this node.
00061         self.set_command = rospy.Service('~set_command', SetCommand, self.set_command_callback)
00062 
00063         # We only use the joint_state publisher if we are not using a real robot (so we can see them in RViz)
00064         if not self.use_real_robot:
00065             self.joint_state_pub = rospy.Publisher('/joint_states', JointState)      
00066         else:
00067             # The joints are controlled by publishing a joint state message on the /cmd_joints topic.
00068             self.cmd_joints_pub = rospy.Publisher('/cmd_joints', JointState)
00069         
00070         # The get_joints command parses a URDF description of the robot to get all the non-fixed joints.
00071         self.cmd_joints = JointState()
00072         self.cmd_joints = PTL.get_joints()
00073         
00074         # Store the last joint command so we can stop and hold a given posture.
00075         self.last_cmd_joints = JointState()
00076         self.last_cmd_joints = PTL.get_joints()
00077         
00078         # Initialize the robot in the stopped state.
00079         self.tracker_command = "STOP"
00080         
00081         while not rospy.is_shutdown():              
00082             # Execute the behavior appropriate for the current command.
00083             if self.tracker_command in self.tracker_commands:
00084                 if self.tracker_command == 'STOP':
00085                     self.stop_joints()
00086                 else:
00087                     self.teleop_joints()
00088                 
00089                 self.cmd_joints.header.stamp = rospy.Time.now()
00090                 
00091                 if self.use_real_robot:
00092                     try:
00093                         self.cmd_joints_pub.publish(self.cmd_joints)
00094                     except:
00095                         pass
00096                 else:
00097                      self.joint_state = self.cmd_joints
00098                      self.joint_state_pub.publish(self.joint_state)
00099             else:
00100                 pass
00101                 
00102             self.last_cmd_joints = self.cmd_joints
00103             
00104             rate.sleep()
00105             
00106     def stop_joints(self):
00107         self.cmd_joints = self.last_cmd_joints
00108         
00109     def relax_joints(self):
00110         pass
00111         
00112     def reset_joints(self):
00113         pass
00114         
00115     def enable_joints(self):
00116         pass
00117             
00118     def teleop_joints(self):
00119         # Fixed Joints: Set position to 0 radians.
00120         self.cmd_joints.position[self.cmd_joints.name.index('left_arm_roll_joint')] = 0
00121         self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_roll_joint')] = 2
00122         
00123         self.cmd_joints.position[self.cmd_joints.name.index('right_arm_roll_joint')] = 0
00124         self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_roll_joint')] = 2
00125         
00126         self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = 0
00127         self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = 2
00128         
00129         self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = 0
00130         self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = 2
00131         
00132                     
00133         # Torso Rotation          
00134         try:
00135             torso_quaternion = self.skeleton['orientation']['torso']
00136             torso_rpy = torso_quaternion.GetRPY()
00137             self.cmd_joints.position[self.cmd_joints.name.index(self.skel_to_joint_map['torso'])] = torso_rpy[1] / 2.0
00138             self.cmd_joints.velocity[self.cmd_joints.name.index('torso_joint')] = self.default_joint_speed
00139         except:
00140             pass
00141         
00142         # Head Pan           
00143         try:
00144             head_quaternion = self.skeleton['orientation']['head']
00145             head_rpy = head_quaternion.GetRPY()
00146             self.cmd_joints.position[self.cmd_joints.name.index('head_pan_joint')] = head_rpy[1]
00147             self.cmd_joints.velocity[self.cmd_joints.name.index('head_pan_joint')] = self.default_joint_speed
00148         except:
00149             pass
00150         
00151         # Head Tilt
00152         try:
00153             self.cmd_joints.position[self.cmd_joints.name.index('head_tilt_joint')] = head_rpy[0]
00154             self.cmd_joints.velocity[self.cmd_joints.name.index('head_tilt_joint')] = self.default_joint_speed
00155         except:
00156             pass
00157         
00158         # Left Arm
00159         try:
00160             left_shoulder_neck = self.skeleton['position']['neck'] - self.skeleton['position']['left_shoulder']
00161             left_shoulder_elbow = self.skeleton['position']['left_elbow'] - self.skeleton['position']['left_shoulder']
00162             left_elbow_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_elbow']
00163             left_shoulder_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_shoulder']
00164             
00165             left_shoulder_neck.Normalize()
00166             left_shoulder_elbow.Normalize()
00167             lh = left_elbow_hand.Normalize()
00168             left_shoulder_hand.Normalize()                           
00169 
00170             left_shoulder_lift_angle = asin(left_shoulder_elbow.y()) + self.HALF_PI           
00171             left_shoulder_pan_angle = asin(left_elbow_hand.x())
00172             left_elbow_angle = -acos(KDL.dot(left_shoulder_elbow, left_elbow_hand))
00173             left_wrist_angle = -left_elbow_angle / 2.0
00174             left_elbow_angle = left_elbow_angle / 4.0
00175             
00176             self.cmd_joints.position[self.cmd_joints.name.index('left_shoulder_lift_joint')] = left_shoulder_lift_angle
00177             self.cmd_joints.velocity[self.cmd_joints.name.index('left_shoulder_lift_joint')] = self.default_joint_speed
00178             
00179             self.cmd_joints.position[self.cmd_joints.name.index('left_shoulder_pan_joint')] = left_shoulder_pan_angle
00180             self.cmd_joints.velocity[self.cmd_joints.name.index('left_shoulder_pan_joint')] = self.default_joint_speed
00181             
00182             self.cmd_joints.position[self.cmd_joints.name.index('left_elbow_joint')] = left_elbow_angle
00183             self.cmd_joints.velocity[self.cmd_joints.name.index('left_elbow_joint')] = self.default_joint_speed                                   
00184         
00185             self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = left_wrist_angle
00186             self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = self.default_joint_speed
00187             
00188 #                left_elbow_rpy = [0]*3
00189 #                left_elbow_rpy = euler_from_quaternion(self.skeleton['orientation']['left_elbow'])
00190 #                left_arm_roll_angle = -left_elbow_rpy[2]
00191 
00192             left_arm_roll_angle = acos(KDL.dot(left_shoulder_elbow, left_shoulder_neck))
00193             #left_arm_roll_angle = -asin(left_shoulder_hand.x())
00194             self.cmd_joints.position[self.cmd_joints.name.index('left_arm_roll_joint')] = 0
00195             self.cmd_joints.velocity[self.cmd_joints.name.index('left_arm_roll_joint')] = self.default_joint_speed
00196         
00197 #                left_wrist_angle = -acos(min(1, abs((lh - 0.265) / (0.30 - 0.265)))) + QUARTER_PI
00198 #                self.cmd_joints.position[self.cmd_joints.name.index('left_wrist_joint')] = 0
00199 #                self.cmd_joints.velocity[self.cmd_joints.name.index('left_wrist_joint')] = self.default_joint_speed
00200         
00201         except KeyError:
00202             pass      
00203             
00204         # Right Arm
00205         try:
00206             right_shoulder_neck = self.skeleton['position']['neck'] - self.skeleton['position']['right_shoulder']
00207             right_shoulder_elbow = self.skeleton['position']['right_elbow'] - self.skeleton['position']['right_shoulder']
00208             right_elbow_hand = self.skeleton['position']['right_hand'] - self.skeleton['position']['right_elbow']
00209             right_shoulder_hand = self.skeleton['position']['left_hand'] - self.skeleton['position']['left_shoulder']
00210             
00211             right_shoulder_neck.Normalize()
00212             right_shoulder_elbow.Normalize()
00213             rh = right_elbow_hand.Normalize()
00214             right_shoulder_hand.Normalize()                
00215 
00216             right_shoulder_lift_angle = -(asin(right_shoulder_elbow.y()) + self.HALF_PI)
00217             right_shoulder_pan_angle = -asin(right_elbow_hand.x())
00218             right_elbow_angle = acos(KDL.dot(right_shoulder_elbow, right_elbow_hand))
00219             right_wrist_angle = -right_elbow_angle / 2.0
00220             right_elbow_angle = right_elbow_angle / 4.0
00221                             
00222             self.cmd_joints.position[self.cmd_joints.name.index('right_shoulder_lift_joint')] = right_shoulder_lift_angle
00223             self.cmd_joints.velocity[self.cmd_joints.name.index('right_shoulder_lift_joint')] = self.default_joint_speed
00224             
00225             self.cmd_joints.position[self.cmd_joints.name.index('right_shoulder_pan_joint')] = right_shoulder_pan_angle
00226             self.cmd_joints.velocity[self.cmd_joints.name.index('right_shoulder_pan_joint')] = self.default_joint_speed
00227             
00228             self.cmd_joints.position[self.cmd_joints.name.index('right_elbow_joint')] = right_elbow_angle
00229             self.cmd_joints.velocity[self.cmd_joints.name.index('right_elbow_joint')] = self.default_joint_speed
00230             
00231             self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = right_wrist_angle
00232             self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = self.default_joint_speed
00233  
00234             right_arm_roll_angle = -asin(right_shoulder_hand.x())
00235             self.cmd_joints.position[self.cmd_joints.name.index('right_arm_roll_joint')] = 0
00236             self.cmd_joints.velocity[self.cmd_joints.name.index('right_arm_roll_joint')] = self.default_joint_speed
00237             
00238 #                right_wrist_angle = acos(min(1, abs((rh - 0.265) / (0.30 - 0.265)))) - QUARTER_PI
00239 #                self.cmd_joints.position[self.cmd_joints.name.index('right_wrist_joint')] = 0
00240 #                self.cmd_joints.velocity[self.cmd_joints.name.index('right_wrist_joint')] = self.default_joint_speed
00241             
00242         except KeyError:
00243             pass          
00244             
00245     def skeleton_handler(self, msg):
00246         for joint in msg.name:  
00247             self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
00248             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)
00249             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)
00250             
00251     def joint_state_handler(self, msg):
00252         for joint in msg.name:  
00253             self.joint_state = msg
00254             
00255     def set_command_callback(self, req):
00256         self.tracker_command = req.command
00257         return SetCommandResponse()
00258 
00259     def shutdown(self):
00260         rospy.loginfo('Shutting down Tracker Joint Controller Node.')
00261         
00262 if __name__ == '__main__':
00263     try:
00264         TrackerJointController()
00265     except rospy.ROSInterruptException:
00266         pass