finished generation in theory

script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py

@@ -7,6 +7,7 @@ from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
 
 from hpp import Error as hpperr
 from numpy import array, matrix
+import random
 
 import sample_com_vel as scv
 
@@ -169,17 +170,31 @@ def pos_quat_to_pinocchio(q):
     q_res[3:6] = quat_end
     return q_res
 
-def gen_contact_candidates_one_limb(limb, data, num_candidates = 10):
+def gen_contact_candidates_one_limb(limb, data, num_candidates = 10, projectToObstacles = False):
     effectorName = limbsCOMConstraints[limb]['effector']
     candidates = []
     config_candidates = [] #DEBUG
-    for i in range(num_candidates):
-        q_contact = fullBody.getSample(limb,randint(0,n_samples - 1))
-        fullBody.setCurrentConfig(q_contact)
-        m = _getTransform(fullBody.getJointPosition(effectorName))
-        candidates.append(m)
-        config_candidates.append(q_contact) #DEBUG
-        #~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
+    if(projectToObstacles):
+        candidates = fullBody.getContactSamplesProjected(limb, fullBody.getCurrentConfig(),[0,0,1],10000)
+        random.shuffle(candidates)
+        print "num candidates", len(candidates)
+        print "num limlb", limb
+        if(len(candidates) > 0):
+            raw_input("enter somethin")
+        for i in range(len(candidates)):
+            q_contact = candidates[i]
+            fullBody.setCurrentConfig(q_contact)
+            m = _getTransform(fullBody.getJointPosition(effectorName))
+            candidates.append(m)
+            config_candidates.append(q_contact) #DEBUG
+    else:
+        for i in range(num_candidates):
+            q_contact = fullBody.getSample(limb,randint(0,n_samples - 1))
+            fullBody.setCurrentConfig(q_contact)
+            m = _getTransform(fullBody.getJointPosition(effectorName))
+            candidates.append(m)
+            config_candidates.append(q_contact) #DEBUG
+            #~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
     data[effectorName]["transforms"] = candidates
     return config_candidates #DEBUG
         
@@ -223,7 +238,7 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto, or
     return False
     
     
-def gen_contact_candidates(limbs, config_gepetto, res, contact_points):
+def gen_contact_candidates(limbs, config_gepetto, res, contact_points, num_candidates = 10, projectToObstacles = False):
     #first generate a com translation current configuration
     fullBody.setCurrentConfig(config_gepetto)
     c = matrix(fullBody.getCenterOfMass())
@@ -232,12 +247,14 @@ def gen_contact_candidates(limbs, config_gepetto, res, contact_points):
         data = {}
         data["v0"] = v0
         data["candidates_per_effector"] = {}
-        configs_candidates = [] #DEBUG      
+        configs_candidates = {} #DEBUG      
         data["init_config"] = config_gepetto  #DEBUG    
         for limb in limbsCOMConstraints.keys(): 
-            print "limb ", limb
+            #~ print "limb ", limb
             if (predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto, contact_points)):  #all good, all contacts kinematically maintained
-                configs_candidates.append(gen_contact_candidates_one_limb(limb, data["candidates_per_effector"], 10)) #DEBUG 
+               val  = gen_contact_candidates_one_limb(limb, data["candidates_per_effector"], num_candidates, projectToObstacles) #DEBUG 
+               if len(val) > 0:
+                    configs_candidates[limb] = val
         if(len(data["candidates_per_effector"].keys()) >0):
             #~ if((data["candidates_per_effector"].has_key('RARM_JOINT5') and not data["candidates_per_effector"].has_key('LARM_JOINT5')) or
                 #~ (data["candidates_per_effector"].has_key('LARM_JOINT5') and not data["candidates_per_effector"].has_key('RARM_JOINT5'))):
@@ -312,25 +329,31 @@ def _genbalance(limbs):
 
 all_qs = []
 all_states = []
-def gen(limbs, num_samples = 1000, coplanar = True):
+def gen(limbs, num_samples = 1000, coplanar = True, num_candidates_per_config = 1, num_contact_candidates = 10, q_entries = None, projectToObstacles = False):
     q_0 = fullBody.getCurrentConfig(); 
     #~ fullBody.getSampleConfig()
     qs = []; qs_gepetto = []; states = []    
     data = {}
     fill_contact_points(limbs, fullBody, data)
-    for _ in range(num_samples):
-        if(coplanar):
-            q = fullBody.generateGroundContact(limbs)
+    if(q_entries != None):
+        num_samples = len(q_entries)
+        print "num_sample", num_samples, len(q_entries)
+    for i in range(num_samples):
+        if(q_entries == None):
+            if(coplanar):
+                q = fullBody.generateGroundContact(limbs)
+            else:
+                q = _genbalance(limbs)
         else:
-            q = _genbalance(limbs)
+            q = q_entries[i]
         q_gep = q[:]
         quat_end = q[4:7]
         q[6] = q[3]
         q[3:6] = quat_end
         res = {}
         res["q"] = q[:]
-        for _ in range(1):
-            gen_contact_candidates(limbs, q_gep, res, data["contact_points"])
+        for _ in range(num_candidates_per_config):
+            gen_contact_candidates(limbs, q_gep, res, data["contact_points"], num_contact_candidates, projectToObstacles)
         if(res.has_key("candidates")): #contact candidates found
             states.append(res)
             qs.append(q)
@@ -343,13 +366,14 @@ def gen(limbs, num_samples = 1000, coplanar = True):
     fname += "configs"
     if(coplanar):
         fname += "_coplanar"
-	data["samples"] = states
+    data["samples"] = states
     from pickle import dump
     #~ f1=open("configs_feet_on_ground_static_eq", 'w+')
     f1=open(fname, 'w+')
     dump(data, f1)
     f1.close()
     all_states.append(states)
+    return all_states
 
 j=0
 
@@ -366,19 +390,21 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg
 #~ limbs = [[lLegId,rLegId, rarmId]]
 #~ limbs = [[larmId, rarmId]]
 
-gen(limbs[0], 10)
-for ls in limbs:
-    gen(ls, 10, False)
+#~ gen(limbs[0], 10)
+#~ for ls in limbs:
+    #~ gen(ls, 10, False)
 #~ gen(limbs[0], 10)
     
 i = 0
+a = None
+b = None
 
-a = all_states[0][0]['candidates'][0]
-b = a ['candidates_per_effector']
+#~ a = all_states[0][0]['candidates'][0]
+#~ b = a ['candidates_per_effector']
 def init():
     r(a['init_config'])
     
-b = a ['candidates_per_effector']
+#~ b = a ['candidates_per_effector']
 
 def rleg():
     r(b['RLEG_JOINT5']['projected_config'])
@@ -392,6 +418,20 @@ def larm():
 def rarm():
     r(b['RARM_JOINT5']['projected_config'])
     
+def rlegi(j):                 
+    r(b['config_candidates'][rLegId][j])
+                             
+def llegi(j):                 
+    r(b['config_candidates'][lLegId][j])
+                             
+def larmi(j):                 
+    r(b['config_candidates'][larmId][j])
+                             
+def rarmi(j):                 
+    r(b['config_candidates'][rarmId][j])
+    
+    
+    
 def c1():
     r(b['config_candidates'][0][0])
     
@@ -404,6 +444,10 @@ def c3():
 def c4():
     r(b['config_candidates'][3][0])
     
+    
+def cij(l,m):
+    r(b['config_candidates'][l][m])
+    
 def inc():
     global i
     global a
@@ -411,4 +455,10 @@ def inc():
     i+=1
     a = all_states[0][i]['candidates'][0]
     b = a ['candidates_per_effector']
+    
+def a():
+    return a
+    
+def b():
+    return b
 

script/tools/tro_capture_point/gen_stair_bauzil.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_bauzil_hrp2_interp as stair
+import gen_hrp2_statically_balanced_positions_2d as gen
+#~ 
+configs = stair.configs[3:6]
+#~ configs = [stair.configs[4]]
+
+limbs = ['1lLeg', '0rLeg', '3Rarm']
+#~ limbs = ['1lLeg', '0rLeg']
+print ("configs", len(configs))
+all_states = gen.gen(limbs, num_samples = 0, coplanar = False, num_candidates_per_config = 10, num_contact_candidates = 10, q_entries = configs, projectToObstacles = True)

script/tools/tro_capture_point/gen_strai_bauzil.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_bauzil_hrp2_path as tp
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,-0.105,0,]
+rLegNormal = [0,1,0]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
+
+lLegId = '1lLeg'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+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, 10000, "manipulability", 0.05, "_6_DOF", True)
+
+
+#~ AFTER loading obstacles
+larmId = '4Larm'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.05,-0.050,-0.050]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
+
+rKneeId = '0RKnee'
+rLeg = 'RLEG_JOINT0'
+rKnee = 'RLEG_JOINT3'
+rLegOffset = [0.105,0.055,0.017]
+rLegNormal = [-1,0,0]
+rLegx = 0.05; rLegy = 0.05
+#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
+#~ 
+lKneeId = '1LKnee'
+lLeg = 'LLEG_JOINT0'
+lKnee = 'LLEG_JOINT3'
+lLegOffset = [0.105,0.055,0.017]
+lLegNormal = [-1,0,0]
+lLegx = 0.05; lLegy = 0.05
+#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
+ #~ 
+
+fullBody.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])
+
+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_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+q_init =  [
+        0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,                         	 # Free flyer 0-6
+        0.0, 0.0, 0.0, 0.0,                                                  # CHEST HEAD 7-10
+        0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.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)
+#~ 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} }
+
+

script/tools/tro_capture_point/stair_bauzil_hrp2_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+
+import stair_bauzil_hrp2_path as tp
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+
+#~ 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.1)
+
+lLegId = 'hrp2_lleg_rom'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
+
+rarmId = 'hrp2_rarm_rom'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+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, 10000, "manipulability", 0.05, "_6_DOF", True)
+
+
+#~ AFTER loading obstacles
+larmId = '4Larm'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.05,-0.050,-0.050]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
+
+rKneeId = '0RKnee'
+rLeg = 'RLEG_JOINT0'
+rKnee = 'RLEG_JOINT3'
+rLegOffset = [0.105,0.055,0.017]
+rLegNormal = [-1,0,0]
+rLegx = 0.05; rLegy = 0.05
+#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
+#~ 
+lKneeId = '1LKnee'
+lLeg = 'LLEG_JOINT0'
+lKnee = 'LLEG_JOINT3'
+lLegOffset = [0.105,0.055,0.017]
+lLegNormal = [-1,0,0]
+lLegx = 0.05; lLegy = 0.05
+#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
+ #~ 
+
+fullBody.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])
+
+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_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
+
+
+fullBody.setCurrentConfig (q_init)
+q_init =  [
+        0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0,                         	 # Free flyer 0-6
+        0.0, 0.0, 0.0, 0.0,                                                  # CHEST HEAD 7-10
+        0.261799388,  0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.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)
+#~ 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} }
+
+

script/tools/tro_capture_point/stair_bauzil_hrp2_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+
+rootJointType = 'freeflyer'
+packageName = 'hpp-rbprm-corba'
+meshPackageName = 'hpp-rbprm-corba'
+urdfName = 'hrp2_trunk_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, 1, 0, 2.2])
+#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
+#~ rbprmBuilder.setAffordanceFilter('3Rarm', ['Support'])
+#~ rbprmBuilder.setAffordanceFilter('0rLeg', ['Support',])
+#~ rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
+rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support'])
+rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
+rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
+#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
+#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
+#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
+#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
+rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [0, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+#~ q_init [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+
+q_goal = q_init [::]
+q_goal [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]
+q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal)
+#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
+#~ afftool.analyseAll()
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+#~ ps.solve ()
+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()
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ 
+#~ pp (2)
+#~ pp (0)
+
+#~ pp (1)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
+r (q_init)

script/tools/tro_capture_point/utils.py

@@ -31,10 +31,7 @@ def compute_contact_points_from_contact_dictionary(robot, contacts):
     N   = mat_zeros((3,ncp));