Commit 7a71de96 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scripts]refactor all memmo / contact talos scripts

parent 87071cd6
from talos_rbprm.talos import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
from scenarios.memmo.talos_circle_path import PathPlanner
from scenarios.memmo.talos_contact_generator import TalosContactGenerator
import time
print("Plan guide trajectory ...")
from . import talos_circle_path as tp
print("Done.")
import time
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
print("Path planning OK.")
f.write("Planning_success: True"+"\n")
f.close()
else :
print("Error during path planning")
f.write("Planning_success: False"+"\n")
f.close()
import sys
sys.exit(1)
fullBody = Robot ()
# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-2,2, -2, 2, 0.9, 1.05])
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
joint6L_bounds_prev=fullBody.getJointBounds('leg_left_6_joint')
joint2L_bounds_prev=fullBody.getJointBounds('leg_left_2_joint')
joint6R_bounds_prev=fullBody.getJointBounds('leg_right_6_joint')
joint2R_bounds_prev=fullBody.getJointBounds('leg_right_2_joint')
fullBody.setJointBounds('leg_left_6_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_left_2_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_right_6_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_right_2_joint',[-0.25,0.25])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
v = FakeViewer()
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
#q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
if abs(tp.q_goal[1]) <= abs(tp.q_goal[0]) :
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
heuristicR = "fixedStep08"
heuristicL = "fixedStep08"
print("Use weight for straight walk")
fullBody.usePosturalTaskContactCreation(True)
else :
fullBody.setPostureWeights(fullBody.postureWeights_straff[::]+[0]*6)
print("Use weight for straff walk")
if tp.q_goal[1] < 0 :
print("start with right leg")
heuristicL = "static"
heuristicR = "fixedStep06"
else:
print("start with left leg")
heuristicR = "static"
heuristicL = "fixedStep06"
fullBody.setCurrentConfig (q_init)
print("Generate limb DB ...")
tStart = time.time()
# generate databases :
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristicR, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = vel_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = vel_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('talos/base_link',0.3)
v(q_init)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
# specify the full body configurations as start and goal state of the problem
if q_goal[1] < 0: # goal on the right side of the circle, start motion with right leg first
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
else :
fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.005,pathId=pId,robustnessTreshold = 3, filterStates = True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print("Done.")
print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
if len(configs) < 2 :
cg_success = False
print("Error during contact generation.")
else:
cg_success = True
print("Contact generation Done.")
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print("Contact generation successful.")
cg_reach_goal = True
else:
print("Contact generation failed to reach the goal.")
cg_reach_goal = False
if len(configs) > 5 :
cg_too_many_states = True
cg_success = False
print("Discarded contact sequence because it was too long.")
else:
cg_too_many_states = False
f = open(statusFilename,"a")
f.write("cg_success: "+str(cg_success)+"\n")
f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n")
f.write("cg_too_many_states: "+str(cg_too_many_states)+"\n")
f.close()
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.pid = 0
self.dt = 0.005
def set_joints_bounds(self):
super().set_joints_bounds()
self.fullbody.setConstrainedJointsBounds()
def set_reference(self, use_postural_task = True):
super().set_reference(use_postural_task)
# weights used for the postural task depend of the direction of the motion
if abs(self.path_planner.q_goal[1]) <= abs(self.path_planner.q_goal[0]):
self.fullbody.setPostureWeights(self.fullbody.postureWeights[::] + [0] * 6)
print("Use weight for straight walk")
self.fullbody.usePosturalTaskContactCreation(True)
else:
self.fullbody.setPostureWeights(self.fullbody.postureWeights_straff[::] + [0] * 6)
print("Use weight for straff walk")
def load_limbs(self, heuristic = "fixedStep06", analysis=None, nb_samples=None, octree_size=None):
# heuristic used depend on the direction of the motion
if abs(self.path_planner.q_goal[1]) <= abs(self.path_planner.q_goal[0]):
heuristicR = "fixedStep08"
heuristicL = "fixedStep08"
else:
if self.path_planner.q_goal[1] < 0:
print("start with right leg")
heuristicL = "static"
heuristicR = "fixedStep06"
self.init_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
self.end_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
else:
print("start with left leg")
heuristicR = "static"
heuristicL = "fixedStep06"
self.init_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
self.end_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
print("Generate limb DB ...")
t_start = time.time()
# generate databases :
nbSamples = 100000
self.fullbody.addLimb(self.fullbody.rLegId, self.fullbody.rleg, self.fullbody.rfoot,
self.fullbody.rLegOffset, self.fullbody.rLegNormal,
self.fullbody.rLegx, self.fullbody.rLegy,
nbSamples, heuristicR, 0.01,
kinematicConstraintsPath=self.fullbody.rLegKinematicConstraints,
kinematicConstraintsMin=0.85)
if heuristicR == "static":
self.fullbody.runLimbSampleAnalysis(self.fullbody.rLegId, "ReferenceConfiguration", True)
self.fullbody.addLimb(self.fullbody.lLegId, self.fullbody.lleg, self.fullbody.lfoot,
self.fullbody.lLegOffset, self.fullbody.rLegNormal,
self.fullbody.lLegx, self.fullbody.lLegy,
nbSamples, heuristicL, 0.01,
kinematicConstraintsPath=self.fullbody.lLegKinematicConstraints,
kinematicConstraintsMin=0.85)
if heuristicL == "static":
self.fullbody.runLimbSampleAnalysis(self.fullbody.lLegId, "ReferenceConfiguration", True)
t_generate = time.time() - t_start
print("Databases generated in : " + str(t_generate) + " s")
def run(self):
super().run()
self.fullbody.resetJointsBounds()
self.write_status(5)
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
if (not cg_success) or cg_too_many_states or (not cg_reach_goal):
import sys
sys.exit(1)
# put back original bounds for wholebody methods
fullBody.setJointBounds('leg_left_6_joint',joint6L_bounds_prev)
fullBody.setJointBounds('leg_left_2_joint',joint2L_bounds_prev)
fullBody.setJointBounds('leg_right_6_joint',joint6R_bounds_prev)
fullBody.setJointBounds('leg_right_2_joint',joint2R_bounds_prev)
from talos_rbprm.talos import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.tools.display_tools import *
import numpy as np
import time
print("Plan guide trajectory ...")
from . import talos_circle_oriented_path as tp
print("Done.")
from scenarios.memmo.talos_circle_oriented_path import PathPlanner
from scenarios.memmo.talos_contact_generator import TalosContactGenerator
import time
statusFilename = tp.statusFilename
pId = 0
f = open(statusFilename,"a")
if tp.ps.numberPaths() > 0 :
print("Path planning OK.")
f.write("Planning_success: True"+"\n")
f.close()
else :
print("Error during path planning")
f.write("Planning_success: False"+"\n")
f.close()
import sys
sys.exit(1)
fullBody = Robot ()
# Set the bounds for the root
fullBody.setJointBounds ("root_joint", [-2,2, -2, 2, 0.9, 1.05])
## reduce bounds on joints along x, to put conservative condition on the contact generation for sideway steps
joint1L_bounds_prev=fullBody.getJointBounds('leg_left_1_joint')
joint6L_bounds_prev=fullBody.getJointBounds('leg_left_6_joint')
joint2L_bounds_prev=fullBody.getJointBounds('leg_left_2_joint')
joint1R_bounds_prev=fullBody.getJointBounds('leg_right_1_joint')
joint6R_bounds_prev=fullBody.getJointBounds('leg_right_6_joint')
joint2R_bounds_prev=fullBody.getJointBounds('leg_right_2_joint')
fullBody.setJointBounds('leg_left_1_joint',[-0.34,1.4])
fullBody.setJointBounds('leg_left_6_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_left_2_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_right_1_joint',[-1.4,0.34])
fullBody.setJointBounds('leg_right_6_joint',[-0.25,0.25])
fullBody.setJointBounds('leg_right_2_joint',[-0.25,0.25])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
try :
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
v = FakeViewer()
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
#q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
if tp.alpha <0.8*np.pi and tp.alpha > 0.2*np.pi : # nearly straight walk
heuristicR = "fixedStep08"
heuristicL = "fixedStep08"
fullBody.usePosturalTaskContactCreation(True)
else: # more complex motion. Do smaller steps and allow non-straight feet orientation
heuristicR = "fixedStep06"
heuristicL = "fixedStep06"
fullBody.setCurrentConfig (q_init)
print("Generate limb DB ...")
tStart = time.time()
# generate databases :
nbSamples = 100000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, heuristicR, 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, heuristicL, 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
if not(tp.alpha <0.8*np.pi and tp.alpha > 0.2*np.pi) : # not close to straight line
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
print("Done.")
print("Databases generated in : "+str(tGenerate)+" s")
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0)[0:7]
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
vel_init = [0,0,0]
acc_init = [0,0,0]
vel_goal = [0,0,0]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = vel_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = vel_goal[::]
q_goal[configSize+3:configSize+6] = acc_goal[::]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
v(q_init)
fullBody.setCurrentConfig (q_goal)
v(q_goal)
v.addLandmark('talos/base_link',0.3)
v(q_init)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
# specify the full body configurations as start and goal state of the problem
if q_goal[1] < 0: # goal on the right side of the circle, start motion with right leg first
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
print("Right foot first")
else :
fullBody.setStartState(q_init,[fullBody.lLegId,fullBody.rLegId])
fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
print("Left foot first")
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.005,pathId=pId,robustnessTreshold = 2, filterStates = True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print("Done.")
print("Contact plan generated in : "+str(tInterpolateConfigs)+" s")
print("number of configs :", len(configs))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
if len(configs) < 2 :
cg_success = False
print("Error during contact generation.")
else:
cg_success = True
print("Contact generation Done.")
if abs(configs[-1][0] - tp.q_goal[0]) < 0.01 and abs(configs[-1][1]- tp.q_goal[1]) < 0.01 and (len(fullBody.getContactsVariations(len(configs)-2,len(configs)-1))==1):
print("Contact generation successful.")
cg_reach_goal = True
else:
print("Contact generation failed to reach the goal.")
cg_reach_goal = False
if len(configs) > 20 :
cg_too_many_states = True
cg_success = False
print("Discarded contact sequence because it was too long.")
else:
cg_too_many_states = False
f = open(statusFilename,"a")
f.write("cg_success: "+str(cg_success)+"\n")
f.write("cg_reach_goal: "+str(cg_reach_goal)+"\n")
f.write("cg_too_many_states: "+str(cg_too_many_states)+"\n")
f.close()
if (not cg_success) or cg_too_many_states:
import sys
sys.exit(1)
# put back original bounds for wholebody methods
fullBody.setJointBounds('leg_left_1_joint',joint1L_bounds_prev)
fullBody.setJointBounds('leg_left_6_joint',joint6L_bounds_prev)
fullBody.setJointBounds('leg_left_2_joint',joint2L_bounds_prev)
fullBody.setJointBounds('leg_right_1_joint',joint1R_bounds_prev)
fullBody.setJointBounds('leg_right_6_joint',joint6R_bounds_prev)
fullBody.setJointBounds('leg_right_2_joint',joint2R_bounds_prev)
import numpy as np
class ContactGenerator(TalosContactGenerator):
def __init__(self):
super().__init__(PathPlanner())
self.pid = 0
self.dt = 0.005
def load_fullbody(self):
from talos_rbprm.talos import Robot
Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
def set_joints_bounds(self):
super().set_joints_bounds()
self.fullbody.setConstrainedJointsBounds()
def load_limbs(self, heuristic = "fixedStep06", analysis=None, nb_samples=None, octree_size=None):
# heuristic used depend on the direction of the motion
if 0.8 * np.pi > self.path_planner.alpha > 0.2 * np.pi: # nearly straight walk
print("use straight walk heuristic")
heuristic = "fixedStep08"
analysis = None
self.fullbody.usePosturalTaskContactCreation(True)
else: # more complex motion. Do smaller steps and allow non-straight feet orientation
print("use smaller steps heuristic")
analysis = "ReferenceConfiguration"
heuristic = "fixedStep06"
self.fullbody.loadAllLimbs(heuristic, analysis, nbSamples= 100000)
if self.path_planner.q_goal[1] < 0:
print("start with right leg")
self.init_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
self.end_contacts = [self.fullbody.rLegId, self.fullbody.lLegId]
else:
print("start with left leg")
self.init_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
self.end_contacts = [self.fullbody.lLegId, self.fullbody.rLegId]
def compute_configs_from_guide(self):
super().compute_configs_from_guide()
vel_init = [0, 0, 0]
acc_init = [0, 0, 0]
vel_goal = [0, 0, 0]
acc_goal = [0, 0, 0]
configSize = self.fullbody.getConfigSize() - self.fullbody.client.robot.getDimensionExtraConfigSpace()
self.q_init[configSize:configSize + 3] = vel_init[::]
self.q_init[configSize + 3:configSize + 6] = acc_init[::]
self.q_goal[configSize:configSize + 3] = vel_goal[::]
self.q_goal[configSize + 3:configSize + 6] = acc_goal[::]
def run(self):
super().run()
self.fullbody.resetJointsBounds()
self.write_status(20)
if __name__ == "__main__":
cg = ContactGenerator()
cg.run()
......@@ -48,7 +48,7 @@ class PathPlanner(TalosPathPlanner):
print("final root position : ", self.q_goal)
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.alpha = alpha
# write problem in files :
f = open(self.status_filename, "w")
f.write("q_init= " + str(self.q_init) + "\n")
......
from scenarios.talos_contact_generator import TalosContactGenerator as Parent
class TalosContactGenerator(Parent):
"""
Add specific method for benchmark,
write result of planning / contact generation in file
"""
def __init__(self,path_planner):
super().__init__(path_planner)
self.status_filename = self.path_planner.status_filename
f = open(self.status_filename, "a")
if self.path_planner.ps.numberPaths() > 0:
print("Path planning OK.")
f.write("Planning_success: True" + "\n")
f.close()
else:
print("Error during path planning")
f.write("Planning_success: False" + "\n")
f.close()
import sys
sys.exit(1)
def write_status(self, max_configs):
if len(self.configs) < 2:
cg_success = False
print("Error during contact generation.")
else:
cg_success = True
print("Contact generation Done.")
if abs(self.configs[-1][0] - self.path_planner.q_goal[0]) < 0.01 and\
abs(self.configs[-1][1] - self.path_planner.q_goal[1]) < 0.01 and\
(len(self.fullbody.getContactsVariations(len(self.configs) - 2, len(self.configs) - 1)) == 1):
print("Contact generation successful.")
cg_reach_goal = True
else:
print("Contact generation failed to reach the goal.")
cg_reach_goal = False
if len(self.configs) > max_configs: