Skip to content

GitLab

Commit 70ea14c6 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scripts] refactor memmo/talos_*_path scripts

parent 211fc7b7
Hide whitespace changes
Inline Side-by-side
script/scenarios/abstract_path_planner.py
View file @ 70ea14c6
......@@ -113,24 +113,32 @@ class AbstractPathPlanner:
def addLandmark(self,a,b):
return
self.v = FakeViewer()
self.pp = PathPlayer(self.v)
try:
self.pp = PathPlayer(self.v)
except Exception:
pass
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
def init_planner(self, kinodynamic = True):
def init_planner(self, kinodynamic = True, optimize = True):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
:param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
"""
self.ps.selectConfigurationShooter("RbprmShooter")
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
if kinodynamic:
self.ps.addPathOptimizer("RandomShortcutDynamic")
self.ps.selectSteeringMethod("RBPRMKinodynamic")
self.ps.selectDistance("Kinodynamic")
self.ps.selectPathPlanner("DynamicPlanner")
if optimize:
if kinodynamic:
self.ps.addPathOptimizer("RandomShortcutDynamic")
else:
self.ps.addPathOptimizer("RandomShortcut")
def solve(self):
......@@ -160,10 +168,11 @@ class AbstractPathPlanner:
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.1)
"""
if path_id < 0:
path_id = self.ps.numberPaths()-1
self.pp.dt = dt
self.pp.displayVelocityPath(path_id)
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths()-1
self.pp.dt = dt
self.pp.displayVelocityPath(path_id)
def play_path(self, path_id = -1, dt = 0.01):
"""
......@@ -171,10 +180,11 @@ class AbstractPathPlanner:
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.01)
"""
if path_id < 0:
path_id = self.ps.numberPaths()-1
self.pp.dt = dt
self.pp(path_id)
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths()-1
self.pp.dt = dt
self.pp(path_id)
def hide_rom(self):
"""
......
script/scenarios/demos/talos_stairs10_path.py
View file @ 70ea14c6
......@@ -17,6 +17,8 @@ class PathPlanner(TalosPathPlanner):
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
self.ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True)
self.ps.setParameter("Kinodynamic/verticalAccelerationBound", 3.)
def compute_extra_config_bounds(self):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
......
script/scenarios/memmo/talos_bauzil_with_stairs_path.py 0 → 100644
View file @ 70ea14c6
from tools.sample_root_config import generate_random_conf_without_orientation
from scenarios.talos_path_planner import TalosPathPlanner
from talos_rbprm.talos_abstract import Robot
class PathPlanner(TalosPathPlanner):
Z_FLOOR = Robot.ref_height
Z_PLATFORM = Robot.ref_height + 0.6
BOUNDS_ALL = [-2.6, 4.7, -1.5, 3.3, Z_FLOOR, Z_PLATFORM]
BOUNDS_ALL_PLAT = [-2.1, 2.4, -0.1, 1.6, Z_PLATFORM, Z_PLATFORM]
BOUNDS_FLOOR = [-2.6, 4.7, -1.5, 3.3, Z_FLOOR, Z_FLOOR]
BOUNDS_PLAT_10 = [1.9, 2.4, -0.1, 1.6, Z_PLATFORM, Z_PLATFORM]
BOUNDS_PLAT_15 = [-2.1, -1.5, -0.1, 0.5, Z_PLATFORM, Z_PLATFORM]
def __init__(self):
super().__init__()
self.status_filename = "/res/infos.log"
self.root_translation_bounds = self.BOUNDS_ALL
self.v_max = 0.3
self.a_max = 0.2
def load_rbprm(self):
# select model with conservative collision geometry for trunk
Robot.urdfName += "_large_reducedROM"
# select conservative ROM for feet
Robot.urdfNameRom += ['talos_lleg_rom_reduced', 'talos_rleg_rom_reduced']
self.rbprmBuilder = Robot()
self.rbprmBuilder.setReferenceEndEffector('talos_lleg_rom_reduced', self.rbprmBuilder.ref_EE_lLeg)
self.rbprmBuilder.setReferenceEndEffector('talos_rleg_rom_reduced', self.rbprmBuilder.ref_EE_rLeg)
def compute_extra_config_bounds(self):
# bounds for the extradof : by default use v_max/a_max on x and y axis and a large value on z axis
self.extra_dof_bounds = [-self.v_max, self.v_max, -self.v_max, self.v_max, -2, 2,
-self.a_max, self.a_max, -self.a_max, self.a_max, -3, 3]
def init_problem(self):
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 500)
# force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
self.ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True)
self.ps.setParameter("Kinodynamic/verticalAccelerationBound", 3.)
self.ps.setMaxIterPathPlanning(50000)
def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
import random
random.seed()
# sample an initial and goal zone : either floor, right platform or left platform. With more weight on the floor
# 0,1,2 are floor, 3 is platform10 4 is platform15
WEIGHT_FLOOR = 3
plat_id_init = random.randint(0, WEIGHT_FLOOR + 1)
plat_id_goal = random.randint(0, WEIGHT_FLOOR + 1)
while plat_id_goal >= WEIGHT_FLOOR and plat_id_goal == plat_id_init: # cannot have init and end on the same platform (too easy)
plat_id_goal = random.randint(0, WEIGHT_FLOOR + 1)
print(("platform id init : ", plat_id_init))
print(("platform id goal : ", plat_id_goal))
#plat_id_init = 3
#plat_id_goal = 0
if plat_id_init < WEIGHT_FLOOR:
print("init of flat floor")
init_bounds = self.BOUNDS_FLOOR[::]
elif plat_id_init == WEIGHT_FLOOR:
print("init on 10cm platform")
init_bounds = self.BOUNDS_PLAT_10[::]
elif plat_id_init == WEIGHT_FLOOR + 1:
print("init on 15cm platform")
init_bounds = self.BOUNDS_PLAT_15[::]
else:
print("Unknown case")
if plat_id_goal < WEIGHT_FLOOR:
print("goal of flat floor")
goal_bounds = self.BOUNDS_FLOOR[::]
elif plat_id_goal == WEIGHT_FLOOR:
print("goal on 10cm platform")
goal_bounds = self.BOUNDS_PLAT_10[::]
elif plat_id_goal == WEIGHT_FLOOR + 1:
print("goal on 15cm platform")
goal_bounds = self.BOUNDS_PLAT_15[::]
else:
print("Unknown case")
root_bounds = [0] * 6
for i in range(3):
root_bounds[i * 2] = min(init_bounds[i * 2], goal_bounds[i * 2])
root_bounds[i * 2 + 1] = max(init_bounds[i * 2 + 1], goal_bounds[i * 2 + 1])
self.rbprmBuilder.setJointBounds("root_joint", root_bounds)
self.q_init = generate_random_conf_without_orientation(self.rbprmBuilder, init_bounds, self.v)
self.q_goal = generate_random_conf_without_orientation(self.rbprmBuilder, goal_bounds, self.v)
print(("init config : ", self.q_init))
print(("goal config : ", self.q_goal))
# write problem in files :
f = open(self.status_filename, "w")
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
f.close()
def run(self):
self.init_problem()
self.init_viewer("multicontact/bauzil_stairs", visualize_affordances=["Support"], reduce_sizes=[0.12,0.,0.])
self.set_random_configs()
self.init_planner()
self.solve()
print("Original path length : ", self.ps.pathLength(self.ps.numberPaths() - 1))
# optimize several time the path
for i in range(5):
print(("Optimize path, " + str(i + 1) + "/5 ... "))
self.ps.optimizePath(self.ps.numberPaths() - 1)
print(("New path length : ", self.ps.pathLength(self.ps.numberPaths() - 1)))
# remove the very beginning and end of the path to avoid orientation discontinuities
self.ps.extractPath(self.ps.numberPaths() - 1, 0.05,self.ps.pathLength(self.ps.numberPaths() - 1) - 0.05)
pathId = self.ps.numberPaths() - 1
self.q_init = self.ps.configAtParam(pathId, 0)
self.q_goal = self.ps.configAtParam(pathId, self.ps.pathLength(pathId))
self.display_path()
#self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
script/scenarios/memmo/talos_circle_oriented_path.py
View file @ 70ea14c6
from talos_rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import numpy as np
from scenarios.talos_path_planner import TalosPathPlanner
from pinocchio import Quaternion
import time
statusFilename = "/res/infos.log"
vMax = 0.5# linear velocity bound for the root
vInit = 0.05# initial / final velocity to fix the direction
vGoal = 0.01
aMax = 0.05# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder.setJointBounds ("root_joint", [-2,2, -2, 2, 1., 1.])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(['talos_lleg_rom','talos_rleg_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",0.5)
ps.setParameter("Kinodynamic/forceYawOrientation",True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp_environments", "multicontact/ground", "planning", vf)
#load the viewer
try :
v = vf.createViewer(displayArrows = True)
except Exception:
print("No viewer started !")
class FakeViewer():
sceneName = ""
def __init__(self):
return
def __call__(self,q):
return
def addLandmark(self,a,b):
return
v = FakeViewer()
v.addLandmark(v.sceneName,0.5)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
q_init = rbprmBuilder.getCurrentConfig ();
q_init[0:3] = [0,0,1.]
q_init[3:7] = [0,0,0,1]
q_init[-6] = vInit
# sample random position on a circle of radius 2m
radius = 1.
import random
random.seed()
#alpha = random.uniform(0.,2.*np.pi)
alpha = random.uniform(0.,2.*np.pi)
print("Test on a circle, alpha = ",alpha)
q_goal = q_init[::]
q_goal [0:3] = [radius*np.sin(alpha), -radius*np.cos(alpha), 1.]
# set final orientation to be along the circle :
vx = np.matrix([1,0,0]).T
v_goal = np.matrix([q_goal[0],q_goal[1],0]).T
quat = Quaternion.FromTwoVectors(vx,v_goal)
q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
q_goal[-6] = vGoal*np.sin(alpha)
q_goal[-5] = -vGoal*np.cos(alpha)
v(q_goal)
print("initial root position : ",q_init)
print("final root position : ",q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# write problem in files :
f = open(statusFilename,"w")
f.write("q_init= "+str(q_init)+"\n")
f.write("q_goal= "+str(q_goal)+"\n")
f.close()
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
success = ps.client.problem.prepareSolveStepByStep()
if not success:
print("planning failed.")
import sys
sys.exit(1)
ps.client.problem.finishSolveStepByStep()
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayVelocityPath(0)
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
import numpy as np
class PathPlanner(TalosPathPlanner):
def __init__(self):
super().__init__()
self.status_filename = "/res/infos.log"
self.v_max = 0.5
self.a_max = 0.05
self.v_init = 0.05
self.v_goal = 0.01
self.radius = 1.
self.root_translation_bounds = [-2,2, -2, 2, 1., 1.]
def init_problem(self):
super().init_problem()
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
self.q_init[0:3] = [0, 0, 1.]
self.q_init[3:7] = [0, 0, 0, 1]
self.q_init[-6] = self.v_init
import random
random.seed()
alpha = random.uniform(0.,2.*np.pi)
#alpha = 4.
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 1.]
# set final orientation to be along the circle :
vx = np.matrix([1, 0, 0]).T
v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T
quat = Quaternion.FromTwoVectors(vx, v_goal)
self.q_goal[3:7] = quat.coeffs().tolist()
# set final velocity to fix the orientation :
self.q_goal[-6] = self.v_goal * np.sin(alpha)
self.q_goal[-5] = -self.v_goal * np.cos(alpha)
self.v(self.q_goal)
print("initial root position : ", self.q_init)
print("final root position : ", self.q_goal)
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
# write problem in files :
f = open(self.status_filename, "w")
f.write("q_init= " + str(self.q_init) + "\n")
f.write("q_goal= " + str(self.q_goal) + "\n")
f.close()
def run(self):
self.init_problem()
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.set_random_configs()
self.init_planner(optimize= False)
success = self.ps.client.problem.prepareSolveStepByStep()
if not success:
print("planning failed.")
import sys
sys.exit(1)
self.ps.client.problem.finishSolveStepByStep()
self.display_path()
#self.play_path()
self.hide_rom()
if __name__ == "__main__":
planner = PathPlanner()
planner.run()
script/scenarios/memmo/talos_circle_path.py
View file @ 70ea14c6
from talos_rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
from scenarios.talos_path_planner import TalosPathPlanner
import numpy as np
import time
statusFilename = "/res/infos.log"
vMax = 0.5# linear velocity bound for the root
aMax = 0.5# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
rbprmBuilder.setJointBounds ("root_joint", [-2,2, -2, 2, 0.95, 1.05])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(['talos_lleg_rom','talos_rleg_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-1.7,1.7,-0.1,0.1,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",0.2)
ps.setParameter("DynamicPlanner/sizeFootY",0.12)
ps.setParameter("DynamicPlanner/friction",0.5)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp_environments", "multicontact/ground", "planning", vf)
try :
v = vf.createViewer(displayArrows = True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self,q):
return
v = FakeViewer()
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
q_init = rbprmBuilder.getCurrentConfig ();
q_init[0:3] = [0,0,1.]
q_init[3:7] = [0,0,0,1]
# sample random position on a circle of radius 2m
radius = 0.3
import random
random.seed()
alpha = random.uniform(0.,2.*np.pi)
print("Test on a circle, alpha = ",alpha)
q_goal = q_init[::]
q_goal [0:3] = [radius*np.sin(alpha), -radius*np.cos(alpha), 1.]
print("initial root position : ",q_init[0:3])
print("final root position : ",q_goal[0:3])
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# write problem in files :
f = open(statusFilename,"w")
f.write("q_init= "+str(q_init)+"\n")
f.write("q_goal= "+str(q_goal)+"\n")
f.close()
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve ()
print("Guide planning time : ",t)
try :
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
pp.displayVelocityPath(0)
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
class PathPlanner(TalosPathPlanner):
def __init__(self):
super().__init__()
self.status_filename = "/res/infos.log"
self.v_max = 0.5
self.a_max = 0.5
self.radius = 0.3
def set_random_configs(self):
"""
randomly sample initial and goal configuration :
"""
self.q_init[0:3] = [0, 0, 1.]
self.q_init[3:7] = [0, 0, 0, 1]
import random
random.seed()
alpha = random.uniform(0., 2. * np.pi)
print("Test on a circle, alpha = ", alpha)
self.q_goal = self.q_init[::]
self.q_goal[0:3] = [self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 1.]
print("initial root position : ", self.q_init[0:3])
print("final root position : ", self.q_goal[0:3])
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
# write problem in files :
f = open(self.status_filename, "w")