computing roms for hyq and phantom

script/scenarios/stair_bauzil_hyq_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_bauzil_hrp2_path as tp
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,-0.105,0,]
+rLegNormal = [0,1,0]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "EFORT", 0.01)
+
+lLegId = '1lLeg'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "EFORT", 0.01)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0,0,-0.1]
+rArmNormal = [0,0,1]
+rArmx = 0.024; rArmy = 0.024
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 30000, "EFORT", 0.05)
+
+
+#~ AFTER loading obstacles
+larmId = '4Larm'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.05,-0.050,-0.050]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
+
+rKneeId = '0RKnee'
+rLeg = 'RLEG_JOINT0'
+rKnee = 'RLEG_JOINT3'
+rLegOffset = [0.105,0.055,0.017]
+rLegNormal = [-1,0,0]
+rLegx = 0.05; rLegy = 0.05
+#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
+#~ 
+lKneeId = '1LKnee'
+lLeg = 'LLEG_JOINT0'
+lKnee = 'LLEG_JOINT3'
+lLegOffset = [0.105,0.055,0.017]
+lLegNormal = [-1,0,0]
+lLegx = 0.05; lLegy = 0.05
+#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
+ #~ 
+
+#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+q_0 = fullBody.getCurrentConfig(); 
+q_init =  [
+        0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,                         # Free flyer 0-6
+        0.0, 0.0, 0.0, 0.0,                                                  # CHEST HEAD 7-10
+        0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # LARM       11-17
+        0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.174532925, # RARM       18-24
+        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # LLEG       25-30
+        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # RLEG       31-36
+        ]; r (q_init)
+
+fullBody.setCurrentConfig (q_goal)
+#~ r(q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+#~ r(q_goal)
+
+fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
+#~ 
+#~ configs = fullBody.interpolate(0.1)
+configs = fullBody.interpolate(0.1)
+#~ configs = fullBody.interpolate(0.15)
+i = 0;
+r (configs[i]); i=i+1; i-1
+#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
+
+#~ f1 = open("secondchoice","w+")
+f1 = open("new","w+")
+f1.write(str(configs))
+f1.close()
+

script/scenarios/stair_bauzil_hyq_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk'
+urdfNameRom = 'hrp2_rom'
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [0, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+
+q_goal = q_init [::]
+q_goal [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal)
+#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "stair_bauzil", "planning")
+ps.solve ()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ 
+#~ pp (2)
+#~ pp (0)
+
+pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+r (q_init)

script/tools/generateROMsHyq.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+
+import quaternion as quat
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 50000
+
+ps = ProblemSolver( fullBody )
+
+rootName = 'base_joint_xyz'
+
+#~ AFTER loading obstacles
+rLegId = 'rfleg'
+rLeg = 'rf_haa_joint'
+rfoot = 'rf_foot_joint'
+rLegOffset = [0,0,0]
+rLegNormal = [0,1,0]
+rLegx = 0.02; rLegy = 0.02
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "EFORT", 0.01)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+lLegOffset = [0,0,0]
+lLegNormal = [0,1,0]
+lLegx = 0.02; lLegy = 0.02
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "EFORT", 0.01)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+rArmOffset = [0,0,0]
+rArmNormal = [1,0,0]
+rArmx = 0.02; rArmy = 0.02
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+lArmOffset = [0,0,0]
+lArmNormal = [1,0,0]
+lArmx = 0.02; lArmy = 0.02
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
+
+
+#make sure this is 0
+q_0 = fullBody.getCurrentConfig ()
+
+def printEffPosition(limbId, nbSamples):
+	limit = nbSamples-1;
+	f1=open('./data/roms/hyq/'+limbId+'.erom', 'w+')
+	for i in range(0,limit):
+		q = fullBody.getSamplePosition(limbId,i)
+		f1.write(str(q[0]) + "," + str(q[1]) + "," + str(q[2]) + "\n")
+	f1.close()
+
+printEffPosition(rarmId, nbSamples)
+printEffPosition(rLegId, nbSamples)
+printEffPosition(larmId, nbSamples)
+printEffPosition(lLegId, nbSamples)

script/tools/generateROMsPhantom.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+
+import quaternion as quat
+
+packageName = "phantomx_description"
+meshPackageName = "phantomx_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "phantomx"
+urdfSuffix = ""
+srdfSuffix = ""
+
+fullBody = FullBody ()
+ 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+nbSamples = 50000
+
+ps = ProblemSolver( fullBody )
+
+rootName = 'base_joint_xyz'
+
+#~ AFTER loading obstacles
+rmId = 'rm'
+rm = 'j_c1_rm'
+rme = 'j_tibia_rm'
+rmOffset = [0,0,0]
+rmNormal = [0,1,0]
+rmx = 0.02; rmy = 0.02
+fullBody.addLimb(rmId,rm,rme,rmOffset,rmNormal, rmx, rmy, nbSamples, "EFORT", 0.01)
+
+rrId = 'rr'
+rr = 'j_c1_rr'
+rre = 'j_tibia_rr'
+rrOffset = [0,0,0]
+rrNormal = [0,1,0]
+rrx = 0.02; rry = 0.02
+fullBody.addLimb(rrId,rr,rre,rrOffset,rrNormal, rrx, rry, nbSamples, "EFORT", 0.01)
+
+rfId = 'rf'
+rf = 'j_c1_rf'
+rfe = 'j_tibia_rf'
+rfOffset = [0,0,0]
+rfNormal = [0,1,0]
+rfx = 0.02; rfy = 0.02
+fullBody.addLimb(rfId,rf,rfe,rfOffset,rfNormal, rfx, rfy, nbSamples, "EFORT", 0.01)
+
+
+lmId = 'lm'
+lm = 'j_c1_lm'
+lme = 'j_tibia_lm'
+lmOffset = [0,0,0]
+lmNormal = [0,1,0]
+lmx = 0.02; lmy = 0.02
+fullBody.addLimb(lmId,lm,lme,lmOffset,lmNormal, lmx, lmy, nbSamples, "EFORT", 0.01)
+
+lrId = 'lr'
+lr = 'j_c1_lr'
+lre = 'j_tibia_lr'
+lrOffset = [0,0,0]
+lrNormal = [0,1,0]
+lrx = 0.02; lry = 0.02
+fullBody.addLimb(lrId,lr,lre,lrOffset,lrNormal, lrx, lry, nbSamples, "EFORT", 0.01)
+
+lfId = 'lf'
+lf = 'j_c1_lf'
+lfe = 'j_tibia_lf'
+lfOffset = [0,0,0]
+lfNormal = [0,1,0]
+lfx = 0.02; lfy = 0.02
+fullBody.addLimb(lfId,lf,lfe,lfOffset,lfNormal, lfx, lfy, nbSamples, "EFORT", 0.01)
+
+#make sure this is 0
+q_0 = fullBody.getCurrentConfig ()
+
+def printEffPosition(limbId, nbSamples):
+	limit = nbSamples-1;
+	f1=open('./data/roms/phantom/'+limbId+'.erom', 'w+')
+	for i in range(0,limit):
+		q = fullBody.getSamplePosition(limbId,i)
+		f1.write(str(q[0]) + "," + str(q[1]) + "," + str(q[2]) + "\n")
+	f1.close()
+
+printEffPosition(rmId, nbSamples)
+printEffPosition(rrId, nbSamples)
+printEffPosition(rfId, nbSamples)
+printEffPosition(lmId, nbSamples)
+printEffPosition(lrId, nbSamples)
+printEffPosition(lfId, nbSamples)