def initScene(Robot, envName="multicontact/ground", genLimbsDB=True):
from hpp.gepetto import Viewer, ViewerFactory
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import ProblemSolver
fullBody = Robot()
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.setJointBounds("root_joint", [-100, 100, -100, 100, -100, 100])
fullBody.client.robot.setExtraConfigSpaceBounds(
[-100, 100, -100, 100, -100, 100, -100, 100, -100, 100, -100, 100])
fullBody.setReferenceConfig(fullBody.referenceConfig[::] + [0] * 6)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
try:
if genLimbsDB:
fullBody.loadAllLimbs("static", nbSamples=100)
else:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print(
"WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails."
)
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_environments/urdf/" + envName + ".urdf", "planning")
v = vf.createViewer(ghost=True, displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def init_problem(self):
"""
Create a ProblemSolver instance, and set the velocity and acceleration bounds
"""
self.ps = ProblemSolver(self.fullbody)
if self.path_planner.v_max >= 0:
self.ps.setParameter("Kinodynamic/velocityBound", self.path_planner.v_max)
if self.path_planner.a_max >= 0:
self.ps.setParameter("Kinodynamic/accelerationBound", self.path_planner.a_max)
def startDefaultSolver(self):
self.repeat += 1
name = self.robot.name
self.problem.selectProblem(str(self.index) + ' ' + str(self.repeat))
self.robot = HyQ(name)
self.ps = ProblemSolver(self.robot)
self.ps.setInitialConfig(self.start_config)
self.ps.addGoalConfig(self.end_config)
self.ps.selectPathPlanner("VisibilityPrmPlanner")
self.ps.addPathOptimizer("RandomShortcut")
def startNodeSolver(self, node):
self.repeat += 1
name = self.robot.name
self.problem.selectProblem(str(self.index) + ' ' + str(self.repeat))
self.robot = HyQ(name)
self.ps = ProblemSolver(self.robot)
cfg = node.getAgentCurrentConfig(self.index)
print 'this iteration, the agent', name, 'starts from ', cfg[0], cfg[1]
self.ps.setInitialConfig(cfg)
self.ps.addGoalConfig(self.end_config)
self.ps.selectPathPlanner("VisibilityPrmPlanner")
self.ps.addPathOptimizer("RandomShortcut")
def initScene(Robot, envName="multicontact/ground"):
from hpp.gepetto import Viewer, ViewerFactory
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import ProblemSolver
fullBody = Robot()
fullBody.client.robot.setDimensionExtraConfigSpace(6)
try:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print "WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails."
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_environments", envName, "planning")
v = vf.createViewer(displayCoM=True)
v(fullBody.getCurrentConfig())
return fullBody, v
def start(self):
# self.problem.selectProblem(0)
self.ps = ProblemSolver(self.agents[0].robot)
self.vf = ViewerFactory(self.ps)
if self.env != None:
self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName,
self.env.name)
self.r = self.vf.createViewer()
for a in self.agents:
a.startDefaultSolver()
a.setBounds()
a.setEnvironment()
a.solve()
a.storePath()
# self.loadAgentView(a.index)
# self.r(a.start_config)
print 'the agent ', a.robot.name, ' now has a backup plan of length', a.getProposedPlanLength(
)
def __init__(self, path_planner):
"""
Constructor, run the guide path and save the results
:param path_planner: an instance of a child class of AbstractPathPlanner
"""
path_planner.run()
self.path_planner = path_planner
self.path_planner.hide_rom()
# ID of the guide path used in problemSolver, default to the last one
self.pid = self.path_planner.ps.numberPaths() - 1
# Save the guide planning problem in a specific instance,
# such that we can use it again even after creating the "fullbody" problem:
self.cl = Client()
self.cl.problem.selectProblem("guide_planning")
path_planner.load_rbprm()
ProblemSolver(path_planner.rbprmBuilder)
self.cl.problem.selectProblem("default")
self.cl.problem.movePathToProblem(
self.pid, "guide_planning",
self.path_planner.rbprmBuilder.getAllJointNames()[1:])
# copy bounds and limbs used from path planning :
self.used_limbs = path_planner.used_limbs
self.root_translation_bounds = self.path_planner.root_translation_bounds
# increase bounds from path planning, to leave room for the root motion during the steps
for i in range(3):
self.root_translation_bounds[2 * i] -= 0.1
self.root_translation_bounds[2 * i + 1] += 0.1
# settings related to the 'interpolate' method:
self.dt = 0.01 # discretisation step used
self.robustness = 0 # robustness treshold
# (see https://github.com/humanoid-path-planner/hpp-centroidal-dynamics/blob/master/include/hpp/centroidal-dynamics/centroidal_dynamics.hh#L215)
self.filter_states = True # if True, after contact generation try to remove all the redundant steps
self.test_reachability = True # if True, check feasibility of the contact transition during contact generation
self.quasi_static = True # if True, use the 2-PAC method to check feasibility, if False use CROC
self.erase_previous_states = True # if False, keep^the previously computed states if 'interpolate' is called a second time
self.static_stability = True # if True, the acceleration computed by the guide is ignored during contact planning
self.init_contacts = self.used_limbs # limbs in contact in the initial configuration
# the order of the limbs in the list define the initial order in which the contacts are removed when then cannot be maintained anymore
self.end_contacts = self.used_limbs # limbs in contact in the final configuration
self.configs = [
] # will contains the whole body configuration computed after calling 'interpolate'
def build_fullbody(Robot, genLimbsDB=True, context = None):
"""
Build an rbprm FullBody instance
:param Robot: The class of the robot
:param genLimbsDB: if true, generate the limbs database
:param context: An optional string that give a name to a corba context instance
:return: a fullbody instance and a problemsolver containing this fullbody
"""
# Local import, as they are optional dependencies
from hpp.corbaserver import createContext, loadServerPlugin, Client, ProblemSolver
from hpp.corbaserver.rbprm import Client as RbprmClient
if context:
createContext(context)
loadServerPlugin(context, 'rbprm-corba.so')
loadServerPlugin(context, 'affordance-corba.so')
hpp_client = Client(context=context)
hpp_client.problem.selectProblem(context)
rbprm_client = RbprmClient(context=context)
else:
hpp_client = None
rbprm_client = None
fullBody = Robot(client = hpp_client, clientRbprm = rbprm_client)
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.setJointBounds("root_joint", [-100, 100, -100, 100, -100, 100])
fullBody.client.robot.setExtraConfigSpaceBounds([-100, 100, -100, 100, -100, 100, -100, 100, -100, 100, -100, 100])
fullBody.setReferenceConfig(fullBody.referenceConfig[::] + [0] * 6)
fullBody.setPostureWeights(fullBody.postureWeights[::] + [0] * 6)
try:
if genLimbsDB:
fullBody.loadAllLimbs("static", nbSamples=100)
else:
fullBody.loadAllLimbs("static", nbSamples=1)
except AttributeError:
print("WARNING initScene : fullBody do not have loadAllLimbs, some scripts may fails.")
ps = ProblemSolver(fullBody)
fullBody.setCurrentConfig(fullBody.referenceConfig[::] + [0] * 6)
return fullBody, ps
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)
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.practicals.ur5 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import ViewerFactory, PathPlayer
robot = Robot('ur5')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/obstacles", "obstacles")
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/table", "table")
vf.loadObstacleModel("hpp_practicals", "ur_benchmark/wall", "wall")
v = vf.createViewer()
q1 = [0, -1.57, 1.57, 0, 0, 0]
q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [1.57, -1.57, -1.8, 0, 0.8, 0]
v(q2)
v(q3)
ps.setInitialConfig(q2)
ps.addGoalConfig(q3)
from motion_planner import MotionPlanner
m = MotionPlanner(robot, ps)
pathId = m.solveBiRRT(maxIter=1000)
pp = PathPlayer(v)
fullBody.setJointBounds("LF_KFE", [-1.4, 0.0])
fullBody.setJointBounds("RF_KFE", [-1.4, 0.0])
fullBody.setJointBounds("LH_KFE", [0.0, 1.4])
fullBody.setJointBounds("RH_KFE", [0.0, 1.4])
fullBody.setJointBounds("root_joint", [-20, 20, -20, 20, -20, 20])
dict_heuristic = {
fullBody.rLegId: "static",
fullBody.lLegId: "static",
fullBody.rArmId: "fixedStep04",
fullBody.lArmId: "fixedStep04",
}
fullBody.loadAllLimbs(dict_heuristic, "ReferenceConfiguration", nbSamples=12)
nbSamples = 1
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
v = vf.createViewer()
rootName = "root_joint"
zero = [0.0, 0.0, 0.0]
rLegId = fullBody.rLegId
rLeg = fullBody.rleg
rfoot = fullBody.rfoot
rLegOffset = fullBody.offset[:]
lLegOffset = fullBody.offset[:]
rArmOffset = fullBody.offset[:]
lArmOffset = fullBody.offset[:]
lLegId = fullBody.lLegId
lLeg = fullBody.lleg
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
#~ rbprmBuilder.setAffordanceFilter("front_prong", ['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])
rbprmBuilder.boundSO3([-3,3,-3.,3.,-3.,-3])
# 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
psGuide = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
psGuide.setParameter("Kinodynamic/velocityBound",vMax)
psGuide.setParameter("Kinodynamic/accelerationBound",aMax)
psGuide.setParameter("DynamicPlanner/sizeFootX",0.01)
psGuide.setParameter("DynamicPlanner/sizeFootY",0.01)
psGuide.setParameter("DynamicPlanner/friction",mu)
# sample only configuration with null velocity and acceleration :
psGuide.setParameter("ConfigurationShooter/sampleExtraDOF",False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (psGuide)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
def play():
gameWidget.update()
def end():
gameWidget.end()
sys.argv = []
gui = ViewerClient()
gui.gui.createWindow("ia")
gui.gui.createWindow("player")
iaRobot = Buggy("buggy_ia")
ips = ProblemSolver(iaRobot)
ivf = ViewerFactory(ips)
ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia")
iv = ivf.createViewer(viewerClient=gui)
iaRobot.client.problem.selectProblem("player")
playerRobot = Buggy("buggy_player")
pps = ProblemSolver(playerRobot)
pvf = ViewerFactory(pps)
pvf.loadObstacleModel('hpp_environments', "scene", "scene_player")
pvf(playerRobot.client.robot.getCurrentConfig())
pv = pvf.createViewer(viewerClient=gui)
gui.gui.addSceneToWindow("buggy_player", 1)
gui.gui.addSceneToWindow("scene_player", 1)
gui.gui.addSceneToWindow("buggy_ia", 0)
gui.gui.addSceneToWindow("scene_ia", 0)
