sample.py
Go to the documentation of this file.
00001 #!/opt/grx/bin/hrpsyspy
00002 import os
00003 import sys
00004 import socket
00005 import math
00006 import time
00007 import rtm
00008 import waitInput
00009 import bodyinfo
00010 import java.lang.System
00011 import org.omg.CORBA.DoubleHolder
00012 import OpenHRP
00013 from OpenHRP.RobotHardwareServicePackage import SwitchStatus
00014 
00015 def init(robotHost=None):
00016     if robotHost != None:
00017       print 'robot host = '+robotHost
00018       java.lang.System.setProperty('NS_OPT',
00019           '-ORBInitRef NameService=corbaloc:iiop:'+robotHost+':2809/NameService')
00020       rtm.initCORBA()
00021 
00022     print "creating components"
00023     rtcList = createComps(robotHost)
00024 
00025     print "connecting components"
00026     connectComps()
00027 
00028     print "activating components"
00029     activateComps(rtcList)
00030 
00031     print "initialized successfully"
00032 
00033 def activateComps(rtcList):
00034     rtm.serializeComponents(rtcList)
00035     for r in rtcList:
00036         r.start()
00037 
00038 def initRTC(module, name):
00039     ms.load(module)
00040     return ms.create(module, name)
00041 
00042 def setJointAnglesDeg(pose, tm, ttm = org.omg.CORBA.DoubleHolder(), wait=True):
00043     if seq_svc:
00044         #ret = seq_svc.setJointAngles(bodyinfo.deg2radPose(pose), tm, ttm)
00045         ret = seq_svc.setJointAngles(bodyinfo.deg2radPose_ROS(pose), tm)
00046         mask = 31
00047         if wait:
00048             seq_svc.isEmpty()
00049             #seq_svc.isEmpty(mask, True)
00050         return ret
00051     else:
00052         print 'seq_svc is Null'
00053         return None
00054 
00055 def goInitial(tm=bodyinfo.timeToInitialPose):
00056     setJointAnglesDeg(bodyinfo.initialPose, tm)
00057 
00058 def goOffPose(tm=bodyinfo.timeToOffPose):
00059     setJointAnglesDeg(bodyinfo.offPose, tm)
00060     servoOff()
00061 
00062 def servoOn(part = 'all'):
00063     waitInputConfirm("!! Robot Motion Warning !! \n Push [OK] to Servo ON "+part)
00064     if rh_svc != None:
00065         rh_svc.servo(part, SwitchStatus.SWITCH_ON)
00066         if part == 'all':
00067             rh_svc.servo('RHAND', SwitchStatus.SWITCH_ON)
00068             rh_svc.servo('LHAND', SwitchStatus.SWITCH_ON)
00069 
00070 def servoOff(part = 'all'):
00071     waitInputConfirm("!! Robot Motion Warning !! \n Push [OK] to Servo OFF "+part)
00072     if rh_svc != None:
00073         rh_svc.servo(part, SwitchStatus.SWITCH_OFF)
00074         if part == 'all':
00075             rh_svc.servo('RHAND', SwitchStatus.SWITCH_OFF)
00076             rh_svc.servo('LHAND', SwitchStatus.SWITCH_OFF)
00077 
00078 def loadPattern(basename, tm=3.0):
00079     seq_svc.loadPattern(basename, tm)
00080 
00081 def testPattern():
00082     waitInputConfirm("!! Robot Motion Warning !! \n Push [OK] to execute "+\
00083                      bodyinfo.testPatternName)
00084     dblHolder = org.omg.CORBA.DoubleHolder()
00085     for p in bodyinfo.testPattern:
00086         print setJointAnglesDeg(p[0], p[1], dblHolder)
00087         print dblHolder.value
00088     waitInputConfirm("finished")
00089 
00090 def badPattern():
00091     waitInputConfirm("!! Robot Motion Warning !! \n Push [OK] to execute "+\
00092                      bodyinfo.badPatternName)
00093     dblHolder = org.omg.CORBA.DoubleHolder()
00094     for p in bodyinfo.badPattern:
00095         print setJointAnglesDeg(p[0], p[1], dblHolder)
00096         print dblHolder.value
00097     waitInputConfirm("finished")
00098 
00099 def createComps(hostname=socket.gethostname()):
00100     global ms, adm_svc, rh, rh_svc, seq, seq_svc, armR, armR_svc,\
00101            armL, armL_svc, grsp, grsp_svc, sa, tk_svc, log, force
00102     ms = rtm.findRTCmanager(hostname)
00103 
00104 #    adm = rtm.findRTC("SystemAdmin0")
00105     adm_svc = None
00106 #    if adm != None:
00107 #        adm.start()
00108 #        adm_svc = OpenHRP.SystemAdminServiceHelper.narrow(adm.service("service0"))
00109 
00110     rh = rtm.findRTC("RobotHardware0")
00111     rh_svc = OpenHRP.RobotHardwareServiceHelper.narrow(rh.service("service0"))
00112 
00113     seq = initRTC("SequencePlayer", "seq")
00114     seq_svc = OpenHRP.SequencePlayerServiceHelper.narrow(seq.service("service0"))
00115     #seq_svc = None
00116 
00117     armR = initRTC("ArmControl", "armR")
00118 #    armR_svc = OpenHRP.ArmControlServiceHelper.narrow(armR.service("service0"))
00119 
00120     armL = initRTC("ArmControl", "armL")
00121 #    armL_svc = OpenHRP.ArmControlServiceHelper.narrow(armL.service("service0"))
00122 
00123     grsp = initRTC("Grasper", "grsp")
00124 #    grsp_svc = OpenHRP.GrasperServiceHelper.narrow(grsp.service("service0"))
00125 
00126     sa = initRTC("StateArbitrator", "sa")
00127 #    tk_svc = OpenHRP.TimeKeeperServiceHelper.narrow(sa.service("service1"))
00128 
00129     log = initRTC("DataLogger", "log")
00130 
00131     # added force sensor RTC load - 2011/10/24
00132     force = initRTC("ForceSensor", "force")
00133     print "force = ", force
00134 
00135     return [rh, seq, armR, armL, sa, grsp, log, force]
00136 
00137 def connectComps():
00138     rtm.connectPorts(rh.port("jointDatOut"),   seq.port("jointDatIn"))
00139     rtm.connectPorts(rh.port("jointDatOut"),  armR.port("jointDatIn"))
00140     rtm.connectPorts(rh.port("jointDatOut"),  armL.port("jointDatIn"))
00141 
00142     rtm.connectPorts(  seq.port("jointDatOut"),    sa.port("jointDatIn0"))
00143     rtm.connectPorts( armR.port("jointDatOut"),    sa.port("jointDatIn1"))
00144     rtm.connectPorts( armL.port("jointDatOut"),    sa.port("jointDatIn2"))
00145 
00146     rtm.connectPorts(   sa.port("jointDatOut"), grsp.port("jointDatIn"))
00147     rtm.connectPorts( grsp.port("jointDatOut"),    rh.port("jointDatIn"))
00148 
00149 def setupLogger():
00150     global log_svc
00151     log_svc = OpenHRP.DataLoggerServiceHelper.narrow(log.service("service0"))
00152     log_svc.add("TimedJointData", "jointDatServo")
00153     log_svc.add("TimedJointData", "jointDatSeq")
00154     rtm.connectPorts( rh.port("jointDatOut"),  log.port("jointDatServo"))
00155     rtm.connectPorts( seq.port("jointDatOut"), log.port("jointDatSeq"))
00156 
00157 def saveLog():
00158     if log_svc == None:
00159       waitInputConfirm("Setup DataLogger RTC first.")
00160       return
00161     log_svc.save('/tmp/sample')
00162     print 'saved'
00163 
00164 def calibrateJoint():
00165     print('calibrateing joints ...'),
00166     if rh_svc.calibrateJoint('all') == 0:
00167       print('finised.')
00168     else:
00169       print('failed. execute servoOff() and try again.')
00170 
00171 def servoOnHands():
00172     servoOn('RHAND')
00173     servoOn('LHAND')
00174 
00175 def servoOffHands():
00176     servoOff('RHAND')
00177     servoOff('LHAND')
00178     
00179 def EngageProtectiveStop():
00180     rh_svc.engageProtectiveStop()
00181   
00182 def DisengageProtectiveStop():
00183     rh_svc.disengageProtectiveStop()
00184 
00185 def reboot():
00186     if adm_svc != None:
00187         waitInputConfirm("Reboot the robot host ?")
00188         adm_svc.reboot("")
00189 
00190 def shutdown():
00191     if adm_svc != None:
00192         waitInputConfirm("Shutdown the robot host ?")
00193         adm_svc.shutdown("")
00194 
00195 def getVersion():
00196     if adm_svc != None:
00197         currentVersion = adm_svc.getVersion("")
00198         waitInputConfirm("Robot system version is: %s"%(currentVersion))
00199 
00200 #
00201 # move arms using Inverse Kinematics
00202 #
00203 def moveRelativeR(dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate=30, wait=True):
00204   return moveRelative(armR_svc, dx, dy, dz, dr, dp, dw, rate, wait)
00205 
00206 def moveRelativeL(dx=0, dy=0, dz=0, dr=0, dp=0, dw=0, rate=30, wait=True):
00207   return moveRelative(armL_svc, dx, dy, dz, dr, dp, dw, rate, wait)
00208 
00209 def moveRelative(armsvc, dx, dy, dz, dr, dp, dw, rate, wait):
00210   x,y,z,r,p,w = getCurrentConfiguration(armsvc)
00211   return move(armsvc, x+dx, y+dy, z+dz, r+dr, p+dp, w+dw, rate, wait) 
00212 
00213 def moveR(x, y, z, r, p, w, rate=30, wait=True):
00214   return move(armR_svc, x, y, z, r, p, w, rate, wait)
00215 
00216 def moveL(x, y, z, r, p, w, rate=30, wait=True):
00217   return move(armL_svc, x, y, z, r, p, w, rate, wait)
00218 
00219 def move(armsvc, x, y, z, r, p, w, rate, wait):
00220   ret = armsvc.setTargetAngular(x, y, z, r, p, w, rate)
00221   while wait and armsvc.checkStatus() == OpenHRP.ArmState.ArmBusyState:
00222     tk_svc.sleep(0.2)
00223   return ret
00224 
00225 def getCurrentConfiguration(armsvc):
00226   x = org.omg.CORBA.DoubleHolder()
00227   y = org.omg.CORBA.DoubleHolder()
00228   z = org.omg.CORBA.DoubleHolder()
00229   r = org.omg.CORBA.DoubleHolder()
00230   p = org.omg.CORBA.DoubleHolder()
00231   w = org.omg.CORBA.DoubleHolder()
00232   armsvc.getCurrentConfiguration(x, y, z, r, p, w)
00233   return [x.value, y.value, z.value, r.value, p.value, w.value]
00234 
00235 def ikTest():
00236   import random
00237   goInitial(8)
00238   x0, y0, z0, r0, p0, w0 = getCurrentConfiguration(armR_svc)
00239   for i in range(10):
00240     x1 =  x0
00241     y1 =  y0 + 0.1*random.random()
00242     z1 =  z0 + 0.1*random.random()
00243     r1 =  r0
00244     p1 =  p0
00245     w1 =  w0
00246     ret = moveR(x1, y1, z1, r1, p1, w1, 10)
00247     x2, y2, z2, r2, p2, w2 = getCurrentConfiguration(armR_svc)
00248     print '\ntarget pos  (x[m], y[m], z[m], r[rad], p[rad], y[rad]) = %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f' %(x1, y1, z1, r1, p1, w1)
00249     print 'current pos (x[m], y[m], z[m], r[rad], p[rad], y[rad]) = %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f' %(x2, y2, z2, r2, p2, w2)
00250     if ret < 0:
00251       print 'ik failed.'
00252     else:
00253       print 'differece   (x[m], y[m], z[m], r[rad], p[rad], y[rad]) = %7.4f %7.4f %7.4f %7.4f %7.4f %7.4f' %(x2-x1, y2-y1, z2-z1, r2-r1, p2-p1, w2-w1)
00254   goInitial(4)
00255 
00256 def ikTest_interrupt():
00257   import random
00258   ttm = org.omg.CORBA.DoubleHolder()
00259   goInitial(8)
00260   x0R, y0R, z0R, r0R, p0R, w0R = getCurrentConfiguration(armR_svc)
00261   x0L, y0L, z0L, r0L, p0L, w0L = getCurrentConfiguration(armL_svc)
00262   for i in range(100):
00263     tk_svc.sleep(0.2)
00264     
00265     # right arm
00266     if random.random() > 0.8:
00267       y1R = y0R - 0.15*random.random() + 0.05
00268       z1R = z0R + 0.15*random.random() + 0.05
00269       ret = moveR(x0R, y1R, z1R, r0R, p0R, w0R, 20, False)
00270       if ret < 0:
00271         print 'ik failed (right).'
00272 
00273     # left arm
00274     if random.random() > 0.7:
00275       y1L = y0L + 0.15*random.random() - 0.05
00276       z1L = z0L + 0.15*random.random() + 0.05
00277       ret = moveL(x0L, y1L, z1L, r0L, p0L, w0L, 20, False)
00278       if ret < 0:
00279         print 'ik failed (left).'
00280 
00281     # chest and neck
00282     if random.random() > 0.8:
00283       chest = random.random()*40 - 20
00284       pan   = random.random()*100 - 50
00285       tilt  = random.random()*45 - 15
00286       seq_svc.setJointAngles(bodyinfo.deg2radPose([[chest,pan,tilt],[],[],[],[]]), 2, ttm)
00287 
00288     # right hand
00289     if random.random() > 0.9:
00290       rhandOpen20()
00291     else:
00292       rhandClose()
00293 
00294     # left hand
00295     if random.random() > 0.9:
00296       lhandClose()
00297     else:
00298       lhandOpen20()
00299 
00300   goInitial(4)
00301 
00302 def ikTest_spline():
00303   import random
00304   goInitial(8)
00305   x0, y0, z0, r0, p0, w0 = getCurrentConfiguration(armR_svc)
00306   nodeseq = []
00307   for i in range(10):
00308     x1 =  x0
00309     y1 =  y0 + 0.1*random.random()
00310     z1 =  z0 + 0.1*random.random()
00311     r1 =  r0
00312     p1 =  p0
00313     w1 =  w0
00314     sn = OpenHRP.SplineNode()
00315     sn.time = 2
00316     sn.node = [x1, y1, z1, r1, p1, w1]
00317     nodeseq.append(sn)
00318   armR_svc.setTargetsSpline(nodeseq)
00319   #goInitial(4)
00320 
00321 #
00322 # grasper
00323 #
00324 def rhandOpen():
00325   ttm = org.omg.CORBA.DoubleHolder()
00326   for i in range(12):
00327     grsp_svc.setJointAngles('RHAND', bodyinfo.anglesFromDistance(i*10), 1.0, ttm)
00328     tk_svc.sleep(0.1)
00329 
00330 def rhandOpen20():
00331   ttm = org.omg.CORBA.DoubleHolder()
00332   grsp_svc.setJointAngles('RHAND', bodyinfo.anglesFromDistance(20), 1.0, ttm)
00333 
00334 def rhandClose():
00335   ttm = org.omg.CORBA.DoubleHolder()
00336   grsp_svc.setJointAngles('RHAND', bodyinfo.anglesFromDistance(0), 3.0, ttm)
00337 
00338 def lhandOpen():
00339   ttm = org.omg.CORBA.DoubleHolder()
00340   grsp_svc.setJointAngles('LHAND', bodyinfo.anglesFromDistance(100), 1.0, ttm)
00341 
00342 def lhandOpen20():
00343   ttm = org.omg.CORBA.DoubleHolder()
00344   grsp_svc.setJointAngles('LHAND', bodyinfo.anglesFromDistance(20), 1.0, ttm)
00345 
00346 def lhandClose():
00347   ttm = org.omg.CORBA.DoubleHolder()
00348   grsp_svc.setJointAngles('LHAND', bodyinfo.anglesFromDistance(0), 1.0, ttm)
00349 
00350 def setRHandAnglesDeg(angles):
00351   ttm = org.omg.CORBA.DoubleHolder()
00352   grsp_svc.setJointAngles('RHAND', [v*math.pi/180.0 for v in angles], 1.0, ttm)
00353 
00354 def setLHandAnglesDeg(angles):
00355   ttm = org.omg.CORBA.DoubleHolder()
00356   grsp_svc.setJointAngles('LHAND', [v*math.pi/180.0 for v in angles], 1.0, ttm)
00357 
00358 
00359 #
00360 # test execution of this script
00361 # e.g: ./sample.py hiro015
00362 #
00363 if __name__ == '__main__' or __name__ == 'main':
00364   if len(sys.argv) > 1:
00365     robotHost = sys.argv[1]
00366   else:
00367     robotHost = None
00368   init(robotHost)
00369   ikTest()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hironx_ros_bridge_old
Author(s): k-okada
autogenerated on Tue Jul 23 2013 11:49:38