accurate contact point computation for andrea + manipulation related interpolation tools

script/scenarios/darpa_hyq_interp.py

@@ -99,7 +99,7 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
 
 r(q_init)
 # computing the contact sequence
-configs = fullBody.interpolate(0.12, 10, 10, False)
+configs = fullBody.interpolate(0.12, 10, 10, True)
 #~ configs = fullBody.interpolate(0.11, 7, 10, True)
 #~ configs = fullBody.interpolate(0.1, 1, 5, True)
 
@@ -125,7 +125,7 @@ limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector'
 
 
 def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
-	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = False, use_window = use_window,
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
 	verbose = verbose, draw = draw)
 
 def play(frame_rate = 1./24.):

script/scenarios/gen_hrp2_statically_balanced_positions_2d.py

@@ -76,6 +76,7 @@ def draw_cp(cid, limb, config):
 	r.client.gui.createScene(scene)
 	for i in range(4):
 		pos = posetc[2*i]
+		print "pos", pos
 		r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
 		r.client.gui.applyConfiguration(scene+"/b"+str(i),pos+[1,0,0,0])
 		r.client.gui.refresh()	
@@ -101,7 +102,7 @@ q_0 = fullBody.getCurrentConfig();
 #~ fullBody.getSampleConfig()
 qs = []; qs_gepetto = []; states = []
 limbs = [lLegId,rLegId] 
-for _ in range(1000):
+for _ in range(10):
 	q = fullBody.generateGroundContact(limbs)
 	q_gep = q[:]
 	quat_end = q[4:7]

script/scenarios/ground_crouch_hyq_interp.py

@@ -55,31 +55,36 @@ legx = 0.02; legy = 0.02
 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")
+#~ 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)
+fullBody.addLimb(rLegId,rLeg,rfoot,offset,normal, legx, legy, nbSamples, "forward", 0.1, cType)
 
-#~ lLegId = 'lhleg'
-#~ lLeg = 'lh_haa_joint'
+lLegId = 'lhleg'
+lLeg = 'lh_haa_joint'
 lfoot = 'lh_foot_joint'
-#~ fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
+fullBody.addLimb(lLegId,lLeg,lfoot,offset,normal, legx, legy, nbSamples, "backward", 0.05, cType)
 
-#~ rarmId = 'rhleg'
-#~ rarm = 'rh_haa_joint'
+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'
+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.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]
@@ -120,16 +125,16 @@ limbsCOMConstraints = { rLegId : {'file': "hyq/"+rLegId+"_com.ineq", 'effector'
 
 
 
-def act(i, numOptim = 0, use_window = False, verbose = False, draw = False):
-	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = use_window,
-	verbose = verbose, draw = draw)
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, time_scale = 20,  verbose = False, draw = False, trackedEffectors = []):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, friction, optim_effectors = optim_effectors, time_scale = time_scale, useCOMConstraints = True, use_window = use_window,
+	verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
 
 def play(frame_rate = 1./24.):
 	play_traj(fullBody,pp,frame_rate)
 	
 def saveAll(name):
 	saveAllData(fullBody, r, name)
-#~ fullBody.exportAll(r, trajec, 'hole_hyq_t_var_04f_andrea');
+#~ saveAll ('hole_hyq_t_var_04f_andrea');
 #~ fullBody.exportAll(r, configs, 'hole_hyq_t_var_04f_andrea_contact_planning');
 #~ saveToPinocchio('obstacle_hyq_t_var_04f_andrea')
 

script/scenarios/ground_crouch_hyq_path.py

@@ -32,7 +32,8 @@ q_init = rbprmBuilder.getCurrentConfig ();
 q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 #~ q_init [0:3] = [-2,  0.47, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
 q_goal = q_init [::]
-q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
+q_goal [0:3] = [-4, 0, 0.63]; r (q_goal)
+#~ q_goal [0:3] = [5, 0, 0.63]; r (q_goal)
 #~ q_goal [0:3] = [-2, 0, 0.63]; r (q_goal)
 
 ps.addPathOptimizer("RandomShortcut")

script/scenarios/manipulation_hrp2_interp.py

@@ -6,7 +6,12 @@ from hpp.gepetto import ViewerFactory
 from hpp.gepetto import Viewer
 import sys
 
-import manipulation_hrp2_path as manipulation
+from hpp.corbaserver.manipulation.romeo import Robot
+from hpp.corbaserver.manipulation import ProblemSolver, Rule
+from hpp.gepetto.manipulation import Viewer, ViewerFactory
+from hpp.gepetto import PathPlayer, PathPlayerGui
+from math import sqrt
+
 
 from os import environ
 ins_dir = environ['DEVEL_DIR']
@@ -152,3 +157,8 @@ def playPaths(rs = None):
     
 trackedEffectors = [0, 0, 0.15, ['LARM_JOINT5']]
 
+#~ for i in range(0,1):
+	#~ trackedEffectors = [0, i * 0.15, (i+1) * 0.15, ['LARM_JOINT5']];
+	#~ act(i,60, use_window = 0, optim_effectors = True, draw = False, verbose = False, trackedEffectors = trackedEffectors)
+
+

script/scenarios/manipulation_romeo_interp.py

+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver as ProblemSolverRbprm
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver import Client
+from hpp.gepetto import ViewerFactory
+from hpp.gepetto import Viewer
+import sys
+
+from hpp.corbaserver.manipulation.romeo import Robot
+from hpp.corbaserver.manipulation import ProblemSolver, Rule
+from hpp.gepetto.manipulation import Viewer, ViewerFactory
+from hpp.gepetto import PathPlayer, PathPlayerGui
+from math import sqrt
+
+
+from hpp.gepetto import PathPlayer, PathPlayerGui
+from math import sqrt
+
+# Load robot and object. {{{3
+
+# Define classes for the objects {{{4
+class Kitchen (object):
+  rootJointType = "anchor"
+  packageName = 'iai_maps'
+  meshPackageName = 'iai_maps'
+  urdfName = 'kitchen_area'
+  urdfSuffix = ""
+  srdfSuffix = ""
+  joint = "kitchen_area/fridge_block_fridge_joint"
+  handle = "kitchen_area/fridge_handle_fridge_handle"
+
+class Cup (object):
+  rootJointType = "freeflyer"
+  packageName = 'hpp_tutorial'
+  meshPackageName = 'hpp_tutorial'
+  urdfName = 'cup'
+  urdfSuffix = ""
+  srdfSuffix = ""
+  joint = "cup/base_joint"
+  handle = "cup/handle"
+
+Robot.srdfSuffix = "_moveit"
+# 4}}}
+
+robot = Robot ('romeo-kitchen', 'romeo')
+ps0 = ProblemSolver (robot)
+#~ r = Viewer (ps)
+vf = ViewerFactory (ps0)
+
+robot.setJointBounds ("romeo/base_joint_xyz" , [-60,20,-50,100, 0, 2])
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+
+
+#~ ps = ProblemSolver( fullBody )
+#~ vf = ViewerFactory (ps)
+#~ r = Viewer (ps)
+
+
+
+robot.setJointBounds ("romeo/base_joint_xyz" , [-60,20,-50,100, 0, 2])
+vf.loadObjectModel (Kitchen, "kitchen_area")
+vf.loadObjectModel (Cup, "cup")
+
+robot.setJointBounds ('cup/base_joint_xyz', [-60,20,-50,100, 0, 2])
+# 3}}}
+
+# Define configurations. {{{3
+robot.setCurrentConfig (robot.getInitialConfig ())
+q_init = robot.getHandConfig ("both", "open")
+rank = robot.rankInConfiguration ['romeo/base_joint_xyz']
+# q_init [rank:rank+7] = [-3.5,-3.7, 0.877, 1, 0, 0, 0]
+q_init [rank:rank+7] = [-4.264,-4.69, 0.877, 0, 0, 0, 1]
+rank = robot.rankInConfiguration ['cup/base_joint_xyz']
+q_init [rank:rank+7] = [-4.8, -4.64, 0.91,0,sqrt(2)/2,sqrt(2)/2,0]
+
+q_goal1 = q_init [::]
+q_goal2 = q_init [::]
+q_goal1 [rank:rank+7] = [-4.73, -3.35, 0.91, 0,sqrt(2)/2,sqrt(2)/2,0]
+q_goal2 [rank:rank+7] = [-4.8, -4.70, 0.91, 0,sqrt(2)/2,sqrt(2)/2,0]
+# 3}}}
+
+
+# Create a new manipulation problem
+cl = Client()
+cl.problem.selectProblem("rbprm")
+cl.problem.selectProblem("default")
+cl.problem.moveRobotToProblem("rbprm") 
+cl.problem.selectProblem("rbprm")
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModelFromActiveRobot('romeo', {'cup': 'freeflyer', 'kitchen_area': 'anchor', 'romeo': 'freeflyer'}, "romeokitchen", 'romeo_description', '', '')
+#~ fullBody.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
+
+ps = ProblemSolverRbprm (robot)
+r = Viewer (ps)
+pp = PathPlayer (fullBody.client.basic, r)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.loadObstacleModel ("hpp_environments", "hrp2/floor_as_mesh", "floor", r)
+#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+fullBody.client.rbprm.rbprm.setAffordanceFilter('0rLeg', ['Support',])
+fullBody.client.rbprm.rbprm.setAffordanceFilter('1lLeg', ['Support'])
+
+import pickle
+
+with open("romeo_kitchen_path_discretized.pickle", 'r') as f:
+    qs = pickle.load(f)
+fullBody.client.rbprm.rbprm.configToPath(qs)
+#~ a = [q for i,q in enumerate(qs) if i % 50 == 0 ]
+
+r.client.gui.addURDF("kitchen_area", "/home_local/dev/hpp/install/share/"+Kitchen.packageName+"/urdf/"+Kitchen.urdfName+".urdf", "")
+r.client.gui.addToGroup("kitchen_area", r.sceneName)
+r.client.gui.addURDF("cup", "/home_local/dev/hpp/install/share/"+Cup.packageName+"/urdf/"+Cup.urdfName+".urdf", "")
+r.client.gui.addToGroup("cup", r.sceneName)
+
+
+#~ r.loadObstacleModel (Kitchen.packageName, Kitchen.urdfName, "kitchen_area2")
+
+
+	
+#~ for j in fullBody.client.basic.obstacle.getObstacleNames(True, False):
+	#~ if( j != 'floor/base_link_0'):
+		#~ fullBody.client.basic.obstacle.removeObstacleFromJoint('floor/base_link_0', j, True, False)
+
+
+
+#~ pp(0)
+
+#~ print "addlef"
+rLegId = '0rLeg'
+rfoot = 'romeo/RAnkleRoll'
+rLeg = 'romeo/RHipYaw'
+rLegOffset = [0,0,-0.06839999246139947]
+rLegNormal = [0,0,1]
+rLegx = 0.1; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, 10000, "static", 0.05, "_6_DOF", True)
+#~ 
+#~ print "addlef"
+lLegId = '1lLeg'
+lLeg = 'romeo/LHipYaw'
+lfoot = 'romeo/LAnkleRoll'
+lLegOffset = [0,0,-0.06839999246139947]
+lLegNormal = [0,0,1]
+lLegx = 0.1; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, 10000, "static", 0.1, "_6_DOF", True)
+
+#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
+
+fullBody.setStartState(qs[0],[rLegId,lLegId])
+fullBody.setEndState(qs[20],[rLegId,lLegId])
+
+#~ configs = fullBody.interpolate(0.15, 0, 10, True)
+#~ 
+
+for j in fullBody.getAllJointNames():
+	#~ if j.startswith("kitchen") or j.startswith("cup"):
+	fullBody.client.basic.obstacle.removeObstacleFromJoint('floor/base_link_0', j, True, False)
+#~ ps.pathLength(0) / 100.
+configs = fullBody.interpolate(ps.pathLength(0) / 50., 0, 10, True)
+
+limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : rfoot},  
+						lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : lLeg}, }
+
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False, trackedEffectors = []):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
+	verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+def saveAll(name):
+	saveAllData(fullBody, r, name)
+	
+def draw_com():
+	global fullBody
+	c = fullBody.getCenterOfMass()
+	scene = "com_" + str(c)
+	r.client.gui.createScene(scene)
+	r.client.gui.addBox(scene+"/b"+str(0),0.1,0.1,0.1, [1,0,0,1])
+	r.client.gui.applyConfiguration(scene+"/b"+str(0),c+[1,0,0,0])
+	r.client.gui.refresh()	
+	r.client.gui.addSceneToWindow(scene,0)
+
+#~ def playPaths(rs = None):
+    #~ import time
+    #~ ps.client.problem.selectProblem("rbprm")
+    #~ ls = [  ps.pathLength(i) for i in range(ps.numberPaths()) ]
+    #~ if rs is None:
+        #~ rs = [ vf.createViewer() ]
+        #~ ps.client.problem.selectProblem("manipulationProblem")
+        #~ rs.append( manipulation.vf.createViewer() )
+    #~ for i in range(1000):
+        #~ ps.client.problem.selectProblem("rbprm")
+        #~ rs[0] (ps.configAtParam(1,i * ls[1] / 1000.))
+        #~ ps.client.problem.selectProblem("manipulationProblem")
+        #~ rs[1] (manipulation.ps.configAtParam(0, i * ls[0] / 1000.))
+        #~ time.sleep(0.5)
+    #~ return rs
+    
+#~ for i in range(1,5):
+    #~ act(i,60, use_window = 0, optim_effectors = True, draw = False, verbose = True)
+    
+#~ trackedEffectors = [0, 0, 0.15, ['LARM_JOINT5']]
+
+#~ for i in range(0,1):
+	#~ trackedEffectors = [0, i * 0.15, (i+1) * 0.15, ['LARM_JOINT5']];
+	#~ act(i,60, use_window = 0, optim_effectors = True, draw = False, verbose = False, trackedEffectors = trackedEffectors)
+	
+#~ act(5,0, use_window = 0, friction = 1, optim_effectors = False, draw = False, verbose = True)
+
+

script/scenarios/polaris/poralis_hrp2_interp.py

@@ -158,4 +158,5 @@ def saveAll(name):
 	saveAllData(fullBody, r, name)
 
 
-for i in range(10,30):  act(i,numOptim = 0, use_window = 0, friction = 1, optim_effectors = False, verbose = False, draw = False)
+for i in range(10,25):  act(i,numOptim = 60, use_window = 0, friction = 1, optim_effectors = True, verbose = False, draw = False);
+saveAll('polarisTestAndrea')

script/scenarios/romeo_kitchen.py

+# vim: foldmethod=marker foldlevel=2
+from hpp.corbaserver.manipulation.romeo import Robot
+from hpp.corbaserver.manipulation import ProblemSolver, Rule
+from hpp.gepetto.manipulation import Viewer, ViewerFactory
+from hpp.gepetto import PathPlayer, PathPlayerGui
+from math import sqrt
+
+# Load robot and object. {{{3
+
+# Define classes for the objects {{{4
+class Kitchen (object):
+  rootJointType = "anchor"
+  packageName = 'iai_maps'
+  meshPackageName = 'iai_maps'
+  urdfName = 'kitchen_area'
+  urdfSuffix = ""
+  srdfSuffix = ""
+  joint = "kitchen_area/fridge_block_fridge_joint"
+  handle = "kitchen_area/fridge_handle_fridge_handle"
+
+class Cup (object):
+  rootJointType = "freeflyer"
+  packageName = 'hpp_tutorial'
+  meshPackageName = 'hpp_tutorial'
+  urdfName = 'cup'
+  urdfSuffix = ""
+  srdfSuffix = ""
+  joint = "cup/base_joint"
+  handle = "cup/handle"
+
+Robot.srdfSuffix = "_moveit"
+# 4}}}
+
+robot = Robot ('romeo-kitchen', 'romeo')
+ps = ProblemSolver (robot)
+vf = ViewerFactory (ps)
+
+robot.setJointBounds ("romeo/base_joint_xyz" , [-6,-2,-5.2,-2.7, 0, 2])
+vf.loadObjectModel (Kitchen, "kitchen_area")
+vf.loadObjectModel (Cup, "cup")
+
+robot.setJointBounds ('cup/base_joint_xyz', [-6,-4,-5,-3,0,1.5])
+# 3}}}
+
+# Define configurations. {{{3
+robot.setCurrentConfig (robot.getInitialConfig ())
+q_init = robot.getHandConfig ("both", "open")
+rank = robot.rankInConfiguration ['romeo/base_joint_xyz']
+# q_init [rank:rank+7] = [-3.5,-3.7, 0.877, 1, 0, 0, 0]
+q_init [rank:rank+7] = [-4.264,-4.69, 0.877, 0, 0, 0, 1]
+rank = robot.rankInConfiguration ['cup/base_joint_xyz']
+q_init [rank:rank+7] = [-4.8, -4.64, 0.91,0,sqrt(2)/2,sqrt(2)/2,0]
+
+q_goal1 = q_init [::]
+q_goal2 = q_init [::]
+q_goal1 [rank:rank+7] = [-4.73, -3.35, 0.91, 0,sqrt(2)/2,sqrt(2)/2,0]
+q_goal2 [rank:rank+7] = [-4.8, -4.70, 0.91, 0,sqrt(2)/2,sqrt(2)/2,0]
+# 3}}}

script/scenarios/run_manip.sh

+#!/bin/bash         
+
+gepetto-viewer-server & 
+hpp-manipulation-server & 
+#~ hpp-rbprm-server &
+ipython -i --no-confirm-exit ./$1
+
+pkill -f  'gepetto-viewer-server'
+pkill -f  'hpp-manipulation-server'
+#~ pkill -f  'hpp-rbprm-server'

script/scenarios/stair_bauzil_hrp2_interp.py

@@ -75,6 +75,9 @@ lLegx = 0.05; lLegy = 0.05
 #~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
  #~ 
 
+fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
 #~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
 #~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
 
@@ -127,8 +130,23 @@ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
 #~ f1.write(str(configs))
 #~ f1.close()
 
+limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},  
+						lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
+						rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
+						#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
+
+#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
 from hpp.gepetto import PathPlayer
-#~ pp = PathPlayer (ps.robot.client.basic, r, 0.001, 0.1)
-pp = PathPlayer (ps.robot.client.basic, r)
-#~ (self, client, publisher, dt=0.01, speed=1)
+pp = PathPlayer (fullBody.client.basic, r)
+
+def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False, trackedEffectors = []):
+	return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
+	verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
+
+def play(frame_rate = 1./24.):
+	play_traj(fullBody,pp,frame_rate)
+	
+def saveAll(name):
+	saveAllData(fullBody, r, name)
 

script/scenarios/test_romeo.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.corbaserver.manipulation.romeo import Robot
+
+import stair_bauzil_hrp2_path as tp
+
+
+
+packageName = "romeo_description"
+meshPackageName = "romeo_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "romeo"
+urdfSuffix = ""
+srdfSuffix = "_collision"
+
+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)
+
+rLegId = '0rLeg'
+rLeg = 'RHipYaw'
+rfoot = 'RAnkleRoll'
+rLegOffset = [0,0,-0.06839999246139947]
+rLegNormal = [0,0,1]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, 10000, "static", 0.05, "_6_DOF", True)
+#~ 
+#~ print "addlef"
+lLegId = '1lLeg'
+lLeg = 'LHipYaw'
+lfoot = 'LAnkleRoll'
+lLegOffset = [0,0,-0.06839999246139947]
+lLegNormal = [0,0,1]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, 10000, "static", 0.1, "_6_DOF", True)
+
+ #~ 
+
+
+#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
+#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]; q_init[2]+=0.2
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]+=0.2
+
+
+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;
+fullBody.draw(configs[i],r); i=i+1; i-1
+
+r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
+#~ fullBody.exportAll(r, configs, 'stair_bauzil_hrp2_robust_2');
+#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[-1])
+#~ q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.draw(q_0,r);
+#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
+#~ 
+#~ 
+#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[1])
+#~ q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.draw(q_0,r);
+#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
+#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
+
+#~ f1 = open("secondchoice","w+")
+#~ f1 = open("hrp2_stair_not_robust_configs","w+")
+#~ f1.write(str(configs))
+#~ f1.close()
+
+limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},  
+						lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
+						rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
+						#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
+
+#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
+from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj