updated profile scenarios

profile/scenarios/darpa_hyq_interp.py

+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+#calling script darpa_hyq_path to compute root path
+import darpa_hyq_path as tp
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+
+ps = tp.ProblemSolver(fullBody)
+r = tp.Viewer (ps)
+
+rootName = 'base_joint_xyz'
+
+#  Creating limbs
+# cType is "_3_DOF": positional constraint, but no rotation (contacts are punctual)
+cType = "_3_DOF"
+# string identifying the limb
+rLegId = 'rfleg'
+# First joint of the limb, as in urdf file
+rLeg = 'rf_haa_joint'
+# Last joint of the limb, as in urdf file
+rfoot = 'rf_foot_joint'
+# Specifying the distance between last joint and contact surface
+offset = [0.,-0.021,0.]
+# Specifying the contact surface direction when the limb is in rest pose
+normal = [0,1,0]
+# Specifying the rectangular contact surface length
+legx = 0.02; legy = 0.02
+# remaining parameters are the chosen heuristic (here, manipulability), and the resolution of the octree (here, 10 cm).
+
+def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
+
+lLegId = 'lhleg'
+rarmId = 'rhleg'
+larmId = 'lfleg'
+
+#~ addLimbDb(rLegId, "static")
+#~ addLimbDb(lLegId, "static")
+#~ addLimbDb(rarmId, "static")
+#~ addLimbDb(larmId, "static")
+
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+#~ 
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
+
+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]
+
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+
+#~ from pickle import load
+#~ f = open("config_"+str(tp.config_i), 'r')
+#~ q_init =  load(f)
+#~ f.close()
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartState(q_init,[])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+
+
+r(q_init)
+# computing the contact sequence
+#~ configs = fullBody.interpolate(0.05, 10, 1, True)
+configs = fullBody.interpolate(0.08, 10, 10, True)
+#~ configs = fullBody.interpolate(0.11, 7, 10, True)
+#~ configs = fullBody.interpolate(0.1, 1, 10, True)
+#~ configs = fullBody.interpolate(0.02, 10, 10, True)
+
+import time
+try:
+	time.sleep(2)
+	fullBody.dumpProfile()
+except Exception as e:
+	pass
+
+	
+	

profile/scenarios/darpa_hyq_path.py

@@ -35,9 +35,21 @@ r = Viewer (ps)
 
 # Setting initial and goal configurations
 q_init = rbprmBuilder.getCurrentConfig ();
-q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-1.8, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
 q_goal = q_init [::]
 q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
+
+config_i = 0
+
+#~ from pickle import load
+#~ f = open("config_"+str(config_i), 'r')
+#~ q_init [0:7] =  load(f)[0:7] 
+#~ f.close()
+
+
+
 #~ q_goal [0:3] = [-1.5, 0, 0.63]; r (q_goal)
 
 # Choosing a path optimizer
@@ -58,14 +70,12 @@ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
 
 # Solve the problem
 t = ps.solve ()
-print t;
 if isinstance(t, list):
 	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
 f = open('log.txt', 'a')
 f.write("path computation " + str(t) + "\n")
 f.close()
 
-
 # Playing the computed path
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (rbprmBuilder.client.basic, r)
@@ -74,3 +84,6 @@ pp = PathPlayer (rbprmBuilder.client.basic, r)
 q_far = q_init [::]
 q_far [0:3] = [-2, -3, 0.63]; 
 r(q_far)
+
+for i in range(1,10):
+	rbprmBuilder.client.basic.problem.optimizePath(i)

profile/scenarios/ground_crouch_hyq_interp.py

@@ -58,15 +58,46 @@ lLegId = 'lhleg'
 rarmId = 'rhleg'
 larmId = 'lfleg'
 
-addLimbDb(rLegId, "static")
-addLimbDb(lLegId, "static")
-addLimbDb(rarmId, "static")
-addLimbDb(larmId, "static")
+#~ addLimbDb(rLegId, "static")
+#~ addLimbDb(lLegId, "static")
+#~ addLimbDb(rarmId, "static")
+#~ addLimbDb(larmId, "static")
 
 lfoot = 'lh_foot_joint'
 rHand = 'rh_foot_joint'
 lHand = 'lf_foot_joint'
 
+#~ lLegId = 'lhleg'
+#~ rarmId = 'rhleg'
+#~ larmId = 'lfleg'
+#~ 
+#~ addLimbDb(rLegId, "static")
+#~ addLimbDb(lLegId, "static")
+#~ addLimbDb(rarmId, "static")
+#~ addLimbDb(larmId, "static")
+
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
+
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
+lfoot = 'lh_foot_joint'
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+
+rarmId = 'rhleg'
+rarm = 'rh_haa_joint'
+rHand = 'rh_foot_joint'
+fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+#~ 
+larmId = 'lfleg'
+larm = 'lf_haa_joint'
+lHand = 'lf_foot_joint'
+fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType)
+
+fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
 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]
@@ -84,49 +115,10 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 r(q_init)
 
 configs = fullBody.interpolate(0.08,1,3) #hole 
-#~ configs = fullBody.interpolate(0.08,1,5) # bridge
-
-#~ r.loadObstacleModel ('hpp-rbprm-corba', "groundcrouch", "contact")
-#~ fullBody.exportAll(r, configs, 'obstacle_hyq_robust_10');
-i = 0;
-r (configs[i]); i=i+1; i-1
 
-
-from hpp.gepetto import PathPlayer
-pp = PathPlayer (fullBody.client.basic, r)
-
-
-from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj, profile
-
-	
-limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector' : rfoot},  
-						lLegId : {'file': "hyq/"+lLegId+"_com.ineq", 'effector' : lfoot},  
-						rarmId : {'file': "hyq/"+rarmId+"_com.ineq", 'effector' : rHand},  
-						larmId : {'file': "hyq/"+larmId+"_com.ineq", 'effector' : lHand} }
-
-
-
-def act(i, numOptim = 0):
-	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False)
-
-def play(frame_rate = 1./24.):
-	play_traj(fullBody,pp,frame_rate)
-	
-def saveAll(name):
-	saveAllData(fullBody, r, name)
-#~ fullBody.exportAll(r, trajec, 'obstacle_hyq_t_var_04f_andrea');
-#~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
-
-#~ profile(fullBody, configs, 4, 10, limbsCOMConstraints)	
-profile(fullBody, configs, 4, len(configs)-4, limbsCOMConstraints)	
 import time
 try:
 	time.sleep(2)
 	fullBody.dumpProfile()
 except Exception as e:
 	pass
-	
-    
-
-
-

profile/scenarios/polaris_hrp2_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import polaris_hrp2_path_no_step as tp
+from numpy import array, matrix
+import numpy as np
+
+
+
+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", [-1,2, -2, 1, 0.5, 2.5])
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+#~ AFTER loading obstacles
+#~ AFTER loading obstacles
+rLegId = 'hrp2_rleg_rom'
+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, 10000, "manipulability", 0.03,  "_6_DOF")
+
+lLegId = 'hrp2_lleg_rom'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.115,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03,  "_6_DOF")
+
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [-0.040,-0.01,-0.085]
+rArmNormal = [1,0,0]
+rArmx = 0.024; rArmy = 0.024
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
+
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.040,0.01,-0.085]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
+
+ #~ 
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
+
+#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+confsize = len(tp.q_init)
+q_init =  [
+        #~ 0.12, -0.45, 0.95, 1.0, 0.0 , 0.0, 0.0,                         	 # Free flyer 0-6
+        0.36, 0, 1.01, 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.17, 		 # LARM       11-17
+        0.261799388, -0.174532925, 0.0, -1, 0.0, 0.0, 0.17, 		 # 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);
+q_init[0:confsize] = tp.q_init[0:confsize]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
+
+
+fullBody.setCurrentConfig (q_init)
+#~ q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
+
+fullBody.setCurrentConfig (q_goal)
+#~ r(q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+#~ r(q_goal)
+
+#~ gui = r.client.gui
+
+
+
+
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+fullBody.setStartState(q_init,[rLegId,lLegId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+#~ 
+#~ r(q_init)
+configs = fullBody.interpolate(0.03, 10, 5, True)
+
+import time
+try:
+	time.sleep(2)
+	fullBody.dumpProfile()
+except Exception as e:
+	pass

profile/scenarios/polaris_hrp2_path_no_step.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_flexible'
+urdfNameRoms =  ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1.4, 1.01, 0, 1.01])
+rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
+#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
+#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
+rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support','Lean'])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
+rbprmBuilder.boundSO3([-0.7,0.7,0,0,-0.0,0.0])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+q0 = rbprmBuilder.getCurrentConfig ();
+q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+q_goal = q_init [::]
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+#~ q_init[0:3] = [0.36, -0.32, 1.01];  r(q_init)
+q_init[0:3] = [0.36, 0, 1.01];  r(q_init)
+rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal)
+#~ q_goal[0:3] = [0.9, -1.1, 0.6]; r(q_goal)
+#~ q_goal[0:3] = [1.5, -1.1, 0.5]; r(q_goal)
+q_goal[0:3] = [0.36, -1, 0.9]; r(q_goal)
+ 
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005])
+afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
+afftool.loadObstacleModel (packageName, "polaris", "planning", r)
+#~ afftool.analyseAll()
+#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+#~ afftool.visualiseAffordances('Lean', r, [1, 0, 0])
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
+t = ps.solve ()
+print t;
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
+
+#~ print ("solving time " + str(t));
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp (0)
+
+#~ pp (1)
+for i in range(1,10):
+	rbprmBuilder.client.basic.problem.optimizePath(i)
+
+	
+
+rob = rbprmBuilder.client.basic.robot
+r(q_init)
+
+q_loin = q_init[::]
+
+r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
+#~ q_loin[0:3] = [100,100,100]
+#~ r (q_loin)
+#~ configs = []
+#~ problem = ps.client.problem
+#~ length = problem.pathLength (0)
+#~ t = 0
+#~ i = 0
+#~ configs = []
+#~ dt = 0.1 / length
+#~ while t < length :
+	#~ q = rbprmBuilder.getCurrentConfig()
+	#~ q[0:9]= problem.configAtParam (0, t)[0:9]
+	#~ configs.append(q)
+	#~ t += dt
+	#~ i = i+1
+	#~ 
+#~ i = 0;
+#~ r(configs[i]); i = +1

profile/scenarios/stair_bauzil_hrp2_interp.py

@@ -131,6 +131,12 @@ def play(frame_rate = 1./24.):
 def saveAll(name):
 	saveAllData(fullBody, r, name)
 
+import time
+try:
+	time.sleep(2)
+	fullBody.dumpProfile()
+except Exception as e:
+	pass
 #~ for i in range(4,25):
     #~ act(i,0)
 

profile/scenarios/standing_hrp2_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import standing_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", [-2,1, -1, 1, 0, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+rLegId = 'hrp2_rleg_rom'
+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, 10000, "manipulability", 0.03,  "_6_DOF")
+
+lLegId = 'hrp2_lleg_rom'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.115,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03,  "_6_DOF")
+
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [-0.040,-0.01,-0.085]
+rArmNormal = [1,0,0]
+rArmx = 0.024; rArmy = 0.024
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
+
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.040,0.01,-0.085]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "EFORT", 0.05, "_6_DOF", False)
+ #~ 
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, larmId, q_0,)
+
+fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+confsize = len(tp.q_init)
+q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
+
+
+fullBody.setCurrentConfig (q_init)
+#~ q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
+
+fullBody.setCurrentConfig (q_goal)
+#~ r(q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+#~ r(q_goal)
+
+#~ gui = r.client.gui
+
+
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+#~ 
+#~ r(q_init)
+configs = fullBody.interpolate(0.1)
+
+import time
+try:
+	time.sleep(2)
+	fullBody.dumpProfile()
+except Exception as e:
+	pass
+

profile/scenarios/standing_hrp2_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder