Commit a9984e01 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

finished generation in theory

parent 40c3c41a
......@@ -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
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)
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} }
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} }
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)
......@@ -31,10 +31,7 @@ def compute_contact_points_from_contact_dictionary(robot, contacts):
N = mat_zeros((3,ncp));
i = 0;