Go to the documentation of this file.00001
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
00052 rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)
00053
00054
00055 self.skeleton = dict()
00056 self.skeleton['confidence'] = dict()
00057 self.skeleton['position'] = dict()
00058 self.skeleton['orientation'] = dict()
00059
00060
00061 self.set_command = rospy.Service('~set_command', SetCommand, self.set_command_callback)
00062
00063
00064 if not self.use_real_robot:
00065 self.joint_state_pub = rospy.Publisher('/joint_states', JointState)
00066 else:
00067
00068 self.cmd_joints_pub = rospy.Publisher('/cmd_joints', JointState)
00069
00070
00071 self.cmd_joints = JointState()
00072 self.cmd_joints = PTL.get_joints()
00073
00074
00075 self.last_cmd_joints = JointState()
00076 self.last_cmd_joints = PTL.get_joints()
00077
00078
00079 self.tracker_command = "STOP"
00080
00081 while not rospy.is_shutdown():
00082
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
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
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
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
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
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
00189
00190
00191
00192 left_arm_roll_angle = acos(KDL.dot(left_shoulder_elbow, left_shoulder_neck))
00193
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
00198
00199
00200
00201 except KeyError:
00202 pass
00203
00204
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
00239
00240
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