r2_helper.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 #roslib.load_manifest("r2_controllers")
00023 roslib.load_manifest("r2_gazebo")
00024 #roslib.load_manifest("r2_sensing")
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 nasa_r2_common_msgs.srv import *
00037 #from r2_gazebo.srv import *
00038 #from r2_sensing.srv import *
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 # pose topics
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 # joint topics
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 # meshes
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 # mesh poses
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 # service calls
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 # poses
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 # joint states
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 #leftHandPowerClose.header.stamp = 0
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 #rightHandPowerClose.header.stamp = 0
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 #leftHandPowerCloseThumb.header.stamp = 0
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 #rightHandPowerCloseThumb.header.stamp = 0
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 


r2_teleop
Author(s): Paul Dinh
autogenerated on Mon Oct 6 2014 02:43:06