[script] update talos_table

e1d8b91a · Pierre Fernbach · 9a628e13 · e1d8b91a
Commit e1d8b91a authored May 24, 2019 by Pierre Fernbach
--- a/script/scenarios/sandbox/dynamic/talos_table.py
+++ b/script/scenarios/sandbox/dynamic/talos_table.py
-from hpp.corbaserver.rbprm.rbprmbuilder import Builder
-from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.talos import Robot
 from hpp.gepetto import Viewer
 import time
 from hpp.corbaserver import ProblemSolver
@@ -7,39 +6,16 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
 import time


-rLegId = 'talos_rleg_rom'
-rLeg = 'leg_right_1_joint'
-rFoot = 'leg_right_6_joint'
-
-lLegId = 'talos_lleg_rom'
-lLeg = 'leg_left_1_joint'
-lFoot = 'leg_left_6_joint'
-
-rArmId = 'talos_rarm_rom'
-rArm = 'arm_right_1_joint'
-rHand = 'arm_right_7_joint'
-
-lArmId = 'talos_larm_rom'
-lArm = 'arm_left_1_joint'
-lHand = 'arm_left_7_joint'
-
-packageName = "talos_data"
-meshPackageName = "talos_data"
-rootJointType = "freeflyer"    
-urdfName = "talos"
-urdfSuffix = "_reduced"
-srdfSuffix = ""
-
-
-fullBody = FullBody ()
-fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody = Robot ()
 fullBody.setJointBounds ("root_joint",  [-5,5, -1.5, 1.5, 0.95, 1.05])
+fullBody.client.robot.setDimensionExtraConfigSpace(6)
+fullBody.client.robot.setExtraConfigSpaceBounds([0]*12)
 ps = ProblemSolver( fullBody )


 from hpp.gepetto import ViewerFactory
 vf = ViewerFactory (ps)
-vf.loadObstacleModel ("hpp-rbprm-corba", "table_140_70_73", "planning")
+#vf.loadObstacleModel ("hpp_environments", "multicontact/table_140_70_73", "planning")



@@ -50,7 +26,8 @@ q_ref = [
        0.0 ,  0.006761,                                                #Chest
        0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1,-0.005,  #Left Arm
        -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,#Right Arm
-        0.,  0.]; # head
+        0.,  0.,
+0,0,0,0,0,0]; # head

 q_init = q_ref[::]
 fullBody.setReferenceConfig(q_ref)
@@ -58,40 +35,14 @@ fullBody.setReferenceConfig(q_ref)

 tStart = time.time()
 # generate databases : 
-
-nbSamples = 1000
-rLegOffset = [0,-0.00018,-0.107]
-rLegOffset[2] += 0.006
-rLegNormal = [0,0,1]
-rLegx = 0.1; rLegy = 0.06
-fullBody.addLimb(rLegId,rLeg,rFoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "static", 0.01)
-fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+nbSamples = 10000
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "EFORT", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
 #fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
-
-lLegOffset = [0,-0.00018,-0.107]
-lLegOffset[2] += 0.006
-lLegNormal = [0,0,1]
-lLegx = 0.1; lLegy = 0.06
-fullBody.addLimb(lLegId,lLeg,lFoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "static", 0.01)
-fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
-#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
-
-
-#rArmOffset = [0.055,-0.04,-0.13]
-rArmOffset = [-0.01,0.,-0.154]
-rArmNormal = [0,0,1]
-rArmx = 0.005; rArmy = 0.005
-fullBody.addLimb(rArmId,rArm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
-fullBody.runLimbSampleAnalysis(rArmId, "ReferenceConfiguration", True)
-
-
-"""
-lArmOffset = [0.055,0.04,-0.13]
-lArmNormal = [0,0,1]
-lArmx = 0.02; lArmy = 0.02
-fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
-fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
-"""
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "EFORT", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
+fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.rArmOffset,fullBody.rArmNormal, fullBody.rArmx, fullBody.rArmx, nbSamples, "EFORT", 0.01)
+fullBody.runLimbSampleAnalysis(fullBody.rArmId, "ReferenceConfiguration", True)

 tGenerate =  time.time() - tStart
 print "generate databases in : "+str(tGenerate)+" s"
@@ -105,81 +56,131 @@ q_init[32] -=1.5 # right elbow
 v(q_init)

 # sphere = target 
-from display_tools import *
+from tools.display_tools import *
 createSphere('target',v,size=0.05,color=v.color.red)
 v.client.gui.setVisibility('target','ON')
 moveSphere('target',v,[0,0,0.5])

 # create contact : 
-fullBody.setStartState(q_init,[lLegId,rLegId])
-q_ref[0:3] = q_init[0:3]
-sref = State(fullBody,q=q_ref,limbsIncontact=[lLegId,rLegId])
-s0 = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
-createSphere('s',v)
-p0 = [-0.25,0.5,0.75]
-#p1 = [-0,0.45,0.8]
-p = [0.1,0.5,0.75]
-moveSphere('s',v,p)
-s0_bis,success = StateHelper.addNewContact(sref,rArmId,p0,[0,0,1])
-#s0_bis2,success = StateHelper.addNewContact(s0_bis,rArmId,p1,[0,0,1])
-s1,success = StateHelper.addNewContact(s0_bis,rArmId,p,[0,0,1])
-assert(success)
-v(s1.q())
-
-#project com
-v(q_init)
-com_i = fullBody.getCenterOfMass()
-com_i[2] -= 0.03
-com_i[0] += 0.06
-createSphere("com",v)
-moveSphere("com",v,com_i)
-s1.projectToCOM(com_i)
-v(s1.q())
-s1_feet = State(fullBody,q=s1.q(),limbsIncontact=[lLegId,rLegId])
-
-s2,success = StateHelper.addNewContact(s0_bis,rArmId,p,[0,0,1])
-com=s2.getCenterOfMass()
-#com[0] += 0.03
-com[1] -= 0.06
-com[2] -= 0.02
-moveSphere("com",v,com)
-s2.projectToCOM(com)
+fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
+
+s0 = State(fullBody,q=q_init,limbsIncontact=[fullBody.lLegId,fullBody.rLegId])
+
+q1 = [-0.4179830939427893,
+ 0.8347627837183168,
+ 0.9946218381742752,
+ -0.0867907317182463,
+ 0.08739711783836794,
+ 0.12321477028110761,
+ 0.9847066736170379,
+ -0.2272949051024129,
+ 0.23532649004655146,
+ -0.6159175763163939,
+ 1.0391593108867676,
+ -0.5788741314607475,
+ -0.04386597370357985,
+ -0.22948450208879723,
+ 0.22171247100334404,
+ -0.47112214269424046,
+ 0.8697762151341462,
+ -0.5537904533973627,
+ -0.030085935384065905,
+ 0.7648494558716157,
+ -0.26096759550246706,
+ 0.3757914594070104,
+ 0.4250988458823696,
+ 0.04725271385732758,
+ -0.44377218500455007,
+ -8.056875771922206e-06,
+ 0.01562999972463939,
+ 0.11397873419634344,
+ -0.0051879023889214865,
+ -0.15886010249154855,
+ -0.618674869330666,
+ -0.5257508117875245,
+ -0.19689267377445485,
+ -0.6491975697318826,
+ 0.14357903922179352,
+ 0.698131700798,
+ -0.005309093376355925,
+ -0.015058725064333728,
+ -0.0014257481207303427,
+0,
+0,
+0,
+0,
+0,
+0]
+
+#s1 = State(fullBody,q=q1,limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId])
+
+
+q2 = [-0.1739726835171786,
+ 0.725403435762449,
+ 0.9672105544468215,
+ -0.08712927323144357,
+ 0.1760957825181967,
+ 0.08020446105256104,
+ 0.9772236231041146,
+ -0.049559661418543045,
+ 0.40455974164339564,
+ -0.4003710685384243,
+ 0.7911412899690295,
+ -0.7581686282462333,
+ -0.21993634278507995,
+ -0.055572016160718905,
+ 0.3901047358882102,
+ -0.2328211479293735,
+ 0.5024200097788586,
+ -0.6346706854454504,
+ -0.20445467325620423,
+ 0.1867985328466371,
+ 0.785398163397,
+ 0.17358863127762683,
+ 0.0,
+ -0.0418602806703596,
+ -0.5374201900273251,
+ 0.0006980663134041395,
+ -0.013186731949612358,
+ 0.09817522991123807,
+ -0.005239088837162844,
+ -0.5118702629493616,
+ -0.12288822176799163,
+ 0.5082036732444083,
+ -1.978470198798287,
+ -0.28424506370808994,
+ -0.1134585429671788,
+ 0.698131700798,
+ -0.005036396255584366,
+ 0.008020752595769589,
+ 0.0008315553354996808,
+0,
+0,
+0,
+0,
+0,
+0]
+
+s2 = State(fullBody,q=q2,limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId])
 v(s2.q())

-q3=q_init[::]
-q3[20]=1.2
-s3_0 = State(fullBody,q=q3,limbsIncontact=[lLegId,rLegId])
-s3,success = StateHelper.addNewContact(s3_0,rArmId,p,[0,0,1])
-assert(success)
-com=s3.getCenterOfMass()
-#com[0] += 0.03
-com[1] -= 0.1
-com[2] -= 0.02
-moveSphere("com",v,com)
-s3.projectToCOM(com)
-v(s3.q())
+com = fullBody.getCenterOfMass()

-"""
-jointsName = [rFoot,lFoot,rHand]
-contactPos = []
-contactNormal = []
-pn = s1.getCenterOfContactForLimb(rLegId)
-contactPos += [pn[0]]
-contactNormal += [pn[1]]
-pn = s1.getCenterOfContactForLimb(lLegId)
-contactPos += [pn[0]]
-contactNormal += [pn[1]]
-pn = s1.getCenterOfContactForLimb(rArmId)
-contactPos += [pn[0]]
-contactNormal += [pn[1]]
+comF = [-0.24796187, 0.74366786,  0.81803711]
+com=comF[::]
+com[0] = -0.2165
+moveSphere("com",v,com)
+s2.projectToCOM(com)


-ps.client.problem.createStaticStabilityConstraint ('staticStability',
-     jointsName, contactPos, contactNormal,'root_joint')
-"""
+s3 = State(fullBody,q=s2.q(),limbsIncontact=[fullBody.lLegId,fullBody.rLegId,fullBody.rArmId])
+s3.projectToCOM(com,10000)
+v(s3.q())


-q2 = q_ref[::]
+#configs = [s0.q(), s1.q(),s2.q()]
+configs = [s0.q(),s2.q()]

+v(configs[-1])