def init_viewer(self,
env_name,
env_package="hpp_environments",
reduce_sizes=[0, 0, 0],
visualize_affordances=[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" +
env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
def init_viewer(self,
env_name,
env_package="hpp_environments",
reduce_sizes=[0, 0, 0],
visualize_affordances=[],
min_area=None):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
:param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance
"""
vf = ViewerFactory(self.ps)
if self.context:
self.afftool = AffordanceTool(context=self.context)
self.afftool.client.affordance.affordance.resetAffordanceConfig(
) # FIXME: this should be called in afftool constructor
else:
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
if min_area is not None:
for (aff_type, min_size) in min_area:
self.afftool.setMinimumArea(aff_type, min_size)
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" +
env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
#~ 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] = [0, 0, 0.648702]
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')
q_init[0:3] = [1.1035428951774233, -1.060021251429404, 0.68]
r(q_init)
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
#~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal)
#~ q_goal[0:3] = [0.9, -1.1, 0.6]; r(q_goal)
q_goal[0:3] = [1.5, -1.1, 0.57]
r(q_goal)
#~ q_goal[0:3] = [0.36, -1, 0.9]; r(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005])
afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
afftool.loadObstacleModel(packageName, "polaris", "planning", r)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01)
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")
#~ 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] = [-0.05, -0.02, 0.5]
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.000005, 0.000005, 0.00005])
afftool.setAffordanceConfig('Lean', [0.00005, 0.000005, 0.00005])
afftool.loadObstacleModel(packageName, "grasp", "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]
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False)
#ps.setParameter("Kinodynamic/forceYawOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# 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.setMinimumArea('Support',0.03)
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planning", vf,reduceSizes=[0.11,0,0])
#afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "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()
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
#~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
q_goal[0:3] = [0.13, 0.05, 0.8]
r(q_goal)
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ 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.2, 0.03, 0.00005])
#~ afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
r.loadObstacleModel(packageName, "scene_wall", "planning")
afftool.analyseAll()
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01)
t = ps.solve()
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()
class AbstractPathPlanner:
rbprmBuilder = None
ps = None
v = None
afftool = None
pp = None
extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self):
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
self.root_translation_bounds = [
0
] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
self.root_rotation_bounds = [
-3.14, 3.14, -0.01, 0.01, -0.01, 0.01
] # bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self.extra_dof = 6 # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self.mu = 0.5 # friction coefficient between the robot and the environment
self.used_limbs = [
] # names of the limbs that must be in contact during all the motion
self.size_foot_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis
self.q_init = []
self.q_goal = []
@abstractmethod
def load_rbprm(self):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def set_configurations(self):
self.rbprmBuilder.client.robot.setDimensionExtraConfigSpace(
self.extra_dof)
self.q_init = self.rbprmBuilder.getCurrentConfig()
self.q_goal = self.rbprmBuilder.getCurrentConfig()
self.q_init[2] = self.rbprmBuilder.ref_height
self.q_goal[2] = self.rbprmBuilder.ref_height
def compute_extra_config_bounds(self):
"""
Compute extra dof bounds from the current values of v_max and a_max
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
"""
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
self.extra_dof_bounds = [
-self.v_max, self.v_max, -self.v_max, self.v_max, 0, 0,
-self.a_max, self.a_max, -self.a_max, self.a_max, 0, 0
]
def set_joints_bounds(self):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
"""
self.rbprmBuilder.setJointBounds("root_joint",
self.root_translation_bounds)
self.rbprmBuilder.boundSO3(self.root_rotation_bounds)
self.rbprmBuilder.client.robot.setExtraConfigSpaceBounds(
self.extra_dof_bounds)
def set_rom_filters(self):
"""
Define which ROM must be in collision at all time and with which kind of affordances
By default it set all the roms in used_limbs to be in contact with 'support' affordances
"""
self.rbprmBuilder.setFilter(self.used_limbs)
for limb in self.used_limbs:
self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
def init_problem(self):
"""
Load the robot, set the bounds and the ROM filters and then
Create a ProblemSolver instance and set the default parameters.
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self.load_rbprm()
self.set_configurations()
self.compute_extra_config_bounds()
self.set_joints_bounds()
self.set_rom_filters()
self.ps = ProblemSolver(self.rbprmBuilder)
# define parameters used by various methods :
if self.v_max >= 0:
self.ps.setParameter("Kinodynamic/velocityBound", self.v_max)
if self.a_max >= 0:
self.ps.setParameter("Kinodynamic/accelerationBound", self.a_max)
if self.size_foot_x > 0:
self.ps.setParameter("DynamicPlanner/sizeFootX", self.size_foot_x)
if self.size_foot_y > 0:
self.ps.setParameter("DynamicPlanner/sizeFootY", self.size_foot_y)
self.ps.setParameter("DynamicPlanner/friction", 0.5)
# sample only configuration with null velocity and acceleration :
self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
def init_viewer(self,
env_name,
env_package="hpp_environments",
reduce_sizes=[0, 0, 0],
visualize_affordances=[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" +
env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
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.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):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
if len(self.q_init) != self.rbprmBuilder.getConfigSize():
raise ValueError(
"Initial configuration vector do not have the right size")
if len(self.q_goal) != self.rbprmBuilder.getConfigSize():
raise ValueError(
"Goal configuration vector do not have the right size")
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.v(self.q_init)
t = self.ps.solve()
print("Guide planning time : ", t)
def display_path(self, path_id=-1, dt=0.1):
"""
Display the path in the viewer, if no path specified display the last one
: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 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):
"""
play the path in the viewer, if no path specified display the last one
: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)
"""
self.show_rom()
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):
"""
Remove the current robot from the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
def show_rom(self):
"""
Add the current robot to the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "ON")
@abstractmethod
def run(self):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
# example of definition:
"""
self.init_problem()
# define initial and goal position
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
pass
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax))
ps.client.problem.setParameter("aMaxZ", omniORB.any.to_any(10.))
ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax))
ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(0.))
ps.client.problem.setParameter("friction", omniORB.any.to_any(MU))
ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14))
r = Viewer(ps, displayArrows=True)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.005])
afftool.loadObstacleModel(packageName,
"darpa_no_mid",
"planning",
r,
reduceSizes=[0.14, 0])
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [1, 0, 0, 0]
q_init[0:3] = [-0.95, 0, 0.53]
r(q_init)
r = Viewer (ps)
# Setting initial configuration to show on the viewer
q_init = robot.getCurrentConfig ()
q_init [0:3] = [-2, 0, 0.63]
robot.setCurrentConfig (q_init)
r (q_init)
# Set coulour variables for different affordance types
SupportColour = [0.0, 0.95, 0.80]
LeanColour = [0.9, 0.5, 0]
# Import the affordance helper class to extract useful surface
# objects from the environment, and create an instance of affordanceTool
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
# Set the affordance configuration. The configuration vector has size 3
# and comprises the error margin, the angle margin for neighbouring triangles
# and the minimum area, in that order.
# If no configuration is set, a default configuration is used.
afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05])
afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05])
# Load obstacle models and visualise affordances. When loading an obstacle,
# the affordance analysis is done automatically.
afftool.loadObstacleModel ("hpp-affordance-corba", "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, SupportColour)
# If affordance configuration is changed, the affordance analysis must
# be relaunched.
[-1, 1, -1, 1, -0.5, 0.5, 0, 0, 0, 0, 0, 0])
indexECS = rbprmBuilder.getConfigSize(
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver and the viewer
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", aMax)
ps.client.problem.setParameter("vMax", vMax)
ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14))
ps.client.problem.setParameter("friction", mu)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r)
#afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r)
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, r.color.lightBrown)
#r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [1, 0, 0, 0]
q_init[0:3] = [0.3, 0, 0.58]
r(q_init)
q_init[8] = 0.8
#q_init[3:7] = [0.7071,0,0,0.7071]
#q_init [0:3] = [1, 1, 0.65]
])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", aMax)
ps.client.problem.setParameter("vMax", vMax)
ps.client.problem.setParameter("tryJump", 1)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.loadObstacleModel(packageName, "ground_jump_easy", "planning", r)
afftool.visualiseAffordances('Support', r, r.color.brown)
r.addLandmark(r.sceneName, 1)
q_init = rbprmBuilder.getCurrentConfig()
#q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config
#q_init [0:3] = [-3, 0, 0.79]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init[0:3] = [-3, 0, 0.7]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
q_goal[0:3] = [0.7, 0, 0.7]
r(q_goal) # pont
#q_goal = [-0.594272,0,0.724869,1,0,0,0,3.26968,0,1.85351,0,0,0]
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", mu)
ps.setParameter("Kinodynamic/forceYawOrientation", True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 500)
# 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, 5.])
afftool.loadObstacleModel("hpp_environments", "multicontact/maze_hard",
"planning", vf)
#load the viewer
try:
v = vf.createViewer(displayArrows=True)
except Exception:
print("No viewer started !")
class FakeViewer():
def __init__(self):
return
def __call__(self, q):
return
r = Viewer(ps)
# Setting initial configuration to show on the viewer
q_init = robot.getCurrentConfig()
q_init[0:3] = [-2, 0, 0.63]
robot.setCurrentConfig(q_init)
r(q_init)
# Set coulour variables for different affordance types
SupportColour = [0.0, 0.95, 0.80]
LeanColour = [0.9, 0.5, 0]
# Import the affordance helper class to extract useful surface
# objects from the environment, and create an instance of affordanceTool
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05])
afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05])
# Load obstacle models and visualise affordances. When loading an obstacle,
# the affordance analysis is done automatically.
afftool.loadObstacleModel("hpp-affordance-corba", "darpa", "planning", r)
afftool.loadObstacleModel("hpp-affordance-corba", "box", "box1", r)
afftool.visualiseAffordances('Support', r, SupportColour)
# If an object is translated or rotated, the affordance analysis must
# be relaunched.
# First, delete the already created affordance objects (this function also
# deletes them from viewer)
afftool.deleteAffordances(r, 'box1/base_link_0')
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [-5, 0, 0.63]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [-2.5, 0, 0.63]
#pass the hole
#~ q_goal [0:3] = [5, 0, 0.63]; r (q_goal) #pass the bridge
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.loadObstacleModel(packageName, "groundcrouch", "planning", r)
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
t = ps.solve()
if isinstance(t, list):
t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
from hpp.gepetto import PathPlayer
pp = PathPlayer(rbprmBuilder.client.basic, r)
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
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceYawOrientation", False)
ps.setParameter("DynamicPlanner/sizeFootX", 0.2)
ps.setParameter("DynamicPlanner/sizeFootY", 0.12)
ps.setParameter("DynamicPlanner/friction", mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
# 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/bauzil_stairs",
"planning",
vf,
reduceSizes=[0.18, 0., 0.])
v = vf.createViewer(displayArrows=True)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName, 1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [0.05, 1.2, 0.98]
q_init[-6:-3] = [0, 0, 0]
v(q_init)
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
import random
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, 'planning', [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Solve the problem
t = ps.solve ()
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
pp (0)
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.loadObstacleModel ("hpp-ompl-benchmark", "cubicles_robot", "robo", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.deleteAffordances(r,'robo/base_link_0')
ps.moveObstacle('robo/base_link_0', [0,-0.75,0, 0.5,0.5,0.5,0.5])
r.computeObjectPosition()
afftool.analyseObject('robo/base_link_0')
afftool.visualiseAffordances('Support', r, [0.75, 0.75, 0.1], 'robo/base_link_0')
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [0, 0, 0.63]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [6, 0, 0.63]
r(q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.loadObstacleModel(packageName, "plane_hole", "planning", r)
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05)
# Solve the problem
t = ps.solve()
if isinstance(t, list):
t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
# Playing the computed path
from hpp.gepetto import PathPlayer
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')
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setParameter("DynamicPlanner/sizeFootX", 0.01)
ps.setParameter("DynamicPlanner/sizeFootY", 0.01)
ps.setParameter("DynamicPlanner/friction", 0.5)
ps.setParameter("Kinodynamic/forceYawOrientation", True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
# 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/slalom_debris",
"planning",
vf,
reduceSizes=[0.05, 0, 0])
v = vf.createViewer(displayArrows=True)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [-1.5, 0, 0.63]
q_init[3:7] = [0, 0, 0, 1]
q_init[-6] = 0.05
v(q_init)
q_init [0:3] = [-1, 0.05, 0.4]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
q_goal [0:3] = [0.13, 0.05, 0.8]; r (q_goal)
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ 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.2, 0.03, 0.00005])
#~ afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
r.loadObstacleModel (packageName, "scene_wall", "planning")
afftool.analyseAll()
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
t = ps.solve ()
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()
#ps.setParameter("Kinodynamic/forceAllOrientation",True)
ps.setParameter("DynamicPlanner/sizeFootX",0.1)
ps.setParameter("DynamicPlanner/sizeFootY",0.1)
ps.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",500)
ps.setParameter("Kinodynamic/verticalAccelerationBound",aMaxZ)
# 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", "ori/slalom2", "planning", vf,reduceSizes=[0.08,0,0])
v = vf.createViewer(displayArrows = True)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
#v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-0.3,-0.4,0.47]
q_init[-6:-3] = [0,0,0]
v (q_init)
ps.setInitialConfig (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = 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] = [2.1, -0.82, 1.0]
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.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005])
#~ afftool.loadObstacleModel (packageName, "twister", "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]
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
extraDofBounds = [
-vMax, vMax, -vMax, vMax, -10., 10., -aMax, aMax, -aMax, aMax, -10., 10.
]
rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds)
indexECS = rbprmBuilder.getConfigSize(
) - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName,
"multicontact/slalom_debris",
"planning",
vf,
reduceSizes=[0., 0., 0.])
v = vf.createViewer(displayArrows=True)
afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
v.addLandmark(v.sceneName, 1)
# force the orientation of the trunk to match the direction of the motion
ps.setParameter("Kinodynamic/forceYawOrientation", True)
ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True)
ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.)
ps.setParameter("DynamicPlanner/sizeFootX", 0.2)
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax))
ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax))
ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(1.))
ps.client.problem.setParameter("friction", omniORB.any.to_any(MU))
ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14))
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.2])
afftool.loadObstacleModel(packageName,
ENV_NAME,
ENV_PREFIX,
r,
reduceSizes=[0.2, 0, 0])
#r.loadObstacleModel (packageName, "ground", "planning")
#afftool.visualiseAffordances('Support', r,r.color.lightYellow)
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [1, 0, 0, 0]
q_init[0:3] = [-0.9, 1, 0.6]
r(q_init)
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
ps.client.problem.setParameter("aMax",omniORB.any.to_any(aMax))
ps.client.problem.setParameter("vMax",omniORB.any.to_any(vMax))
ps.client.problem.setParameter("orientedPath",omniORB.any.to_any(0.))
ps.client.problem.setParameter("friction",omniORB.any.to_any(MU))
ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.14))
r = Viewer (ps,displayArrows = True)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.2])
afftool.loadObstacleModel (packageName, ENV_NAME, ENV_PREFIX, r,reduceSizes=[0.17,0,0])
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r,r.color.lightYellow)
r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [1,0,0,0]
q_init [0:3] = [0.05, 0.86, 0.61]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax))
ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax))
ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(0.))
ps.client.problem.setParameter("friction", omniORB.any.to_any(MU))
ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14))
r = Viewer(ps, displayArrows=True)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName, ENV_NAME, ENV_PREFIX, r)
#r.loadObstacleModel (packageName, "ground", "planning")
#afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [1, 0, 0, 0]
q_init[0:3] = [-0.9, 0.2, 0.58]
r(q_init)
q_init[-6] = 0.05
rbprmBuilder.setCurrentConfig(q_init)
q_goal = q_init[::]
# 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/plateforme_surfaces", "planning", vf,reduceSizes=[0.18,0,0])
v = vf.createViewer(displayArrows = True)
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [0,0,0,1]
q_init [0:3] = [0.16, 0.25, 1.14]
v (q_init)
ps.setInitialConfig (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[0] = 1.09
r = Viewer(ps)
# Setting initial configuration to show on the viewer
q_init = robot.getCurrentConfig()
q_init[0:3] = [-2, 0, 0.63]
robot.setCurrentConfig(q_init)
r(q_init)
# Set coulour variables for different affordance types
SupportColour = [0.0, 0.95, 0.80]
LeanColour = [0.9, 0.5, 0]
# Import the affordance helper class to extract useful surface
# objects from the environment, and create an instance of affordanceTool
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
# Set the affordance configuration. The configuration vector has size 3
# and comprises the error margin, the angle margin for neighbouring triangles
# and the minimum area, in that order.
# If no configuration is set, a default configuration is used.
afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05])
afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05])
# Load obstacle models and visualise affordances. When loading an obstacle,
# the affordance analysis is done automatically.
afftool.loadObstacleModel("hpp-affordance-corba", "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, SupportColour)
# If affordance configuration is changed, the affordance analysis must
# be relaunched.
[-1, 1, -1, 1, -0.5, 0.5, 0, 0, 0, 0, 0, 0])
indexECS = rbprmBuilder.getConfigSize(
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver and the viewer
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", aMax)
ps.client.problem.setParameter("vMax", vMax)
ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24))
ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14))
ps.client.problem.setParameter("friction", mu)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(ENV_PACKAGE_NAME,
ENV_NAME,
ENV_PREFIX,
r,
reduceSizes=[0.14, 0.])
#afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r)
#r.loadObstacleModel (packageName, "ground", "planning")
#afftool.visualiseAffordances('Support', r, r.color.lightBrown)
#r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [1, 0, 0, 0]
q_init[0:3] = [0.3, 0, 0.58]
# 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",mu)
# 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/plateforme_not_flat", "planning", vf, reduceSizes=[0.1,0,0])
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)
])
# We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4, 0.4, -3, 3, -3, 3])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", aMax)
ps.client.problem.setParameter("vMax", vMax)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.loadObstacleModel(packageName, "darpa", "planning", r)
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [-2, 0, 0.63]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [3, 0, 0.63]
#q_goal[0:3]=[3,-4,0.4]#position easy
q_goal[7:10] = [0, 0, 0] #velocity
r(q_goal)
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Solve the problem
t = ps.solve ()
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
# Playing the computed path
from hpp.gepetto import PathPlayer
])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.1, 0.1, -0.65, 0.65, -0.2, 0.2])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -1, 1, -2, 2, 0, 0, 0, 0, 0, 0])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
ps.client.problem.setParameter("aMax", aMax)
ps.client.problem.setParameter("vMax", vMax)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel(packageName, "downSlope", "planning", r)
#r.loadObstacleModel (packageName, "ground", "planning")
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
r.addLandmark(r.sceneName, 1)
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig()
q_init[3:7] = [0.9659, 0, 0.2588, 0]
q_init[0:3] = [-1.25, 1, 1.7]
r(q_init)
#q_init[3:7] = [0.7071,0,0,0.7071]
#q_init [0:3] = [1, 1, 0.65]
rbprmBuilder.setCurrentConfig(q_init)
q_init = rbprmBuilder.getCurrentConfig ();
#~ q_init[0:3] = [0.36, -0.32, 1.01]; r(q_init)
q_init[0:3] = [0.36, 0, 1.01]; r(q_init)
rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal)
#~ q_goal[0:3] = [0.9, -1.1, 0.6]; r(q_goal)
#~ q_goal[0:3] = [1.5, -1.1, 0.5]; r(q_goal)
q_goal[0:3] = [0.36, -1, 0.9]; r(q_goal)
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005])
afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005])
afftool.loadObstacleModel (packageName, "polaris", "planning", r)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [1, 0, 0])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.01)
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")
