hironx_ros_bridge_old: bodyinfo.py Source File

Go to the documentation of this file.
00001 import os
00002 import sys
00003 import math
00004 import org.omg.CosNaming
00005 import jp.go.aist.hrp.simulator
00006 
00007 modelName ='HIRONX'
00008 url='file:///opt/grx/%s/model/main.wrl'%(modelName)
00009 
00010 # setup path
00011 sys.path.append(os.popen('rospack find openhrp3').readlines()[0][:-1]+'/share/OpenHRP-3.1/java/openhrpstubskel.jar')
00012 EXTRA_JAR_PATH=os.popen('rospack find hrpsys').readlines()[0][:-1]+'/share/hrpsys/jar/'
00013 for f in os.listdir(EXTRA_JAR_PATH):
00014   if f.endswith(".jar") and not sys.path.count(EXTRA_JAR_PATH+f):
00015     sys.path.insert(0, EXTRA_JAR_PATH+f)
00016     print 'bodyinfo.py: '+f+' is added.'
00017 
00018 timeToInitialPose = 10.0 # [sec]
00019 initialPose = [
00020    [ 0,    0,    0],   
00021    [-0.6,  0, -100,  15.2,  9.4,  3.2],
00022    [ 0.6,  0, -100, -15.2,  9.4, -3.2],
00023    [],   # [ 0,    0,    0,   0],
00024    [],   # [ 0,    0,    0,   0],
00025 ]
00026 
00027 timeToOffPose = 10.0 # [sec]
00028 offPose = [ 
00029    [0,    0,    0],   
00030    [25, -140, -159,  45,  0, 0], 
00031    [-25, -140, -159, -45,  0, 0],
00032    [],   # [ 0,    0,    0,   0],
00033    [],   # [ 0,    0,    0,   0],
00034 ]
00035 
00036 rhandOpen  = [90.0, -90.0, -90.0, 90.0]
00037 rhandClose = [ 0.0,   0.0,   0.0,  0.0]
00038 lhandOpen  = [90.0, -90.0, -90.0, 90.0]
00039 lhandClose = [ 0.0,   0.0,   0.0,  0.0]
00040 
00041 testPatternName = 'Hello Action' # just displayed on dialog
00042 testPattern = [
00043     [[ [  0,  0,-10], [ -0.6,  0,-100, 15.2,  9.4, 3.2], [ 0.6,  0,-100,-15.2,  9.4, -3.2],
00044        [ 0,  0,  0,  0], [  0,  0,  0,  0] ], 3],
00045     [[ [ 10, 20,-10], [-10.6,  0,-100, 15.2,  9.4, 3.2], [-9.4,  0,-100,-15.2,  9.4, -3.2],
00046        [45,-45,-45, 45], [ 45,-45,-45, 45] ], 3],
00047     [[ [-10,-20,-10], [  9.4,  0,-100, 15.2,  9.4, 3.2], [10.6,  0,-100,-15.2,  9.4, -3.2],
00048        [90,-90,-90, 90], [ 90,-90,-90, 90] ], 3],
00049     [[ [ 10, 20,-10], [-34,  -50,-136,-110, -25,  12],   [34,  -50,-136,110.0,-25.0,-12.0],
00050        [45,-45,-45, 45], [ 45,-45,-45, 45] ], 3],
00051     [[ [  0,  0,  0], [ -0.6,  0,-100, 15.2,  9.4, 3.2], [ 0.6,  0,-100,-15.2,  9.4, -3.2],
00052        [ 0,  0,  0,  0], [  0,  0,  0,  0] ], 3],
00053 ]
00054 
00055 testPattern2Name = 'Hello Action(No Hands)' # just displayed on dialog
00056 testPattern2 = [
00057     [[ [  0,  0,-10], [ -0.6,  0,-100, 15.2,  9.4, 3.2], [ 0.6,  0,-100,-15.2,  9.4, -3.2],
00058        [], [] ], 3],
00059     [[ [ 10, 20,-10], [-10.6,  0,-100, 15.2,  9.4, 3.2], [-9.4,  0,-100,-15.2,  9.4, -3.2],
00060        [], [] ], 3],
00061     [[ [-10,-20,-10], [  9.4,  0,-100, 15.2,  9.4, 3.2], [10.6,  0,-100,-15.2,  9.4, -3.2],
00062        [], [] ], 3],
00063     [[ [ 10, 20,-10], [-34,  -50,-136,-110, -25,  12],   [34,  -50,-136,110.0,-25.0,-12.0],
00064        [], [] ], 3],
00065     [[ [  0,  0,  0], [ -0.6,  0,-100, 15.2,  9.4, 3.2], [ 0.6,  0,-100,-15.2,  9.4, -3.2],
00066        [], [] ], 3],
00067 ]
00068 
00069 badPatternName = 'Bad Pattern'
00070 badPattern = [
00071   [[[5.8623891671491013e-07, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-24.063999472883673, -24.063857952308275, -132.35299971970591, -39.534592353365412, 68.753904664625352, -12.603922769794654], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 4.0],
00072   [[[0.0002690654886253667, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-24.063392882465969, -24.064318667671341, -132.35213054273066, -39.531304606945397, 68.753016007085094, -12.602852828408027], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.06138940928120018],
00073   [[[0.0029440489948407132, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-24.054228078758172, -24.069391521397868, -132.33836351282929, -39.478882864803928, 68.74267583775638, -12.589768076649065], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.18885306435839944],
00074   [[[0.019501215464708538, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-23.958275581652746, -24.111605733940181, -132.19170522488301, -38.930526158522639, 68.652528950101669, -12.471287795771568], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.5719746870164002],
00075   [[[0.042793864833615164, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-23.416901340133379, -24.329440735310587, -131.36062822417239, -35.848164023210494, 68.178162867567991, -11.838511889026156], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 1.2078005663919997],
00076   [[[0.042789652447905363, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-23.374620607191051, -24.346156549800412, -131.29566741527603, -35.607613422502766, 68.141616181647763, -11.789615441606575], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.065160552800000104],
00077   [[[-1.3938602113155345, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [-2.2492334873287017, -32.400543871812516, -98.747073541032904, 0.62282403091444416, 50.398302726828867, 12.084561108397974], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 9.6614643546319989],
00078   [[[-1.8439420506603965, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [3.324597346529119, -34.51092046453347, -90.643379075453666, 5.9309181853061794, 45.74241960866383, 18.356082076428802], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 1.415493877760003],
00079   [[[-1.981863878146493, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [5.0100798975369543, -35.129814577931107, -88.359275949671087, 7.4270553737571312, 44.335173384380653, 20.251833262756108], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.39897046384000134],
00080   [[[-1.9956312174451061, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [5.1778312829362703, -35.189402360512055, -88.135610988143668, 7.5735604527889651, 44.195528901987721, 20.440496690945224], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.039068062799998415],
00081   [[[-2.0921089793387955, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [6.3395167343679795, -35.596501880094451, -86.588952100191463, 8.5866554879976835, 43.24083029821653, 21.759900005459478], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.27015905767999726],
00082   [[[-3.077713843311813, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [16.961579579424924, -38.917218010519363, -72.446818189471799, 17.850057210925161, 35.399189539396275, 32.862658970770717], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 2.4702442068400003],
00083   [[[-3.9762657917238342, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [24.662221727399228, -41.005297950642792, -61.412309956716541, 25.077898024104279, 30.392327945794577, 40.026204752011438], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 1.9274269568400015],
00084   [[[-7.5082165643107981, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [41.135452762258097, -44.686163891931031, -35.808718571917368, 47.621775607683475, 21.035181160258968, 53.926340770632883], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 6.0117096102399969],
00085   [[[-8.718918211341272, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [43.917394905525271, -44.956259634302469, -31.457797969788672, 54.596318655128798, 20.060342173256462, 55.636810011086368], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 1.8598809100000011],
00086   [[[-9.2263502611897525, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [44.830362472062781, -44.946333140501828, -30.024712494860687, 57.823189455331431, 19.908768480385781, 56.02268791878226], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.86050016995999812],
00087   [[[-10.12671619398116, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [45.973522135329851, -44.654090860475193, -28.226998397984829, 61.697547254739909, 20.1901039358243, 56.022977835426595], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 1.8519489600399979],
00088   [[[-10.184312776336684, 0.0, 0.0], [-0.59948574104538033, 0.0, -99.999985561788449, 15.199252501891939, 9.399601812239208, 3.1996828069080823], [46.017095575649549, -44.61837209856894, -28.158185593831824, 61.757722147173524, 20.242211582502478, 55.980572083093371], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772], [0.9000021045914971, -0.60000140306099814, -0.60000140306099814, 0.39998183678082772]], 0.27724490652000355]
00089   ]
00090 
00091 def anglesFromDistance(gripDist):
00092     
00093     safetyMargin = 3
00094 
00095     l1 = 33
00096     l2 = 41.9
00097     l3 = 30
00098     l4 = l2 - safetyMargin*2
00099     l5 = 19
00100 
00101     #if gripDist < 0.0 or gripDist > (l1+l4)*2:
00102     if gripDist < 0.0 or gripDist > (l2+l5 - safetyMargin)*2:
00103         return None
00104     
00105     xPos   = gripDist/2.0 + safetyMargin
00106     #print 'xPos =', xPos
00107     a2Pos  = xPos - l5
00108     #print 'a2Pos =', a2Pos
00109     a1radH = math.acos(a2Pos/l2)
00110     #print 'a1radH = ', a1radH
00111     a1rad  = math.pi/2.0 - a1radH
00112     a2rad  = -a1rad
00113     #dEnd   = l2 * math.cos(a1rad) + l3
00114 
00115     return a1rad, a2rad, -a1rad, -a2rad #, dEnd
00116 
00117 
00118 # this is for compatibility between different robots
00119 def deg2radPose(pose):
00120     ret = []
00121     for p in pose:
00122       ret.append([jv*math.pi/180.0 for jv in p])
00123     return ret
00124 
00125 # deg2radPose for ROS-SequencePlayer
00126 def deg2radPose_ROS(pose):
00127     ret = []
00128     for p in pose:
00129       ret += [jv*math.pi/180.0 for jv in p]
00130     return ret
00131 
00132 # get bodyInfo from modelloader
00133 def init(nameContext):
00134   global url, linkInfo
00135   obj = nameContext.resolve([org.omg.CosNaming.NameComponent('ModelLoader', '')])
00136   mdlldr = jp.go.aist.hrp.simulator.ModelLoaderHelper.narrow(obj)
00137   bodyInfo = mdlldr.getBodyInfo(url)
00138   linkInfo = bodyInfo.links()
00139 
00140 def jointId(jointName):
00141   for l in linkInfo:
00142     if jointName == l.name:
00143       return l.jointId
00144 
00145 def jointName(jointId):
00146   for l in linkInfo:
00147     if jointId == jid:
00148       return l.name