darpa trunk large

CMakeLists.txt

@@ -215,6 +215,7 @@ install(FILES
 	data/meshes/hyq/hyq_trunk_large.stl
 	data/meshes/hyq/hyq_trunk_large2.stl
 	data/meshes/hyq/hyq_trunk_large3.stl
+	data/meshes/hyq/hyq_trunk_large4.stl
 	data/meshes/hyq/hyq_rom.stl
 	data/meshes/hyq/hyq_rhleg_rom.stl
 	data/meshes/hyq/hyq_rfleg_rom.stl

data/urdf/hyq/hyq_trunk_large.urdf

@@ -16,7 +16,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0" scale="5 5 5"/>
       <geometry>
-        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk_large.stl" scale="1 1 1"/>
+        <mesh filename="package://hpp-rbprm-corba/meshes/hyq/hyq_trunk_large4.stl" scale="1 1 1"/>
       </geometry>
     </collision>
   </link>

profile/log.txt

+path computation 190045
+contact: 114
+no contact: 2
+planner failed: 1
+unstable contact: 4

profile/profile.py

@@ -9,7 +9,7 @@ import time
 #~ scenarios = ['standing_hrp2']
 scenarios = ['darpa_hyq']
 #~ scenarios = ['stair_bauzil_hrp2']
-n_trials = 20
+n_trials = 10
 
 stats = ['balance','collision','ik']
 stats_optim = ['time_cwc','com_traj']

profile/scenarios/darpa_hyq_interp.py

@@ -83,10 +83,10 @@ r(q_init)
 
 configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 0, filterStates = True)
 
-"""
+
 print "number of configs =", len(configs)
 r(configs[-1])
-
+"""
 from hpp.gepetto import PathPlayer
 pp = PathPlayer (fullBody.client.basic, r)
 

profile/scenarios/darpa_hyq_pathKino.py

@@ -73,6 +73,7 @@ ps.selectPathPlanner("DynamicPlanner")
 r(q_init)
 
 
+
 t = ps.solve ()
 if isinstance(t, list):
 	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
@@ -82,6 +83,7 @@ f.close()
 
 
 
+
 camera = [0.6293167471885681,
  -9.560577392578125,
  10.504343032836914,

script/dynamic/log.txt

@@ -2487,3 +2487,75 @@ test balance                            0.08	0.16	0.30	20.33	127
 contact: 133
 planner succeeded: 1
 unstable contact: 2
+contact: 10
+no contact: 3
+planner failed: 1
+unstable contact: 1
+contact: 15
+no contact: 6
+planner failed: 2
+unstable contact: 1
+contact: 9
+no contact: 4
+planner failed: 1
+unstable contact: 1
+contact: 8
+no contact: 3
+planner failed: 1
+unstable contact: 1
+contact: 136
+planner failed: 1
+unstable contact: 1
+contact: 508
+planner failed: 1
+unstable contact: 1
+contact: 9
+no contact: 2
+planner failed: 1
+unstable contact: 1
+contact: 296
+planner failed: 1
+unstable contact: 1
+contact: 269
+planner failed: 1
+contact: 400
+planner failed: 1
+contact: 90
+planner failed: 1
+unstable contact: 1
+contact: 256
+planner failed: 1
+unstable contact: 1
+contact: 254
+planner failed: 1
+unstable contact: 1
+contact: 535
+planner failed: 1
+unstable contact: 1
+contact: 100
+planner failed: 1
+unstable contact: 1
+contact: 72
+planner failed: 1
+unstable contact: 1
+contact: 361
+planner failed: 1
+unstable contact: 1
+contact: 420
+planner failed: 1
+unstable contact: 1
+contact: 9
+no contact: 3
+planner failed: 1
+contact: 264
+planner failed: 1
+unstable contact: 1
+
+*** PROFILING RESULTS [ms] (min - avg - max - total time - nSamples) ***
+collision                               0.04	0.12	0.44	38.60	324
+complete generation                     40886.81	40886.81	40886.81	40886.81	1
+ik                                      0.04	0.09	0.20	37.89	423
+test balance                            0.10	0.15	0.25	21.70	146
+contact: 150
+planner succeeded: 1
+unstable contact: 4

script/dynamic/stair_bauzil_hrp2_interp.py

@@ -31,14 +31,14 @@ 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, 20000, "manipulability", 0.1)
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "forward", 0.1)
 
 lLegId = 'hrp2_lleg_rom'
 lLeg = 'LLEG_JOINT0'
 lLegOffset = [0,0,-0.105]
 lLegNormal = [0,0,1]
 lLegx = 0.09; lLegy = 0.05
-fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "forward", 0.1)
 
 rarmId = 'hrp2_rarm_rom'
 rarm = 'RARM_JOINT0'
@@ -47,7 +47,7 @@ rArmOffset = [0,0,-0.1]
 rArmNormal = [0,0,1]
 rArmx = 0.024; rArmy = 0.024
 #disabling collision for hook
-fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "manipulability", 0.05, "_6_DOF", True)
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 50000, "forward", 0.05, "_6_DOF", True)
 
 """
 
@@ -105,7 +105,7 @@ acc_goal = tp.ps.configAtParam(0,tp.ps.pathLength(0))[tp.indexECS+3:tp.indexECS+
 
 
 
-fullBody.setStaticStability(False)
+fullBody.setStaticStability(True)
 # Randomly generating a contact configuration at q_init
 fullBody.setCurrentConfig (q_init)
 r(q_init)
@@ -131,9 +131,9 @@ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId])
 
 
 
-configs = fullBody.interpolate(0.1,pathId=1,robustnessTreshold = 3, filterStates = True)
+configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 0, filterStates = True)
 print "number of configs :", len(configs)
-
+r(configs[-1])
 
 
 from hpp.gepetto import PathPlayer