From dbfd89cfc714b39942e98f007694724fe291d107 Mon Sep 17 00:00:00 2001
From: t steve <pro@stevetonneau.fr>
Date: Tue, 23 May 2017 13:29:45 +0200
Subject: [PATCH] cleaning

---
 .gitignore                                    |    1 +
 .../siggraph_asia/down_hrp2_interp_comp.py    |   55 +
 .../siggraph_asia/grasp_hrp2_interp_comp.py   |   54 +
 .../demos/siggraph_asia/hrp2_model_climb.py   |   72 +
 .../demos/siggraph_asia/hrp2_model_crazy.py   |   72 +
 script/scenarios/demos/siggraph_asia/log.txt  | 1418 -----------------
 .../demos/siggraph_asia/scale_hrp2_interp.py  |    9 +-
 .../demos/siggraph_asia/spiderman/log.txt     |   65 -
 .../siggraph_asia/stair_bauzil_hrp2_interp.py |    2 +-
 src/rbprmbuilder.impl.cc                      |    2 -
 10 files changed, 258 insertions(+), 1492 deletions(-)
 create mode 100644 script/scenarios/demos/siggraph_asia/down_hrp2_interp_comp.py
 create mode 100644 script/scenarios/demos/siggraph_asia/grasp_hrp2_interp_comp.py
 create mode 100644 script/scenarios/demos/siggraph_asia/hrp2_model_climb.py
 create mode 100644 script/scenarios/demos/siggraph_asia/hrp2_model_crazy.py
 delete mode 100644 script/scenarios/demos/siggraph_asia/log.txt
 delete mode 100644 script/scenarios/demos/siggraph_asia/spiderman/log.txt

diff --git a/.gitignore b/.gitignore
index 634acfb..15c4801 100755
--- a/.gitignore
+++ b/.gitignore
@@ -6,6 +6,7 @@
 
 # Precompiled Headers
 *.gch
+*.txt
 *.pch
 
 # Compiled Dynamic libraries
diff --git a/script/scenarios/demos/siggraph_asia/down_hrp2_interp_comp.py b/script/scenarios/demos/siggraph_asia/down_hrp2_interp_comp.py
new file mode 100644
index 0000000..36d6596
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/down_hrp2_interp_comp.py
@@ -0,0 +1,55 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.gepetto import PathPlayer
+
+import down_hrp2_path as path_planner
+import hrp2_model_grasp as model
+from hrp2_model import *
+import time
+
+
+ps = path_planner.ProblemSolver( model.fullBody )
+r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
+fullBody = model.fullBody
+fullBody.setJointBounds ("base_joint_xyz", [-3,3, -2, 2, 0, 1])
+pp = PathPlayer (fullBody.client.basic, r)
+
+from plan_execute import a, b, c, d, e, init_plan_execute
+init_plan_execute(model.fullBody, r, path_planner, pp)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+q_init =  [
+        0.2,0, 0.48, 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, -0.523598776, 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)
+
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId, rarmId]) #,rarmId,larmId])
+fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
+
+configs = d(0.005); e()
+
+from bezier_traj import *
+init_bezier_traj(model.fullBody, r, pp, configs, model.limbsCOMConstraints)
+#~ AFTER loading obstacles
+
+
+#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
+#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
+#~ gen(0,1)
diff --git a/script/scenarios/demos/siggraph_asia/grasp_hrp2_interp_comp.py b/script/scenarios/demos/siggraph_asia/grasp_hrp2_interp_comp.py
new file mode 100644
index 0000000..f5f1a29
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/grasp_hrp2_interp_comp.py
@@ -0,0 +1,54 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+from hpp.gepetto import PathPlayer
+
+import grasp_hrp2_path as path_planner
+import hrp2_model_grasp as model
+from hrp2_model import *
+import time
+
+
+ps = path_planner.ProblemSolver( model.fullBody )
+r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
+fullBody = model.fullBody
+fullBody.setJointBounds ("base_joint_xyz", [-3,3, -2, 2, 0, 1])
+pp = PathPlayer (fullBody.client.basic, r)
+
+from plan_execute import a, b, c, d, e, init_plan_execute
+init_plan_execute(model.fullBody, r, path_planner, pp)
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+q_init =  [
+        -0.05, -1.12, 0.5, 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, -0.523598776, 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)
+
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+q_init = fullBody.generateContacts(q_init, [0,0,1])
+
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId, rarmId]) #,rarmId,larmId])
+fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
+
+configs = d(0.01); e(0.01)
+
+from bezier_traj import *
+init_bezier_traj(model.fullBody, r, pp, configs, model.limbsCOMConstraints)
+#~ AFTER loading obstacles
+
+
+#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
+#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
+#~ gen(0,1)
diff --git a/script/scenarios/demos/siggraph_asia/hrp2_model_climb.py b/script/scenarios/demos/siggraph_asia/hrp2_model_climb.py
new file mode 100644
index 0000000..c96f981
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/hrp2_model_climb.py
@@ -0,0 +1,72 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import time
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_mad_legs"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+
+
+#~ AFTER loading obstacles
+
+rLegId = 'hrp2_rleg_rom'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]       
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.01)
+                                                                                                
+lLegId = 'hrp2_lleg_rom'                                                                                
+lLeg = 'LLEG_JOINT0'                                                                     
+lLegx = 0.09; lLegy = 0.05      
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]                                                                  
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20, "manipulability", 0.01)
+
+#~ AFTER loading obstacles
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [0,0,-0.1075]
+lArmNormal = [0,0,1]
+lArmx = 0.024; lArmy = 0.024
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
+
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0,0,-0.1075]
+rArmNormal = [0,0,1]
+rArmx = 0.024; rArmy = 0.024
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
+#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
+#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF")
+
+#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
+
+
+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} }
+
diff --git a/script/scenarios/demos/siggraph_asia/hrp2_model_crazy.py b/script/scenarios/demos/siggraph_asia/hrp2_model_crazy.py
new file mode 100644
index 0000000..7af5f7d
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/hrp2_model_crazy.py
@@ -0,0 +1,72 @@
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import time
+
+
+
+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)
+
+
+#~ AFTER loading obstacles
+
+rLegId = 'hrp2_rleg_rom'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,0,-0.105]
+rLegNormal = [0,0,1]       
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
+                                                                                                
+lLegId = 'hrp2_lleg_rom'                                                                                
+lLeg = 'LLEG_JOINT0'                                                                     
+lLegx = 0.09; lLegy = 0.05      
+lLegOffset = [0,0,-0.105]
+lLegNormal = [0,0,1]                                                                  
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+
+#~ AFTER loading obstacles
+larmId = 'hrp2_larm_rom'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [0,0,-0.1075]
+lArmNormal = [0,0,1]
+lArmx = 0.024; lArmy = 0.024
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
+
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [0,0,-0.1075]
+rArmNormal = [0,0,1]
+rArmx = 0.024; rArmy = 0.024
+#disabling collision for hook
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
+#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
+#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF")
+
+fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
+fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
+
+
+
+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} }
+
diff --git a/script/scenarios/demos/siggraph_asia/log.txt b/script/scenarios/demos/siggraph_asia/log.txt
deleted file mode 100644
index 0e3fdcf..0000000
--- a/script/scenarios/demos/siggraph_asia/log.txt
+++ /dev/null
@@ -1,1418 +0,0 @@
-path computation 27
-path computation 8
-path computation 11
-path computation 20
-path computation 4
-path computation 4
-path computation 4
-path computation 9
-path computation 4
-path computation 4
-path computation 7
-path computation 10
-path computation 4
-path computation 8
-path computation 10
-path computation 8
-path computation 9
-path computation 13
-path computation 10
-path computation 8
-path computation 13
-path computation 9
-path computation 10
-path computation 17
-path computation 11
-path computation 8
-path computation 5
-path computation 9
-path computation 9
-path computation 8
-path computation 13
-path computation 9
-path computation 4
-path computation 7
-path computation 14
-path computation 13
-path computation 8
-path computation 4
-path computation 3
-path computation 4
-path computation 6
-path computation 7
-path computation 2
-path computation 8
-path computation 3
-path computation 2
-path computation 10
-path computation 4
-path computation 9
-path computation 8
-path computation 2
-path computation 4
-path computation 3
-path computation 3
-path computation 6
-path computation 20
-path computation 9
-path computation 15
-path computation 17
-path computation 8
-path computation 7
-path computation 10
-path computation 7
-path computation 11
-path computation 8
-path computation 15
-path computation 10
-path computation 12
-path computation 13
-path computation 18
-path computation 17
-path computation 8
-path computation 14
-path computation 17
-path computation 15
-path computation 10
-path computation 15
-path computation 6
-path computation 8
-path computation 14
-path computation 11
-path computation 8
-path computation 12
-path computation 8
-path computation 9
-path computation 12
-path computation 9
-path computation 4
-path computation 12
-path computation 15
-path computation 13
-path computation 7
-path computation 7
-path computation 4
-path computation 10
-path computation 14
-path computation 12
-path computation 12
-path computation 13
-path computation 8
-path computation 11
-path computation 7
-path computation 15
-path computation 8
-path computation 5
-path computation 4
-path computation 7
-path computation 4
-path computation 9
-path computation 3
-path computation 5
-path computation 10
-path computation 9
-path computation 3
-path computation 5
-path computation 1
-path computation 2
-path computation 2
-path computation 5
-path computation 2
-path computation 4
-path computation 4
-path computation 1
-path computation 7
-path computation 1
-path computation 9
-path computation 2
-path computation 5
-path computation 3
-path computation 3
-path computation 2
-path computation 5
-path computation 5
-path computation 6
-path computation 7
-path computation 3
-path computation 4
-path computation 2
-path computation 3
-path computation 11
-path computation 4
-path computation 2
-path computation 6
-path computation 9
-path computation 6
-path computation 9
-path computation 6
-path computation 12
-path computation 16
-path computation 5
-path computation 10
-path computation 7
-path computation 7
-path computation 5
-path computation 3
-path computation 6
-path computation 5
-path computation 6
-path computation 7
-path computation 9
-path computation 8
-path computation 4
-path computation 8
-path computation 5
-path computation 3
-path computation 6
-path computation 11
-path computation 7
-path computation 4
-path computation 5
-path computation 6
-path computation 9
-path computation 10
-path computation 11
-path computation 7
-path computation 9
-path computation 3
-path computation 3
-path computation 4
-path computation 3
-path computation 3
-path computation 3
-path computation 5
-path computation 3
-path computation 4
-path computation 6
-path computation 7
-path computation 9
-path computation 7
-path computation 4
-path computation 7
-path computation 7
-path computation 5
-path computation 4
-path computation 5
-path computation 7
-path computation 7
-path computation 4
-path computation 4
-path computation 6
-path computation 5
-path computation 9
-path computation 9
-path computation 8
-path computation 6
-path computation 3
-path computation 7
-path computation 7
-path computation 9
-path computation 5
-path computation 6
-path computation 5
-path computation 9
-path computation 9
-path computation 9
-path computation 11
-path computation 6
-path computation 11
-path computation 9
-path computation 4
-path computation 10
-path computation 63
-path computation 3
-path computation 7
-path computation 4
-path computation 4
-path computation 11
-path computation 5
-path computation 5
-path computation 6
-path computation 4
-path computation 4
-path computation 4
-path computation 7
-path computation 4
-path computation 3
-path computation 6
-path computation 5
-path computation 8
-path computation 10
-path computation 16
-path computation 16
-path computation 14
-path computation 5
-path computation 8
-path computation 7
-path computation 4
-path computation 5
-path computation 5
-path computation 4
-path computation 3
-path computation 11
-path computation 5
-path computation 3
-path computation 4
-path computation 3
-path computation 11
-path computation 6
-path computation 7
-path computation 4
-path computation 3
-path computation 5
-path computation 9
-path computation 5
-path computation 8
-path computation 8
-path computation 4
-path computation 7
-path computation 7
-path computation 4
-path computation 3
-path computation 3
-path computation 6
-path computation 3
-path computation 4
-path computation 9
-path computation 5
-path computation 3
-path computation 3
-path computation 7
-path computation 8
-path computation 8
-path computation 3
-path computation 9
-path computation 10
-path computation 4
-path computation 5
-path computation 9
-path computation 5
-path computation 6
-path computation 4
-path computation 7
-path computation 11
-path computation 8
-path computation 4
-path computation 10
-path computation 6
-path computation 4
-path computation 8
-path computation 5
-path computation 8
-path computation 5
-path computation 4
-path computation 6
-path computation 3
-path computation 2
-path computation 3
-path computation 11
-path computation 6
-path computation 4
-path computation 12
-path computation 5
-path computation 6
-path computation 4
-path computation 7
-path computation 7
-path computation 5
-path computation 7
-path computation 10
-path computation 10
-path computation 4
-path computation 10
-path computation 7
-path computation 7
-path computation 11
-path computation 4
-path computation 11
-path computation 5
-path computation 5
-path computation 8
-path computation 8
-path computation 5
-path computation 9
-path computation 8
-path computation 6
-path computation 2
-path computation 5
-path computation 2
-path computation 2
-path computation 8
-path computation 13
-path computation 7
-path computation 10
-path computation 9
-path computation 6
-path computation 7
-path computation 21
-path computation 28
-path computation 3
-path computation 4
-path computation 22
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 1
-path computation 3
-path computation 1
-path computation 1
-path computation 2
-path computation 1
-path computation 1
-path computation 2
-path computation 2
-path computation 1
-path computation 3
-path computation 1
-path computation 3
-path computation 3
-path computation 3
-path computation 2
-path computation 3
-path computation 2
-path computation 2
-path computation 2
-path computation 4
-path computation 4
-path computation 3
-path computation 4
-path computation 2
-path computation 2
-path computation 2
-path computation 3
-path computation 3
-path computation 3
-path computation 2
-path computation 3
-path computation 3
-path computation 19
-path computation 2
-path computation 3
-path computation 3
-path computation 2
-path computation 5
-path computation 2
-path computation 2
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 22
-path computation 2
-path computation 3
-path computation 10
-path computation 15
-path computation 15
-path computation 2
-path computation 3
-path computation 2
-path computation 2
-path computation 7
-path computation 3
-path computation 4
-path computation 3
-path computation 4
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 4
-path computation 2
-path computation 17
-path computation 17
-path computation 3
-path computation 4
-path computation 2
-path computation 3
-path computation 3
-path computation 6
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 4
-path computation 3
-path computation 2
-path computation 2
-path computation 1
-path computation 1
-path computation 1
-path computation 2
-path computation 1
-path computation 1
-path computation 1
-path computation 3
-path computation 4
-path computation 4
-path computation 1
-path computation 4
-path computation 3
-path computation 4
-path computation 3
-path computation 4
-path computation 3
-path computation 5
-path computation 2
-path computation 2
-path computation 3
-path computation 2
-path computation 3
-path computation 17
-path computation 22
-path computation 4
-path computation 3
-path computation 2
-path computation 4
-path computation 4
-path computation 3
-path computation 4
-path computation 2
-path computation 3
-path computation 4
-path computation 3
-path computation 5
-path computation 3
-path computation 2
-path computation 3
-path computation 2
-path computation 2
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 2
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 2
-path computation 3
-path computation 4
-path computation 3
-path computation 2
-path computation 4
-path computation 2
-path computation 5
-path computation 4
-path computation 3
-path computation 1
-path computation 1
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 3
-path computation 34
-path computation 3
-path computation 3
-path computation 3
-path computation 2
-path computation 3
-path computation 31
-path computation 42
-path computation 22
-path computation 23
-path computation 22
-path computation 14
-path computation 18
-path computation 16
-path computation 8
-path computation 12
-path computation 4
-path computation 4
-path computation 9
-path computation 8
-path computation 12
-path computation 16
-path computation 8
-path computation 14
-path computation 10
-path computation 13
-path computation 13
-path computation 7
-path computation 11
-path computation 4
-path computation 4
-path computation 11
-path computation 7
-path computation 12
-path computation 7
-path computation 9
-path computation 4
-path computation 4
-path computation 9
-path computation 7
-path computation 4
-path computation 12
-path computation 9
-path computation 13
-path computation 6
-path computation 13
-path computation 5
-path computation 8
-path computation 10
-path computation 16
-path computation 12
-path computation 14
-path computation 10
-path computation 11
-path computation 13
-path computation 14
-path computation 10
-path computation 16
-path computation 4
-path computation 6
-path computation 10
-path computation 4
-path computation 6
-path computation 10
-path computation 12
-path computation 9
-path computation 12
-path computation 11
-path computation 9
-path computation 9
-path computation 8
-path computation 7
-path computation 7
-path computation 17
-path computation 7
-path computation 7
-path computation 8
-path computation 7
-path computation 9
-path computation 7
-path computation 11
-path computation 8
-path computation 10
-path computation 14
-path computation 16
-path computation 7
-path computation 12
-path computation 15
-path computation 12
-path computation 11
-path computation 15
-path computation 15
-path computation 11
-path computation 11
-path computation 11
-path computation 11
-path computation 15
-path computation 16
-path computation 16
-path computation 14
-path computation 4
-path computation 14
-path computation 9
-path computation 7
-path computation 5
-path computation 3
-path computation 11
-path computation 20
-path computation 19
-path computation 7
-path computation 20
-path computation 4
-path computation 11
-path computation 8
-path computation 30
-path computation 22
-path computation 15
-path computation 25
-path computation 21
-path computation 23
-path computation 22
-path computation 32
-path computation 21
-path computation 30
-path computation 27
-path computation 16
-path computation 22
-path computation 22
-path computation 5
-path computation 7
-path computation 9
-path computation 4
-path computation 13
-path computation 9
-path computation 2
-path computation 2
-path computation 2
-path computation 5
-path computation 8
-path computation 12
-path computation 86
-path computation 114
-path computation 222
-path computation 140
-path computation 261
-path computation 98
-path computation 194
-path computation 80
-path computation 285
-path computation 138
-path computation 122
-path computation 124
-path computation 266
-path computation 113
-path computation 48
-path computation 131
-path computation 195
-path computation 151
-path computation 109
-path computation 10
-path computation 17
-path computation 7
-path computation 8
-path computation 14
-path computation 23
-path computation 10
-path computation 10
-path computation 14
-path computation 11
-path computation 14
-path computation 12
-path computation 15
-path computation 21
-path computation 10
-path computation 9
-path computation 16
-path computation 10
-path computation 15
-path computation 38
-path computation 37
-path computation 24
-path computation 27
-path computation 33
-path computation 3
-path computation 6
-path computation 7
-path computation 10
-path computation 9
-path computation 6
-path computation 7
-path computation 4
-path computation 3
-path computation 5
-path computation 3
-path computation 13
-path computation 14
-path computation 33
-path computation 16
-path computation 23
-path computation 18
-path computation 22
-path computation 22
-path computation 18
-path computation 16
-path computation 24
-path computation 25
-path computation 32
-path computation 402
-path computation 383
-path computation 299
-path computation 63
-path computation 156
-path computation 25
-path computation 14
-path computation 17
-path computation 21
-path computation 14
-path computation 20
-path computation 24
-path computation 27
-path computation 21
-path computation 23
-path computation 21
-path computation 22
-path computation 13
-path computation 6
-path computation 8
-path computation 6
-path computation 11
-path computation 6
-path computation 7
-path computation 11
-path computation 10
-path computation 9
-path computation 8
-path computation 7
-path computation 7
-path computation 3
-path computation 5
-path computation 5
-path computation 5
-path computation 5
-path computation 10
-path computation 15
-path computation 10
-path computation 5
-path computation 6
-path computation 12
-path computation 13
-path computation 11
-path computation 6
-path computation 9
-path computation 5
-path computation 8
-path computation 15
-path computation 5
-path computation 8
-path computation 11
-path computation 9
-path computation 11
-path computation 13
-path computation 12
-path computation 12
-path computation 5
-path computation 3
-path computation 12
-path computation 7
-path computation 7
-path computation 5
-path computation 8
-path computation 9
-path computation 8
-path computation 5
-path computation 7
-path computation 4
-path computation 10
-path computation 8
-path computation 4
-path computation 12
-path computation 12
-path computation 6
-path computation 8
-path computation 8
-path computation 5
-path computation 6
-path computation 9
-path computation 3
-path computation 12
-path computation 6
-path computation 6
-path computation 3
-path computation 5
-path computation 7
-path computation 7
-path computation 7
-path computation 7
-path computation 11
-path computation 9
-path computation 9
-path computation 3
-path computation 3
-path computation 12
-path computation 4
-path computation 6
-path computation 5
-path computation 10
-path computation 6
-path computation 3
-path computation 3
-path computation 4
-path computation 6
-path computation 3
-path computation 3
-path computation 6
-path computation 11
-path computation 7
-path computation 6
-path computation 7
-path computation 5
-path computation 5
-path computation 6
-path computation 5
-path computation 13
-path computation 9
-path computation 15
-path computation 12
-path computation 7
-path computation 5
-path computation 7
-path computation 3
-path computation 5
-path computation 9
-path computation 10
-path computation 11
-path computation 3
-path computation 3
-path computation 10
-path computation 6
-path computation 8
-path computation 3
-path computation 3
-path computation 5
-path computation 12
-path computation 9
-path computation 7
-path computation 3
-path computation 3
-path computation 9
-path computation 7
-path computation 3
-path computation 11
-path computation 6
-path computation 7
-path computation 12
-path computation 8
-path computation 12
-path computation 7
-path computation 9
-path computation 9
-path computation 4
-path computation 7
-path computation 3
-path computation 5
-path computation 5
-path computation 7
-path computation 7
-path computation 12
-path computation 11
-path computation 13
-path computation 9
-path computation 13
-path computation 3
-path computation 5
-path computation 5
-path computation 6
-path computation 6
-path computation 4
-path computation 12
-path computation 11
-path computation 5
-path computation 8
-path computation 10
-path computation 13
-path computation 8
-path computation 5
-path computation 5
-path computation 12
-path computation 8
-path computation 3
-path computation 11
-path computation 8
-path computation 13
-path computation 8
-path computation 9
-path computation 9
-path computation 5
-path computation 5
-path computation 7
-path computation 5
-path computation 8
-path computation 10
-path computation 5
-path computation 6
-path computation 9
-path computation 9
-path computation 5
-path computation 3
-path computation 10
-path computation 8
-path computation 3
-path computation 3
-path computation 6
-path computation 6
-path computation 9
-path computation 9
-path computation 5
-path computation 3
-path computation 13
-path computation 8
-path computation 3
-path computation 8
-path computation 3
-path computation 11
-path computation 7
-path computation 5
-path computation 6
-path computation 8
-path computation 10
-path computation 7
-path computation 7
-path computation 12
-path computation 8
-path computation 6
-path computation 5
-path computation 4
-path computation 8
-path computation 9
-path computation 6
-path computation 6
-path computation 8
-path computation 24
-path computation 3
-path computation 3
-path computation 4
-path computation 5
-path computation 13
-path computation 9
-path computation 7
-path computation 3
-path computation 9
-path computation 3
-path computation 6
-path computation 13
-path computation 4
-path computation 7
-path computation 9
-path computation 12
-path computation 3
-path computation 11
-path computation 11
-path computation 5
-path computation 9
-path computation 7
-path computation 6
-path computation 3
-path computation 9
-path computation 9
-path computation 9
-path computation 6
-path computation 7
-path computation 8
-path computation 8
-path computation 10
-path computation 10
-path computation 8
-path computation 4
-path computation 10
-path computation 8
-path computation 5
-path computation 9
-path computation 11
-path computation 3
-path computation 9
-path computation 5
-path computation 3
-path computation 6
-path computation 7
-path computation 10
-path computation 6
-path computation 6
-path computation 6
-path computation 8
-path computation 15
-path computation 10
-path computation 12
-path computation 6
-path computation 3
-path computation 5
-path computation 4
-path computation 4
-path computation 3
-path computation 3
-path computation 8
-path computation 10
-path computation 10
-path computation 11
-path computation 11
-path computation 5
-path computation 6
-path computation 4
-path computation 7
-path computation 14
-path computation 7
-path computation 4
-path computation 12
-path computation 12
-path computation 5
-path computation 6
-path computation 13
-path computation 9
-path computation 13
-path computation 8
-path computation 10
-path computation 7
-path computation 6
-path computation 7
-path computation 10
-path computation 9
-path computation 9
-path computation 3
-path computation 3
-path computation 5
-path computation 3
-path computation 6
-path computation 7
-path computation 10
-path computation 12
-path computation 4
-path computation 10
-path computation 6
-path computation 9
-path computation 8
-path computation 8
-path computation 13
-path computation 3
-path computation 12
-path computation 14
-path computation 11
-path computation 3
-path computation 6
-path computation 7
-path computation 9
-path computation 5
-path computation 8
-path computation 11
-path computation 10
-path computation 15
-path computation 10
-path computation 8
-path computation 9
-path computation 3
-path computation 3
-path computation 11
-path computation 7
-path computation 10
-path computation 5
-path computation 9
-path computation 6
-path computation 11
-path computation 12
-path computation 7
-path computation 5
-path computation 12
-path computation 6
-path computation 6
-path computation 10
-path computation 5
-path computation 4
-path computation 3
-path computation 8
-path computation 10
-path computation 7
-path computation 11
-path computation 4
-path computation 3
-path computation 6
-path computation 3
-path computation 3
-path computation 3
-path computation 10
-path computation 3
-path computation 6
-path computation 11
-path computation 6
-path computation 14
-path computation 11
-path computation 10
-path computation 6
-path computation 3
-path computation 8
-path computation 11
-path computation 3
-path computation 3
-path computation 5
-path computation 35
-path computation 12
-path computation 5
-path computation 3
-path computation 3
-path computation 24
-path computation 59
-path computation 33
-path computation 17
-path computation 6
-path computation 15
-path computation 10
-path computation 6
-path computation 10
-path computation 8
-path computation 25
-path computation 51
-path computation 34
-path computation 64
-path computation 18
-path computation 9
-path computation 12
-path computation 9
-path computation 14
-path computation 11
-path computation 9
-path computation 9
-path computation 8
-path computation 10
-path computation 13
-path computation 7
-path computation 8
-path computation 11
-path computation 11
-path computation 15
-path computation 11
-path computation 13
-path computation 9
-path computation 10
-path computation 18
-path computation 10
-path computation 12
-path computation 10
-path computation 8
-path computation 8
-path computation 11
-path computation 17
-path computation 11
-path computation 13
-path computation 12
-path computation 11
-path computation 7
-path computation 9
-path computation 12
-path computation 9
-path computation 7
-path computation 7
-path computation 7
-path computation 9
-path computation 7
-path computation 11
-path computation 10
-path computation 11
-path computation 9
-path computation 7
-path computation 7
-path computation 7
-path computation 8
-path computation 11
-path computation 11
-path computation 7
-path computation 11
-path computation 10
-path computation 7
-path computation 17
-path computation 13
-path computation 16
-path computation 16
-path computation 13
-path computation 14
-path computation 11
-path computation 12
-path computation 9
-path computation 11
-path computation 12
-path computation 13
-path computation 10
-path computation 18
-path computation 8
-path computation 10
-path computation 11
-path computation 8
-path computation 14
-path computation 8
-path computation 7
-path computation 7
-path computation 14
-path computation 7
-path computation 14
-path computation 8
-path computation 12
-path computation 12
-path computation 7
-path computation 9
-path computation 11
-path computation 27
-path computation 47
-path computation 7
-path computation 10
-path computation 10
-path computation 7
-path computation 7
-path computation 13
-path computation 11
-path computation 7
-path computation 11
-path computation 11
-path computation 9
-path computation 9
-path computation 12
-path computation 7
-path computation 14
-path computation 10
-path computation 7
-path computation 11
-path computation 7
-path computation 12
-path computation 13
-path computation 7
-path computation 10
-path computation 10
-path computation 15
-path computation 15
-path computation 10
-path computation 11
-path computation 7
-path computation 10
-path computation 17
-path computation 7
-path computation 15
-path computation 13
-path computation 8
-path computation 12
-path computation 16
-path computation 8
-path computation 10
-path computation 10
-path computation 7
-path computation 8
-path computation 9
-path computation 11
-path computation 14
-path computation 10
-path computation 14
-path computation 30
-path computation 20
-path computation 26
-path computation 47
-path computation 17
-path computation 16
-path computation 22
-path computation 25
-path computation 25
-path computation 20
-path computation 21
-path computation 26
-path computation 20
-path computation 29
-path computation 21
-path computation 21
-path computation 22
-path computation 23
-path computation 14
-path computation 29
-path computation 17
-path computation 27
-path computation 18
-path computation 16
-path computation 24
-path computation 14
-path computation 27
-path computation 24
-path computation 33
-path computation 35
-path computation 14
-path computation 12
-path computation 11
-path computation 21
-path computation 2
-path computation 2
-path computation 2
-path computation 1
-path computation 2
-path computation 1
-path computation 2
-path computation 2
-path computation 2
-path computation 2
-path computation 4
-path computation 2
-path computation 4
-path computation 2
-path computation 1
-path computation 1
-path computation 3
-path computation 1
-path computation 3
-path computation 3
-path computation 7
-path computation 1
-path computation 4
-path computation 1
-path computation 6
-path computation 1
-path computation 1
-path computation 1
-path computation 2
-path computation 2
-path computation 1
-path computation 4
-path computation 1
-path computation 4
-path computation 1
-path computation 2
-path computation 5
-path computation 4
-path computation 1
-path computation 1
-path computation 2
-path computation 2
-path computation 3
-path computation 1
-path computation 4
-path computation 2
-path computation 4
-path computation 2
-path computation 2
-path computation 4
-path computation 5
-path computation 3
-path computation 1
-path computation 0
-path computation 0
-path computation 1
-path computation 2
-path computation 1
-path computation 4
-path computation 2
-path computation 7
-path computation 4
-path computation 3
-path computation 1
-path computation 4
-path computation 2
-path computation 1
-path computation 5
-path computation 4
-path computation 4
-path computation 1
-path computation 5
-path computation 4
-path computation 2
-path computation 2
-path computation 4
-path computation 2
-path computation 9
-path computation 2
-path computation 2
-path computation 8
-path computation 1
-path computation 2
-path computation 3
-path computation 7
-path computation 4
-path computation 7
-path computation 5
-path computation 5
-path computation 6
-path computation 3
-path computation 5
-path computation 5
-path computation 6
-path computation 7
-path computation 46
-path computation 66
-path computation 57
-path computation 26
-path computation 49
-path computation 6
-path computation 7
-path computation 7
-path computation 9
-path computation 8
-path computation 9
-path computation 54
-path computation 51
-path computation 25
-path computation 40
-path computation 22
-path computation 30
-path computation 26
-path computation 30
-path computation 34
-path computation 45
-path computation 33
diff --git a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
index f5298e8..a29017e 100644
--- a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
+++ b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
@@ -58,8 +58,7 @@ tp = ttp
 
 #~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
 #~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
-#~ gen(0,1)
-
+#~ g
 com_vel = [0.,0.,0.]
 com_acc = [0.,0.,0.]
 
@@ -125,10 +124,8 @@ def go0(sid, rg, num_optim = 0, mu = 0.6, s =None):
     if s == None:
         s = max(norm(array(configs[sid+1]) - array(configs[sid])), 1.) * 1.5
         print "$$$$$$$$$$$$$$$ S $$$$$$$$ *********************444444444444444444444444444 ", s
-    for i in range(rg-1):
-        path = gen(sid+i,1,mu=mu,num_optim=num_optim, s=s, gen_traj = False)
-	i = rg -1
-    path = gen(sid+i,1,mu=mu,num_optim=num_optim, s=s, gen_traj = True)
+    for i in range(rg):
+        path = gen(sid+i,1,mu=mu,num_optim=num_optim, s=s)
     return path
 
 def go2(sid, rg = 1, num_optim = 0, mu = 0.5, t =2, s =None):
diff --git a/script/scenarios/demos/siggraph_asia/spiderman/log.txt b/script/scenarios/demos/siggraph_asia/spiderman/log.txt
deleted file mode 100644
index b60b32c..0000000
--- a/script/scenarios/demos/siggraph_asia/spiderman/log.txt
+++ /dev/null
@@ -1,65 +0,0 @@
-path computation 7
-path computation 5
-path computation 8
-path computation 2
-path computation 5
-path computation 2
-path computation 2
-path computation 4
-path computation 2
-path computation 1
-path computation 1
-path computation 2
-path computation 1
-path computation 1
-path computation 2
-path computation 2
-path computation 1
-path computation 2
-path computation 2
-path computation 1
-path computation 3
-path computation 1
-path computation 4
-path computation 1
-path computation 1
-path computation 1
-path computation 0
-path computation 0
-path computation 0
-path computation 0
-path computation 23
-path computation 1
-path computation 1
-path computation 1
-path computation 0
-path computation 1
-path computation 2
-path computation 1
-path computation 3
-path computation 1
-path computation 2
-path computation 0
-path computation 3
-path computation 4
-path computation 2
-path computation 1
-path computation 1
-path computation 1
-path computation 3
-path computation 3
-path computation 1
-path computation 1
-path computation 1
-path computation 2
-path computation 1
-path computation 3
-path computation 1
-path computation 1
-path computation 1
-path computation 1
-path computation 1
-path computation 1
-path computation 4
-path computation 1
-path computation 4
diff --git a/script/scenarios/demos/siggraph_asia/stair_bauzil_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/stair_bauzil_hrp2_interp.py
index 3ab72a1..0dd8920 100755
--- a/script/scenarios/demos/siggraph_asia/stair_bauzil_hrp2_interp.py
+++ b/script/scenarios/demos/siggraph_asia/stair_bauzil_hrp2_interp.py
@@ -360,7 +360,7 @@ def reset():
 #~ a = go_stop(8,1,0,mu=2)
 #~ a = go(10,1,0,mu =2)
 
-a = go0    (0 ,10,0,mu=2)
+#~ a = go0    (0 ,10,0,mu=2)
 #~ a = go0    (1 ,1,0,mu=2)
 #~ a = go0    (2 ,1,0,mu=2)
 #~ a = go0    (3 ,1,0,mu=2)
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 1c1c09e..7e9c1b9 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -1709,10 +1709,8 @@ assert(s2 == s1 +1);
                 throw std::runtime_error("in comRRTFromPos, at least one com trajectory is not present in problem solver");
             }
             State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
-std::cout << "state 1" << paths[cT1]->end().head<3>() << std::endl;
             State s1Bis(state1);
             s1Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Bis,paths[cT1]->end().head<3>());
-std::cout << "ok. state 2" << paths[cT2]->end().head<3>() << std::endl;
             State s2Bis(state2);
             s2Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Bis,paths[cT2]->end().head<3>());
 
-- 
GitLab