00001
00002
00003 """
00004 Copyright (c) 2012, General Motors, Co.
00005 All rights reserved.
00006
00007 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00008 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00009 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00010 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00011 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00012 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00013 SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS
00014 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00015 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00016 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00017 POSSIBILITY OF SUCH DAMAGE.
00018 """
00019
00020 import roslib;
00021 roslib.load_manifest('r2_teleop')
00022
00023 roslib.load_manifest("r2_gazebo")
00024
00025 import rospy
00026 import math
00027 import time
00028 import PyKDL as kdl
00029
00030 import marker_helper
00031
00032 from sensor_msgs.msg import JointState
00033 from geometry_msgs.msg import Pose
00034 from geometry_msgs.msg import PoseStamped
00035
00036 from r2_msgs.srv import *
00037
00038
00039
00040 from interactive_markers.interactive_marker_server import *
00041 from interactive_markers.menu_handler import *
00042
00043 TORAD = math.pi/180.0
00044 TODEG = 1.0/TORAD
00045
00046
00047 left_pose_pub = rospy.Publisher('r2_controller/left/pose_command', PoseStamped)
00048 right_pose_pub = rospy.Publisher('r2_controller/right/pose_command', PoseStamped)
00049 gaze_pose_pub = rospy.Publisher('r2_controller/gaze/pose_command', PoseStamped)
00050
00051
00052 left_jnt_pub = rospy.Publisher('r2_controller/left_arm/joint_command', JointState)
00053 right_jnt_pub = rospy.Publisher('r2_controller/right_arm/joint_command', JointState)
00054 neck_jnt_pub = rospy.Publisher('r2_controller/neck/joint_command', JointState)
00055 waist_jnt_pub = rospy.Publisher('r2_controller/waist/joint_command', JointState)
00056
00057
00058 left_palm_mesh = "package://r2_description/meshes/Left_Palm.dae"
00059 left_thumb_carp_mesh = "package://r2_description/meshes/Left_Thumb_Carp.dae"
00060 left_thumb_carp_mtcar = "package://r2_description/meshes/Left_Thumb_MtCar.dae"
00061
00062 right_palm_mesh = "package://r2_description/meshes/Right_Palm.dae"
00063 right_thumb_carp_mesh = "package://r2_description/meshes/Left_Thumb_Carp.dae"
00064 right_thumb_carp_mtcar = "package://r2_description/meshes/Right_Thumb_MtCar.dae"
00065
00066 thumb_prox_mesh = "package://r2_description/meshes/Thumb_Proximal.dae"
00067 thumb_dist_mesh = "package://r2_description/meshes/Thumb_Dist.dae"
00068 finger_prox_mesh = "package://r2_description/meshes/Finger_Proximal.dae"
00069 finger_mid_mesh = "package://r2_description/meshes/Finger_Mid.dae"
00070 finger_dist_mesh = "package://r2_description/meshes/Finger_Dist.dae"
00071
00072 body_mesh = "package://r2_description/meshes/Body_Cover.dae"
00073 head_mesh = "package://r2_description/meshes/Head.dae"
00074 backpack_mesh = "package://r2_description/meshes/Backpack.dae"
00075
00076
00077 left_palm_mesh_pose = Pose()
00078 right_palm_mesh_pose = Pose()
00079 waist_mesh_pose = Pose()
00080 head_mesh_pose = Pose()
00081 backpack_mesh_pose = Pose()
00082
00083
00084 def SetArmsToCartMode(left_frame, right_frame) :
00085 SetArmToCartMode('left', left_frame)
00086 SetArmToCartMode('right', right_frame)
00087
00088 def SetArmToCartMode(arm, frame) :
00089
00090 rospy.wait_for_service('r2_controller/set_tip_name')
00091 set_tip_name = rospy.ServiceProxy('r2_controller/set_tip_name', SetTipName)
00092
00093 print "setting ", arm, " tip to: ", frame
00094 try:
00095 resp1 = set_tip_name(arm, frame)
00096 print "Set Tip Name: ", resp1.result
00097 except rospy.ServiceException, e:
00098 print "Service call failed for ", arm, " arm: %s"%e
00099
00100 def SetArmToJointMode(arm) :
00101
00102 rospy.wait_for_service('r2_controller/set_joint_mode')
00103 set_joint_mode = rospy.ServiceProxy('r2_controller/set_joint_mode', SetJointMode)
00104
00105 print "setting ", arm, " to joint mode"
00106 try:
00107 resp1 = set_joint_mode(arm)
00108 print "Set ", arm, " joint mode: ", resp1.result
00109 except rospy.ServiceException, e:
00110 print "Service call failed for ", arm, " arm: %s"%e
00111
00112 def SetPower(p) :
00113
00114 rospy.wait_for_service('r2_controller/power')
00115 power = rospy.ServiceProxy('r2_controller/power', Power)
00116
00117 print "setting motor power to: ", p
00118 try:
00119 resp1 = power('motor', p)
00120 print "Set Power: ", resp1.status
00121 except rospy.ServiceException, e:
00122 print "Service call failed for setting power: %s"%e
00123
00124 return resp1.status
00125
00126 def Servo(s, mode) :
00127
00128 rospy.wait_for_service('r2_controller/servo')
00129 Servo = rospy.ServiceProxy('r2_controller/servo', Servo)
00130
00131 print "servoing: ", s, " in mode: ", m
00132 try:
00133 resp1 = servo(s, mode)
00134 print "Servo Status: ", resp1.status
00135 except rospy.ServiceException, e:
00136 print "Service call failed for servoing: %s"%e
00137
00138 return resp1.status
00139
00140 def SegmentTableTop() :
00141
00142 rospy.wait_for_service('tabletop_segmentation/take_snapshot')
00143 take_snapshot = rospy.ServiceProxy('tabletop_segmentation/take_snapshot', TakeSnapshot)
00144
00145 print "taking snapshot"
00146 try:
00147 resp1 = take_snapshot(True)
00148 except rospy.ServiceException, e:
00149 print "Service call failed for taking snapshot %s"%e
00150
00151 def ParseTableScene() :
00152
00153 rospy.wait_for_service('tabletop_clustering/parse_scene')
00154 parse_scene = rospy.ServiceProxy('tabletop_clustering/parse_scene', ParseTableScene)
00155
00156 rospy.loginfo("parsing table scene")
00157 try:
00158 resp1 = parse_scene(True)
00159 print "response : ", resp
00160 except rospy.ServiceException, e:
00161 print "Service call failed for parsing table scene %s"%e
00162
00163
00164
00165 leftCartReadyPose = PoseStamped()
00166 leftCartReadyPose.header.seq = 0
00167 leftCartReadyPose.header.stamp = 0
00168 leftCartReadyPose.header.frame_id = "r2/waist_center"
00169 leftCartReadyPose.pose.position.x = 0.27
00170 leftCartReadyPose.pose.position.y = -0.35
00171 leftCartReadyPose.pose.position.z = -.22
00172 leftCartReadyPose.pose.orientation.x = -0.707
00173 leftCartReadyPose.pose.orientation.y = 0
00174 leftCartReadyPose.pose.orientation.z = 0
00175 leftCartReadyPose.pose.orientation.w = 0.707
00176
00177 rightCartReadyPose = PoseStamped()
00178 rightCartReadyPose.header.seq = 0
00179 rightCartReadyPose.header.stamp = 0
00180 rightCartReadyPose.header.frame_id = "r2/waist_center"
00181 rightCartReadyPose.pose.position.x = 0.27
00182 rightCartReadyPose.pose.position.y = 0.35
00183 rightCartReadyPose.pose.position.z = -.22
00184 rightCartReadyPose.pose.orientation.x = 0.707
00185 rightCartReadyPose.pose.orientation.y = 0
00186 rightCartReadyPose.pose.orientation.z = 0
00187 rightCartReadyPose.pose.orientation.w = 0.707
00188
00189
00190 leftArmJointNames = ['/r2/left_arm/joint'+str(i) for i in range(7)]
00191 leftThumbJointNames = ['/r2/left_arm/hand/thumb/joint' +str(i) for i in range(4)]
00192 leftIndexJointNames = ['/r2/left_arm/hand/index/joint'+str(i) for i in range(3)]
00193 leftMiddleJointNames = ['/r2/left_arm/hand/middle/joint'+str(i) for i in range(3)]
00194 leftRingJointNames = ['/r2/left_arm/hand/ring/joint'+str(i) for i in range(1)]
00195 leftLittleJointNames = ['/r2/left_arm/hand/little/joint'+str(i) for i in range(1)]
00196 leftHandNames = leftThumbJointNames + leftIndexJointNames + leftMiddleJointNames + leftRingJointNames + leftLittleJointNames
00197 leftJointNames = leftArmJointNames + leftHandNames
00198
00199 rightArmJointNames = ['/r2/right_arm/joint'+str(i) for i in range(7)]
00200 rightThumbJointNames = ['/r2/right_arm/hand/thumb/joint'+str(i) for i in range(4)]
00201 rightIndexJointNames = ['/r2/right_arm/hand/index/joint'+str(i) for i in range(3)]
00202 rightMiddleJointNames = ['/r2/right_arm/hand/middle/joint'+str(i) for i in range(3)]
00203 rightRingJointNames = ['/r2/right_arm/hand/ring/joint'+str(i) for i in range(1)]
00204 rightLittleJointNames = ['/r2/right_arm/hand/little/joint'+str(i) for i in range(1)]
00205 rightHandNames = rightThumbJointNames + rightIndexJointNames + rightMiddleJointNames + rightRingJointNames + rightLittleJointNames
00206 rightJointNames = rightArmJointNames + rightHandNames
00207
00208 neckJointNames = ['/r2/neck/joint0', '/r2/neck/joint1', '/r2/neck/joint2']
00209 waistJointNames = ['/r2/waist/joint0']
00210
00211 leftJointReadyPose = JointState()
00212 rightJointReadyPose = JointState()
00213 neckJointReadyPose = JointState()
00214 waistJointReadyPose = JointState()
00215
00216 leftJointReadyPose.header.seq = 0
00217 leftJointReadyPose.header.stamp = 0
00218 leftJointReadyPose.header.frame_id = "r2/waist_center"
00219 leftJointReadyPose.name = leftJointNames
00220 leftJointReadyPose.position = [50.0*TORAD, -80.0*TORAD, -105.0*TORAD, -140.0*TORAD, 80.0*TORAD, 0.0*TORAD, 0.0*TORAD]+[0*TORAD]*12
00221
00222 rightJointReadyPose.header.seq = 0
00223 rightJointReadyPose.header.stamp = 0
00224 rightJointReadyPose.header.frame_id = "world"
00225 rightJointReadyPose.name = rightJointNames
00226 rightJointReadyPose.position = [-50.0*TORAD, -80.0*TORAD, 105.0*TORAD, -140.0*TORAD, -80.0*TORAD, 0.0*TORAD, 0.0*TORAD]+[0*TORAD]*12
00227
00228 neckJointReadyPose.header.seq = 0
00229 neckJointReadyPose.header.stamp = 0
00230 neckJointReadyPose.header.frame_id = "world"
00231 neckJointReadyPose.name = neckJointNames
00232 neckJointReadyPose.position = [0.0*TORAD, 0.0*TORAD, -5.0*TORAD]
00233
00234 waistJointReadyPose.header.seq = 0
00235 waistJointReadyPose.header.stamp = 0
00236 waistJointReadyPose.header.frame_id = "world"
00237 waistJointReadyPose.name = waistJointNames
00238 waistJointReadyPose.position = [180.0*TORAD]
00239
00240 leftHandClose = JointState()
00241 leftHandClose.header.stamp = 0
00242 leftHandClose.header.frame_id = "world"
00243 leftHandClose.name = leftHandNames
00244 leftHandClose.position = [-70*TORAD, 50*TORAD, 50*TORAD, 0*TORAD, 0*TORAD, 90*TORAD, 90*TORAD, 0*TORAD, 90*TORAD, 80*TORAD, 170*TORAD, 170*TORAD]
00245
00246 rightHandClose = JointState()
00247 rightHandClose.header.stamp = 0
00248 rightHandClose.header.frame_id = "world"
00249 rightHandClose.name = rightHandNames
00250 rightHandClose.position = [70*TORAD, 50*TORAD, 50*TORAD, 0*TORAD, 0*TORAD, 90*TORAD, 90*TORAD, 0*TORAD, 90*TORAD, 80*TORAD, 170*TORAD, 170*TORAD]
00251
00252 leftHandOpen = JointState()
00253 leftHandOpen.header.stamp = 0
00254 leftHandOpen.header.frame_id = "world"
00255 leftHandOpen.name = leftHandNames
00256 leftHandOpen.position = [0*TORAD]*12
00257
00258 rightHandOpen = JointState()
00259 rightHandOpen.header.stamp = 0
00260 rightHandOpen.header.frame_id = "world"
00261 rightHandOpen.name = rightHandNames
00262 rightHandOpen.position = [0*TORAD]*12
00263
00264
00265 leftHandPowerClose = JointState()
00266
00267 leftHandPowerClose.header.frame_id = "world"
00268 leftHandPowerClose.name = leftHandNames
00269 leftHandPowerClose.position = [-100*TORAD, 0*TORAD, 0*TORAD, 0*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 170*TORAD, 170*TORAD]
00270
00271 rightHandPowerClose = JointState()
00272
00273 rightHandPowerClose.header.frame_id = "world"
00274 rightHandPowerClose.name = rightHandNames
00275 rightHandPowerClose.position = [100*TORAD, 0*TORAD, 0*TORAD, 0*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 150*TORAD, 150*TORAD]
00276
00277
00278 leftHandPowerCloseThumb = JointState()
00279
00280 leftHandPowerCloseThumb.header.frame_id = "world"
00281 leftHandPowerCloseThumb.name = leftHandNames
00282 leftHandPowerCloseThumb.position = [-200*TORAD, 100*TORAD, 100*TORAD, 50*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 170*TORAD, 170*TORAD]
00283
00284 rightHandPowerCloseThumb = JointState()
00285
00286 rightHandPowerCloseThumb.header.frame_id = "world"
00287 rightHandPowerCloseThumb.name = rightHandNames
00288 rightHandPowerCloseThumb.position = [200*TORAD, 100*TORAD, 100*TORAD, 50*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 0*TORAD, 100*TORAD, 70*TORAD, 150*TORAD, 150*TORAD]
00289
00290
00291
00292 def SetUpMeshData() :
00293
00294 lq = kdl.Rotation.RPY(3.14, 0, 1.57).GetQuaternion()
00295 left_palm_mesh_pose.position.x = 0.0
00296 left_palm_mesh_pose.position.y = 0.0
00297 left_palm_mesh_pose.position.z = 0.0
00298 left_palm_mesh_pose.orientation.x = lq[0]
00299 left_palm_mesh_pose.orientation.y = lq[1]
00300 left_palm_mesh_pose.orientation.z = lq[2]
00301 left_palm_mesh_pose.orientation.w = lq[3]
00302
00303 rq = kdl.Rotation.RPY(3.14, 0, 1.57).GetQuaternion()
00304 right_palm_mesh_pose.position.x = 0.0
00305 right_palm_mesh_pose.position.y = 0.0
00306 right_palm_mesh_pose.position.z = 0.0
00307 right_palm_mesh_pose.orientation.x = rq[0]
00308 right_palm_mesh_pose.orientation.y = rq[1]
00309 right_palm_mesh_pose.orientation.z = rq[2]
00310 right_palm_mesh_pose.orientation.w = rq[3]
00311
00312 wq = kdl.Rotation.RPY(-1.57, 0, -1.57).GetQuaternion()
00313 waist_mesh_pose.position.x = 0.02
00314 waist_mesh_pose.position.y = 0.05
00315 waist_mesh_pose.position.z = -0.5625
00316 waist_mesh_pose.orientation.x = wq[0]
00317 waist_mesh_pose.orientation.y = wq[1]
00318 waist_mesh_pose.orientation.z = wq[2]
00319 waist_mesh_pose.orientation.w = wq[3]
00320
00321 bq = kdl.Rotation.RPY(0, 0, 0.25).GetQuaternion()
00322 backpack_mesh_pose.position.x = 0.0
00323 backpack_mesh_pose.position.y = 0.0
00324 backpack_mesh_pose.position.z = -0.54
00325 backpack_mesh_pose.orientation.x = bq[0]
00326 backpack_mesh_pose.orientation.y = bq[1]
00327 backpack_mesh_pose.orientation.z = bq[2]
00328 backpack_mesh_pose.orientation.w = bq[3]
00329
00330 hq = kdl.Rotation.RPY(0, 1.57, 0).GetQuaternion()
00331 head_mesh_pose.position.x = 0.0
00332 head_mesh_pose.position.y = 0.07
00333 head_mesh_pose.position.z = 0.0
00334 head_mesh_pose.orientation.x = hq[0]
00335 head_mesh_pose.orientation.y = hq[1]
00336 head_mesh_pose.orientation.z = hq[2]
00337 head_mesh_pose.orientation.w = hq[3]
00338
00339
00340 def getJointCommand(jd, name, d) :
00341 jnt_cmd = JointState()
00342 j_act = jd.position[jd.name.index(name)]
00343 j_ref = j_act + d
00344 jnt_cmd.header.seq = 0
00345 jnt_cmd.header.stamp = rospy.get_rostime()
00346 jnt_cmd.header.frame_id = "r2/robot_base"
00347 jnt_cmd.name = [name]
00348 jnt_cmd.position = [j_ref]
00349 return jnt_cmd
00350
00351 def makeLeftHandSetpointMarker ( msg ):
00352 control = InteractiveMarkerControl()
00353 control.interaction_mode = InteractiveMarkerControl.MENU
00354 control.markers.append( marker_helper.makeMesh(msg, left_palm_mesh, left_palm_mesh_pose, 1.1) )
00355 msg.controls.append( control )
00356 return control
00357
00358 def makeRightHandSetpointMarker ( msg ):
00359 control = InteractiveMarkerControl()
00360 control.interaction_mode = InteractiveMarkerControl.MENU
00361 control.markers.append( marker_helper.makeMesh(msg, right_palm_mesh, right_palm_mesh_pose, 1.1) )
00362 msg.controls.append( control )
00363 return control
00364