Commit 8bc0f1e4 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[tool] add/update tools script

parent ed054fb4
...@@ -139,6 +139,8 @@ install(FILES ...@@ -139,6 +139,8 @@ install(FILES
data/urdf/flat_step.urdf data/urdf/flat_step.urdf
data/urdf/flat_hole.urdf data/urdf/flat_hole.urdf
data/urdf/cross_gap.urdf data/urdf/cross_gap.urdf
data/urdf/floor_bauzil.urdf
data/urdf/floor_bauzil_obstacles.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
) )
install(FILES install(FILES
...@@ -215,6 +217,8 @@ install(FILES ...@@ -215,6 +217,8 @@ install(FILES
data/srdf/flat_step.srdf data/srdf/flat_step.srdf
data/srdf/flat_hole.srdf data/srdf/flat_hole.srdf
data/srdf/cross_gap.srdf data/srdf/cross_gap.srdf
data/srdf/floor_bauzil.srdf
data/srdf/floor_bauzil_obstacles.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
) )
install(FILES install(FILES
...@@ -295,6 +299,8 @@ install(FILES ...@@ -295,6 +299,8 @@ install(FILES
data/meshes/flat_hole.stl data/meshes/flat_hole.stl
data/meshes/cross_gap.stl data/meshes/cross_gap.stl
data/meshes/cross_gap2.stl data/meshes/cross_gap2.stl
data/meshes/floor_bauzil.stl
data/meshes/floor_bauzil_obstacles.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
) )
......
...@@ -771,7 +771,7 @@ module hpp ...@@ -771,7 +771,7 @@ module hpp
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error); boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error); floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
floatSeq isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in boolean addPathPerPhase, in floatSeq timings,in short numPointsPerPhase)raises (Error); floatSeq isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in boolean addPathPerPhase, in floatSeq timings,in short numPointsPerPhase)raises (Error);
......
...@@ -7,7 +7,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper ...@@ -7,7 +7,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
#from display_tools import * #from display_tools import *
import talos.flatGround_pyrene_pathKino as tp import talos.flatGround_pyrene_pathKino as tp
import time import time
from planning.robot_config.talos import * from robot_config.talos import *
tPlanning = tp.tPlanning tPlanning = tp.tPlanning
...@@ -156,7 +156,7 @@ print "number of configs :", len(configsFull) ...@@ -156,7 +156,7 @@ print "number of configs :", len(configsFull)
from planning.configs.talos_flatGround import * from configs.talos_flatGround import *
from generate_contact_sequence import * from generate_contact_sequence import *
beginState = 0 beginState = 0
......
...@@ -4,7 +4,7 @@ from hpp.corbaserver import Client ...@@ -4,7 +4,7 @@ from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent from hpp.corbaserver.robot import Robot as Parent
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
import omniORB.any import omniORB.any
from planning.configs.straight_walk_config import * from configs.talos_flatGround import *
import time import time
class Robot (Parent): class Robot (Parent):
......
from stair_bauzil_hrp2_interp import *
import generate_muscod_problem as mp
from pathlib2 import Path
import muscodSSH as ssh
tryWarmStart = True
print "run bench with feasibility criterion"
com = fullBody.getCenterOfMass()
if com[0] > 1.25 :
success = True
else :
success = False
numConf = len(configs)
if success :
# muscod without warm start :
if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
mp.generate_muscod_problem(filename_xml,True)
successMuscod,ssh_ok = ssh.call_muscod()
time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
if tryWarmStart :
if not success :
# generate warm start from planning :
if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs = generateContactSequenceWithInitGuess(ps,fullBody,configs,beginState,endState)
cs.saveAsXML(filename, "ContactSequence")
mp.generate_muscod_problem(filename_xml,True)
successMuscod,ssh_ok = ssh.call_muscod()
time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodWarmStartConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
else :
muscodWarmStartConverged= True
else :
muscodWarmStartConverged = False
else :
muscodConverged = False
muscodWarmStartConverged = False
tInterpolateConfigs = 0.
numConf = 0.
name = "/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_croc.log"
f = open(name,"a")
f.write("new\n")
f.write("success "+str(success)+"\n")
f.write("muscodNoWarmStart "+str(muscodConverged)+"\n")
f.write("muscodWarmStart "+str(muscodWarmStartConverged)+"\n")
f.write("time "+str(tInterpolateConfigs)+"\n")
f.write("configs "+str(numConf)+"\n")
f.close()
from stair_bauzil_hrp2_interp import *
import generate_muscod_problem as mp
from pathlib2 import Path
import muscodSSH as ssh
tryWarmStart = False
print "run bench without feasibility criterion"
com = fullBody.getCenterOfMass()
if com[0] > 1.3:
success = True
else :
success = False
#success = False
numConf = len(configs)
if success :
# muscod without warm start :
if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
mp.generate_muscod_problem(filename_xml,True)
successMuscod,ssh_ok = ssh.call_muscod()
time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
crocConverged = True
for id_state in range(beginState,endState):
pid = fullBody.isDynamicallyReachableFromState(id_state,id_state+1,numPointsPerPhases=0)
if len(pid) == 0:
print "Croc did not converge for state "+str(id_state)
crocConverged = False
else :
crocConverged = False
muscodConverged = False
tInterpolateConfigs = 0.
numConf = 0.
name = "/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_noCroc.log"
f = open(name,"a")
f.write("new\n")
f.write("success "+str(success)+"\n")
f.write("muscodNoWarmStart "+str(muscodConverged)+"\n")
f.write("crocConverged "+str(crocConverged)+"\n")
f.write("time "+str(tInterpolateConfigs)+"\n")
f.write("configs "+str(numConf)+"\n")
f.close()
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
from disp_bezier import * from disp_bezier import *
max_acc = 1 import numpy as np
pointsPerPhase=4 max_acc = 5.
pointsPerPhase=5
ROUND = 1000. ROUND = 1000.
global curves_initGuess global curves_initGuess
...@@ -10,6 +11,14 @@ global timings_initGuess ...@@ -10,6 +11,14 @@ global timings_initGuess
curves_initGuess = [] curves_initGuess = []
timings_initGuess = [] timings_initGuess = []
def stdVecToMatrix(std_vector):
vec_l = []
for vec in std_vector:
vec_l.append(vec)
res = np.hstack(tuple(vec_l))
return res
def compute_time_array(t_qp,t_c): def compute_time_array(t_qp,t_c):
...@@ -45,57 +54,70 @@ def check_acceleration_bounds(ddc_qp,t_discretized): ...@@ -45,57 +54,70 @@ def check_acceleration_bounds(ddc_qp,t_discretized):
print "acceleration between : "+str(min)+" ; "+str(max) print "acceleration between : "+str(min)+" ; "+str(max)
return True; return True;
def check_projection(s,c,dc,ddc,t): def check_projection(s,c,t):
return True
success = s.projectToCOM(c(t).transpose().tolist()[0],500) success = s.projectToCOM(c(t).transpose().tolist()[0],500)
if not success : if not success :
print "Unable to project to com at time : "+str(t) print "Unable to project to com at time : "+str(t)
return False return False
# check dynamic stability else:
return True
def check_stability(s,c,dc,ddc,t):
q = s.q() q = s.q()
q[-6:-3] = dc(t).transpose().tolist()[0] q[-6:-3] = dc(t).transpose().tolist()[0]
q[-3:] = ddc(t).transpose().tolist()[0] q[-3:] = ddc(t).transpose().tolist()[0]
success = s.fullBody.isConfigBalanced(q,s.getLimbsInContact()) success = s.fullBody.isConfigBalanced(q,s.getLimbsInContact(),CoM = c(t).transpose().tolist()[0])
if not success : if not success :
print "UNSTABLE at time : "+str(t) print "UNSTABLE at time : "+str(t)
return False #print "q = ",q
return True return False
else :
return True
def check_projection_path(s0,s1,c,dc,ddc,t_qp,t_per_phase): def check_projection_path(s0,s1,c,dc,ddc,t_per_phase):
kin_valid = True
stab_valid = True
moving_limb = s0.contactsVariations(s1) moving_limb = s0.contactsVariations(s1)
s0_orig = State(s0.fullBody,q=s0.q(),limbsIncontact=s0.getLimbsInContact())
s1_orig = State(s1.fullBody,q=s1.q(),limbsIncontact=s1.getLimbsInContact())
if len(moving_limb)>1: if len(moving_limb)>1:
print "Too many contact variation between adjacent states" print "Too many contact variation between adjacent states"
return False return False,False
if len(moving_limb)==0: if len(moving_limb)==0:
print "No contact between adjacent states" print "No contact between adjacent states"
return True return True,True
print "test for phase 0 :" #print "test for phase 0 :"
for t in t_per_phase[0]: for t in t_per_phase[0]:
if not check_projection(s0,c,dc,ddc,t): if kin_valid and not check_projection(s0,c,t):
return False kin_valid = False
if stab_valid and not check_stability(s0_orig,c,dc,ddc,t):
stab_valid = False
smid,success = StateHelper.removeContact(s0,moving_limb[0]) smid,success = StateHelper.removeContact(s0,moving_limb[0])
if not success: smid_orig,success_orig = StateHelper.removeContact(s0_orig,moving_limb[0])
if not (success and success_orig):
print "Error in creation of intermediate state" print "Error in creation of intermediate state"
return False return False,False
print "test for phase 1 :" #print "test for phase 1 :"
for t in t_per_phase[1]: for t in t_per_phase[1]:
#if t == t_per_phase[0][-1]: if kin_valid and not check_projection(smid,c,t):
# t -= EPS kin_valid = False
if not check_projection(smid,c,dc,ddc,t): if stab_valid and not check_stability(smid_orig,c,dc,ddc,t):
return False stab_valid = False
print "test for phase 2" #print "test for phase 2"
# go in reverse order for p2, it's easier for the projection : # go in reverse order for p2, it's easier for the projection :
for i in range(1,len(t_per_phase[2])+1): for i in range(1,len(t_per_phase[2])+1):
t = t_per_phase[1][-i] t = t_per_phase[2][-i]
if not check_projection(s1,c,dc,ddc,t): if kin_valid and not check_projection(s1,c,t):
return False kin_valid = False
if stab_valid and not check_stability(s1_orig,c,dc,ddc,t):
t = t_per_phase[1][-1] stab_valid = False
if not check_projection(s1,c,dc,ddc,t):
return False return kin_valid,stab_valid
return True
def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None): def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
global curves_initGuess global curves_initGuess
...@@ -116,14 +138,96 @@ def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None): ...@@ -116,14 +138,96 @@ def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
print "### test acceleration bounds ###" print "### test acceleration bounds ###"
valid = valid and check_acceleration_bounds(ddc_qp,t_discretized) valid = valid and check_acceleration_bounds(ddc_qp,t_discretized)
print "### test kinematic projection ###" print "### test kinematic projection ###"
valid = valid and check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_qp,t_per_phase) valid = valid and check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_per_phase)
curves_initGuess.append(c_qp) curves_initGuess.append(c_qp)
timings_initGuess.append(t_qp) timings_initGuess.append(t_qp)
return valid return valid
def genTimeArray(Ts,dt):
t = 0.
res = []
current_t = 0.
for phase_id in range(len(Ts)):
t_phase = []
while t <= (current_t + Ts[phase_id]):
t_phase += [t]
t += dt
res += [t_phase]
current_t += Ts[phase_id]
return res
def check_traj_valid(ps,fullBody,s0_,s1_,pIds,dt=0.001):
# create copy of original states :
s0 = State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact())
s1 = State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact())
fullBody.setStaticStability(False)
Ts = [ps.pathLength(int(pIds[1])), ps.pathLength(int(pIds[2])), ps.pathLength(int(pIds[3]))]
t_array = genTimeArray(Ts,dt)
#print "Time array : ",t_array
c_qp = fullBody.getPathAsBezier(int(pIds[0]))
dc_qp = c_qp.compute_derivate(1)
ddc_qp = dc_qp.compute_derivate(1)
return check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_array)
def check_muscod_traj(fullBody,cs,s0_,s1_):
if cs.size() != 3 :
print "Contact sequence must be of size 3 (one step)"
return False,False
kin_valid = True
stab_valid = True
stab_valid_discretized = True
# create copy of original states :
s0 = State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact())
s1 = State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact())
moving_limb = s0.contactsVariations(s1)
smid,success = StateHelper.removeContact(s0,moving_limb[0])
if not success:
print "Error in creation of intermediate state"
return False,False
phases = []
c_array = []
dc_array = []
ddc_array = []
t_array = []
states = [s0,smid,s1]
states_copy = [State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact()), State(fullBody,q=smid.q(),limbsIncontact=smid.getLimbsInContact()), State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact()) ]
for k in range(3) :
phases += [cs.contact_phases[k]]
state_traj = stdVecToMatrix(phases[k].state_trajectory).transpose()
control_traj = stdVecToMatrix(phases[k].control_trajectory).transpose()
c_array = state_traj[:,:3]
dc_array = state_traj[:,3:6]
ddc_array = control_traj[:,:3]
t_array = stdVecToMatrix(phases[k].time_trajectory).transpose()
for i in range(len(c_array)):
#if kin_valid and not states[k].projectToCOM(c_array[i].tolist()[0],500) :
# kin_valid = False
q = states_copy[k].q()
q[-6:-3] = dc_array[i].tolist()[0]
q[-3:] = ddc_array[i].tolist()[0]
if stab_valid_discretized and not states_copy[k].fullBody.isConfigBalanced(q,states_copy[k].getLimbsInContact(),CoM = c_array[i].tolist()[0]) :
stab_valid_discretized = False
for i in range(int(len(c_array)/100)):
q = states_copy[k].q()
q[-6:-3] = dc_array[i*100].tolist()[0]
q[-3:] = ddc_array[i*100].tolist()[0]
if stab_valid and not states_copy[k].fullBody.isConfigBalanced(q,states_copy[k].getLimbsInContact(),CoM = c_array[i*100].tolist()[0]) :
stab_valid = False
return kin_valid,stab_valid, stab_valid_discretized
def check_contact_plan(ps,r,pp,fullBody,idBegin,idEnd): def check_contact_plan(ps,r,pp,fullBody,idBegin,idEnd):
fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0]) fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
...@@ -166,4 +270,12 @@ valid = True ...@@ -166,4 +270,12 @@ valid = True
t_qp = [ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))] t_qp = [ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]
t_discretized,t_per_phase = compute_time_array(t_qp) t_discretized,t_per_phase = compute_time_array(t_qp)
""" """
\ No newline at end of file
"""
cs = ContactSequenceHumanoid(0)
cs.loadFromXML('/home/pfernbac/Documents/muscod/uc-dual/PROBLEM/random/res/contact_sequence_trajectory.xml',CONTACT_SEQUENCE_XML_TAG)
"""
...@@ -5,11 +5,11 @@ from pinocchio.utils import * ...@@ -5,11 +5,11 @@ from pinocchio.utils import *
import locomote import locomote
from locomote import WrenchCone,SOC6,ControlType,IntegratorType,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid from locomote import WrenchCone,SOC6,ControlType,IntegratorType,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
import planning.generate_muscod_problem as mp import generate_muscod_problem as mp
import muscodSSH as ssh import muscodSSH as ssh
from planning.config import * from config import *
statesPerStep=4 # number of double support configs from the planning per call to muscod statesPerStep=5 # number of double support configs from the planning per call to muscod
stepSize=statesPerStep*2 - 1 # contact_sequence contain double support AND simple support states stepSize=statesPerStep*2 - 1 # contact_sequence contain double support AND simple support states
def solveMuscodProblem(configsFull,cs): def solveMuscodProblem(configsFull,cs):
......
name = 's2'
gui.addSphere(name,0.01,[1,0,0,1])
gui.setVisibility(name,"ALWAYS_ON_TOP")
gui.addToGroup(name,"world")
gui.applyConfiguration(name,p)
gui.refresh()
gui=r.client.gui
name = 's2'
gui.addSphere(name,0.01,[1,0,0,1])
gui.setVisibility(name,"ALWAYS_ON_TOP")
gui.addToGroup(name,r.sceneName)
gui.applyConfiguration(name,p)
gui.refresh()
...@@ -4,7 +4,6 @@ from hpp.gepetto import Viewer ...@@ -4,7 +4,6 @@ from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
import numpy as np import numpy as np
#from hpp.corbaserver.rbprm.tools.cwc_trajectory import * #from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr from hpp import Error as hpperr
from numpy import array, matrix from numpy import array, matrix
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
...@@ -21,7 +20,7 @@ srdfSuffix = "" ...@@ -21,7 +20,7 @@ srdfSuffix = ""
fullBody = FullBody () fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.3,0.3, -0.3, 0.3, 0.6, 0.7]) fullBody.setJointBounds ("base_joint_xyz", [-0.7,0.7, -0.7, 0.7, 0.6, 0.7])
fullBody.setJointBounds ("CHEST_JOINT0", [0.,0.]) fullBody.setJointBounds ("CHEST_JOINT0", [0.,0.])
fullBody.setJointBounds ("CHEST_JOINT1", [0.,0.]) fullBody.setJointBounds ("CHEST_JOINT1", [0.,0.])
fullBody.setJointBounds ("HEAD_JOINT0", [0.,0.]) fullBody.setJointBounds ("HEAD_JOINT0", [0.,0.])
...@@ -61,7 +60,8 @@ rLegLimbOffset=[0,0,-0.035]#0.035 ...@@ -61,7 +60,8 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal = [0,0,1] rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05 rLegx = 0.09; rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward") #fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",limbOffset=rLegLimbOffset) fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True) fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db") #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
...@@ -71,11 +71,14 @@ lLegLimbOffset=[0,0,0.035] ...@@ -71,11 +71,14 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal = [0,0,1] lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05 lLegx = 0.09; lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward") #fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "manipulability", 0.01,"_6_DOF",limbOffset=lLegLimbOffset) fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.2)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True) fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db") #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
rleg = 'RLEG_JOINT0'
rfoot = "RLEG_JOINT5"
lleg = 'LLEG_JOINT0'
lfoot = "LLEG_JOINT5"
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}, limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
...@@ -223,21 +226,35 @@ def _feet_far_enough(fullBody,q): ...@@ -223,21 +226,35 @@ def _feet_far_enough(fullBody,q):