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", 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):
"""
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 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
from hpp.corbaserver import *
from hpp.corbaserver.nassime import RobotChaine
robot = RobotChaine ('robot_chaine', True)
robot.setJointBounds ("base_joint_xyz", [-4, 12, -4, 12, -2, 4])
robot.tf_root = 'base_link'
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import Viewer
v = Viewer (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [1, 2, 1]
q_goal [0:3] = [9.15, 7.5, 0.5]
v.loadObstacleModel ("iai_maps", "env_simple", "simple")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.clearRoadmap()
ps.selectPathPlanner("interactive")
white=[1.0,1.0,1.0,1.0]
brown=[0.85,0.75,0.15,0.5]
ps.addPathOptimizer ("RandomShortcut")
v.solveAndDisplay("rm1",50,white,0.05,1,brown)
#ps.solve()
from hpp.corbaserver.pr2 import Robot
robot = Robot('pr2', False)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from gepetto.corbaserver import Client as GuiClient
guiClient = GuiClient()
from hpp.gepetto import Viewer
Viewer.sceneName = '0_scene_hpp_'
r = Viewer(ps, guiClient)
from hpp.gepetto import PathPlayer
pp = PathPlayer(robot.client, r)
rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-4, 4, -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.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)
from hpp.corbaserver.pr2 import Robot
robot = Robot("pr2")
robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])
from hpp_ros import ScenePublisher
r = ScenePublisher(robot)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
q_init = [
-3.2,
-4,
0,
0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.2,
0.0,
from hpp.corbaserver.hrp2 import Robot
from hpp.gepetto import ViewerFactory
Robot.urdfSuffix = '_reduced'
Robot.srdfSuffix = ''
# Define which problems are to be solved
walk_forward=True
walk_sideway=True
walk_oblique=True
walk_various=True
# Load HRP2 and a screwgun {{{3
robot = Robot ('hrp2')
ps = ProblemSolver (robot)
#~ ps.selectPathPlanner ("DiffusingPlanner")
# ps.addPathOptimizer ("SmallSteps")
vf = ViewerFactory (ps)
robot.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
#~ vf.loadObstacleModel ("hpp_tutorial", "box", "box")
#~ ps.moveObstacle ("box/base_link_0", [0.5,0.415,0.7,1,0,0,0])
wcl = WSClient ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (60)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
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)
from hpp.corbaserver.pr2 import Robot
from hpp.gepetto import PathPlayer
robot = Robot ('pr2')
robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.selectPathValidation ("Dichotomy", 0)
#/usr/bin/env python
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", [-3, 3, -3, 3, 0, 1])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
# Script which goes with potential_description package.
# Load simple 'robot' cylinder and obstacle to test methods.
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.simple_robot import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
import time
import sys
import matplotlib.pyplot as plt
sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
kRange = 5
robot = Robot ('simple_robot')
robot.setJointBounds('base_joint_xy', [-kRange, kRange, -kRange, kRange])
ps = ProblemSolver (robot)
cl = robot.client
Viewer.withFloor = True
r = Viewer (ps)
pp = PathPlayer (cl, r)
# q = [x, y, z, theta] # (z not considered since planar)
q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1
#q1 = [2.4, -4.6, 1.0, 0.0]; q2 = [-0.4, 4.6, 1.0, 0.0] # obstS 2
ps.setInitialConfig (q1)
ps.addGoalConfig (q2)
# Load box obstacle in HPP for collision avoidance #
#cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','')
cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','')
from hpp.corbaserver import ProblemSolver
from hpp.gepetto import Viewer
import time
white=[1.0,1.0,1.0,1.0]
green=[0.23,0.75,0.2,0.5]
yellow=[0.85,0.75,0.15,0.5]
print("chargement robot")
robot = Robot ('robot_boule', True)
robot.setJointBounds ("base_joint_xyz", [0,5,0,2,0,2])
# room : -5,4,-7,5,0.5,0.5
robot.tf_root = 'base_link'
ps = ProblemSolver (robot)
v = Viewer (ps)
q_init = robot.getCurrentConfig ()
q_init[0:3] = [0.5, 1, 1] #z=0.4 pour sphere
v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [5,1, 1]
#v (q_goal)
print("chargement map")
v.loadObstacleModel ("iai_maps", "tunnel", "tunnel")
ps.selectPathPlanner("rrtPerso")
#/usr/bin/env python
# Script which goes with animals_description package.
# Easy way to test planning algo (no internal DoF) on SO3 joint.
from hpp.corbaserver.ant_sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import numpy as np
from viewer_display_library import normalizeDir, plotVerticalCone, plotCone, plotPath, plotVerticalConeWaypoints, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-2, 2, -2, 2, -0.1, 8])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta]
q1 = [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0]
q2 = [1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("animals_description","cave","cave")
#cl.obstacle.loadObstacleModel('animals_description','cave','cave')
r(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
# verif orientation:
# Script which goes with gravity.launch, to simulate Hrp2 in the space with a spaceship from a movie and an emu character from Nasa.gov. See gravity_description package.
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
import time
import sys
sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
#robot.setJointBounds('base_joint_xyz', [-5, 10, -10, 10, -5, 5])
robot.setJointBounds('base_joint_xyz', [-3, 10, -4, 4, -3, 5])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
pp = PathPlayer (cl, r)
# Difficult init config
q3 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]
q2 = [6.55, -2.91, 1.605, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
q4 = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]
# Load obstacles in HPP
cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','') # do not move (position in urdf)
cl.obstacle.loadObstacleModel('gravity_description','emu','') # loaded as an obstacle for now
# RUNS version! launch command: "python -i environment3d_runs.py"
# Script which goes with animals_description package, runs version of Benchmark3.
# The script launches a point-robot and the cave environment.
# It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints).
# Do not forget to launch it in Release mode !
from hpp.corbaserver.sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 1.6]) # low end
#robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -6, 4.5, -2.5, 7]) # top end
ps = ProblemSolver (robot)
cl = robot.client
cl.obstacle.loadObstacleModel('animals_description','cave','')
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-0.18, 3.5, -0.11, 1, 0, 0, 0, 0, 0, 1, 0];
q22 = [0.4, -5.2, -0.7, 1, 0, 0, 0, 0, 0, 1, 0] # cave low end
#q22 = [0.27, -5.75, 5.90, 1, 0, 0, 0, 0, 0, 1, 0] # cave top end
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
vmax = 8; mu = 1.2
#vmax = 6.5; mu = 0.5
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)
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
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setTranslationBounds (-3, 3, -3, 3, 0, 1)
cl = robot.client
r = ScenePublisher (robot)
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps = ProblemSolver (robot)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
# lock hands in closed position
lockedDofs = robot.leftHandClosed ()
for name, value in lockedDofs.iteritems ():
ps.lockDof (name, value, 0, 0)
lockedDofs = robot.rightHandClosed ()
for name, value in lockedDofs.iteritems ():
ps.lockDof (name, value, 0, 0)
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)
#pp (pathId)
from hpp.corbaserver.pr2 import Robot
robot = Robot ('pr2')
robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
from hpp_ros import ScenePublisher
r = ScenePublisher (robot)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
r (q_init)
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
r (q_goal)
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
ps.setInitialConfig (q_init)
# Script which goes with potential_description package.
# Load planar 'robot' cylinder and concave obstacles.
from hpp.corbaserver.potential import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
import time
import sys
import matplotlib.pyplot as plt
sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script')
kRange = 5
robot = Robot ('potential')
robot.setJointBounds('base_joint_xy', [-kRange, kRange, -kRange, kRange])
ps = ProblemSolver (robot)
cl = robot.client
# q = [x, y, theta] # (z not considered since planar)
q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1
#q1 = [-4, 4, 0]; q2 = [4, -4, 0] # obstS 1
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','obstacles_concaves')
#ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1])
#ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.selectPathPlanner ("VisibilityPrmPlanner")
ps.selectPathValidation ("Dichotomy", 0.)
from hpp.corbaserver.ur5_robot import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import CORBA
robot = Robot ('ur5')
cl = robot.client
ps = ProblemSolver (robot)
# q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF#
q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0]
q3 = [0, -1.57, 1.57, 3.267256451, 0, 0]
q_init = q1;
# q_goal = q2
q_goal = q3
from hpp.gepetto import ViewerFactory, PathPlayerGui
vf = ViewerFactory (ps)
# vf.loadObstacleModel ("ur_description","obstacles","obstacles")
vf.loadObstacleModel ("ur_description","table","table")
# vf.loadObstacleModel ("ur_description","wall","wall")
vf(q1)
ps.lockJoint("elbow_joint", [q1[2]])
ps.selectPathValidation("Progressive", .05)
ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3))
ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True))
ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False))
#/usr/bin/env python
# Script which goes with puzzle_description package.
# RViz command : roslaunch puzzle_description puzzle.launch
# Easy way to test planning algo (no internal DoF) on SO3 joint.
from hpp.corbaserver.puzzle import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
robot = Robot ('puzzle') # object5
robot.setJointBounds('base_joint_xyz', [-0.9, 0.9, -0.9, 0.9, -1., 1.])
#robot.setJointBounds('base_joint_xyz', [-0.6, 0.6, -0.6, 0.6, -0.3, 1.0])
ps = ProblemSolver (robot)
cl = robot.client
q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#q1 = [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] # simpler
#q1 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]; q2 = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0] # simpler2
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
r.loadObstacleModel ("puzzle_description","decor_easy","decor_easy")
r(q2) # r(q1)
#q1bis = q2; q2bis = [0.0, 0.0, -0.8, 1.0, 0.0, 0.0, 0.0]
#ps.resetGoalConfigs (); ps.setInitialConfig (q1bis); ps.addGoalConfig (q2bis); ps.solve ()
# ps.resetGoalConfigs (); ps.setInitialConfig (q1); ps.addGoalConfig (q2bis); ps.solve ()
#i = ps.numberPaths()-1
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
robot.setJointBounds ("base_joint_xyz", (-3, 3, -3, 3, 0, 1))
cl = robot.client
q0 = robot.getInitialConfig ()
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle,
robot.rightAnkle, "",
Problem.SLIDING)
ps = ProblemSolver (robot)
ps.setNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot"])
# lock hands in closed position
lockedjoints = robot.leftHandClosed ()
for name, value in lockedjoints.iteritems ():
ps.lockJoint (name, value)
lockedjoints = robot.rightHandClosed ()
for name, value in lockedjoints.iteritems ():
ps.lockJoint (name, value)
from hpp.corbaserver.pr2 import Robot
robot = Robot ('pr2')
robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
# On travis, we do not compile the viewer (yet?)
# from hpp.gepetto import ViewerFactory
# r = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
# r (q_init)
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
# r (q_goal)
# r.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "kitchen/")
from hpp.corbaserver.pr2 import Robot
robot = Robot('pr2')
robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['torso_lift_joint']
q_init[rank] = 0.2
vf(q_init)
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['l_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['l_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['r_elbow_flex_joint']
q_goal[rank] = -0.5
vf(q_goal)
vf.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig(q_init)
#/usr/bin/env python
# Benchmark3. Script which goes with animals_description package.
# The script launches a point-robot and the cave_mono_short environment.
# It defines init and final configs, and solve them for 2 couples of mu / vmax (parabola constraints).
from hpp.corbaserver.sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-2.6, 2.6, -3, 4.2, -2.5, 4])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-0.3, 3.65, -0.25, 1, 0, 0, 0, 0, 0, 1, 0];
q22 = [-0.18, -2.2, -0.4, 1, 0, 0, 0, 0, 0, 1, 0]
#cl.obstacle.loadObstacleModel('animals_description','cave','')
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","cave","cave")
#addLight (r, [-0.3, 3.8, 0,1,0,0,0], "li"); addLight (r, [-0.18, -3, 0.1,1,0,0,0], "li1"); addLight (r, [-0.3, 4, 0,1,0,0,0], "li3")
r(q11)
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
# Benchmark2mesh. Script which goes with animals_description package.
# The script launches a point-robot and the environment containing an environment 3d mesh (with windows).
# It defines init and final configs, and solve them for 2 couples of mu / vmax (parabola constraints).
#blender/urdf_to_blender.py -p /local/mcampana/devel/hpp/videos/ -i robot.urdf -o robot_blend.py
from hpp.corbaserver.sphere import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap, plotEdgesRoadmap
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-5, 5, -5, 5, -1, 7])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [-3.8, 2.4, 1.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [3.5, -3.3, 0.4, 0, 0, 0, 1, 0, 0, 1, 0]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps); gui = r.client.gui
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","envir3d_window_mesh","envir3d_window_mesh")
#addLight (r, [-3,3,4,1,0,0,0], "li"); addLight (r, [3,-3,4,1,0,0,0], "li1")
r(q11)
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(q1)
joint1L_bounds_prev = fullBody.getJointBounds('leg_left_1_joint')
joint3L_bounds_prev = fullBody.getJointBounds('leg_left_3_joint')
joint1R_bounds_prev = fullBody.getJointBounds('leg_right_1_joint')
joint3R_bounds_prev = fullBody.getJointBounds('leg_right_3_joint')
fullBody.setJointBounds('leg_left_1_joint', [-0.2, 0.7])
fullBody.setJointBounds('leg_left_3_joint', [-1.3, 0.4])
fullBody.setJointBounds('leg_right_1_joint', [-0.7, 0.2])
fullBody.setJointBounds('leg_right_3_joint', [-1.3, 0.4])
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(6)
vMax = 0.5 # linear velocity bound for the root
aMax = 0.5 # linear acceleration bound for the root
fullBody.client.robot.setExtraConfigSpaceBounds(
[-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0])
ps = ProblemSolver(fullBody)
vf = ViewerFactory(ps)
ps.setParameter("Kinodynamic/velocityBound", vMax)
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setRandomSeed(random.SystemRandom().randint(0, 999999))
#load the viewer
try:
v = vf.createViewer(displayCoM=True)
except Exception:
print("No viewer started !")
class FakeViewer():
sceneName = ""
def __init__(self):
args = parser.parse_args()
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix = '_capsule'
#Robot.urdfSuffix = ''
#Robot.srdfSuffix= ''
robot = Robot('hrp2_14')
robot.setJointBounds("root_joint",
[-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1])
cl = robot.client
q0 = robot.getInitialConfig()
# Add constraints
ps = ProblemSolver(robot)
ps.addPartialCom('hrp2_14', ['root_joint'])
robot.createSlidingStabilityConstraint("balance/", 'hrp2_14', robot.leftAnkle,
robot.rightAnkle, q0)
ps.setNumericalConstraints("balance", [
"balance/relative-com",
"balance/relative-pose",
"balance/pose-left-foot",
])
# lock hands in closed position
lockedjointDict = robot.leftHandClosed()
lockedJoints = list()
for name, value in lockedjointDict.iteritems():
ljName = "locked_" + name
ps.createLockedJoint(ljName, name, value)
# 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(Robot.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom :
rbprmBuilder.setAffordanceFilter(rom, ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.01,0.01,-0.01,0.01,-0.01,0.01])
# 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,-2.,2.,-aMax,aMax,-aMax,aMax,-aMaxZ,aMaxZ])
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("Kinodynamic/verticalAccelerationBound",aMaxZ)
#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
from hpp.corbaserver.pr2 import Robot
robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
vf (q_init)
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf (q_goal)
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig (q_init)
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
#/usr/bin/env python
# Script which goes with gravity.launch, to simulate Hrp2 in the space with a spaceship from a movie and an emu character from Nasa.gov. See gravity_description package.
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.emu import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
robot = Robot ('emu')
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
pp = PathPlayer (cl, r)
#r.loadObstacleModel ("gravity_description","gravity_decor","gravity_decor") # visual
#r.loadObstacleModel ("gravity_description","stone","stone")
xStone = 10
yEmu = 5+0.2
zEmu = 6-1
robot.setJointBounds('base_joint_xyz', [xStone-2, xStone+2, yEmu-1, yEmu+2, zEmu-0.5, zEmu+0.5])
# List of configs
q1 = [xStone+2, yEmu+0, zEmu, 1.0, 0.0, 0.0, 0.0]
q2 = [xStone+1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, 0.707106781]
q3 = [xStone+1, yEmu+1.4, zEmu, 0, 0, 0, 1]
q4 = [xStone+0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, 0.707106781]
#q5 = [xStone+0, yEmu+2, zEmu, -1, 0, 0, 0]
q5 = [xStone+0, yEmu+2, zEmu, -0.99, 0.14003571, 0.013, 0.011]
q6 = [xStone-0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, -0.707106781]
q7 = [xStone-1, yEmu+1.4, zEmu, 0, 0, 0, -1]
q8 = [xStone-1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, -0.707106781]
q9 = [xStone-2, yEmu+0, zEmu, 1, 0, 0, 0]
from hpp import Quaternion
robot = Robot ('romeo')
robot.setJointBounds ("base_joint_x", [-4, 3])
robot.setJointBounds ("base_joint_y", [-5, 3])
robot.setJointBounds ("base_joint_z", [0, 2])
r = ScenePublisher (robot)
# Get half-sitting configuration
q = robot.getInitialConfig ()
q[0] = -3
q[1] = -4
r (q)
ps = ProblemSolver (robot)
# create static stability constraint
wcl = WholebodyStepClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q, robot.leftAnkle,
robot.rightAnkle)
# Define quatenion for righthand orientation constraints
#quat = Quaternion()
#quat.fromRPY(-1.55, -1.55, 0.0) # Right hand horizontal, grasping to -z
#quat.fromRPY(-1.55, 0.0, 0.0) # Right hand vertical, pointing down
#quat.fromRPY(0.0, 0.0, -1.55) # Right hand vertical, pointing forward
#quat.normalize()
ps.resetConstraints()
from hpp.gepetto import Viewer
import time
white = [1.0, 1.0, 1.0, 1.0]
green = [0.23, 0.75, 0.2, 0.5]
brown = [0.85, 0.75, 0.15, 0.5]
print("chargement robot")
robot = Robot('robot_boule', True)
robot.setJointBounds("base_joint_xyz", [0, 9, 0, 12, 0.5, 0.5])
#robot.setJointBounds("base_joint_SO3",[1,1,0,0,0,0,0,0])
# room : -5,4,-7,5,0.5,0.5
robot.tf_root = 'base_link'
ps = ProblemSolver(robot)
v = Viewer(ps)
q_init = robot.getCurrentConfig()
q_init[0:3] = [9, 7.5, 0.5]
v(q_init)
q_goal = q_init[::]
q_goal[0:3] = [0, 1.5, 0.5]
#v (q_goal)
print("chargement map")
v.loadObstacleModel("iai_maps", "labyrinth2", "labyrinth2")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.selectPathPlanner("rrtConnect")
#/usr/bin/env python
import time
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.pr2_with_set import Robot
robot = Robot ('pr2_with_set') # will containt the set fixed on the left hand... (moving with l_wrist_flex_joint)
robot.setJointBounds ("base_joint_xy", [-4, -0.5, -7, -1.5])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
Viewer.withFloor = True
r = Viewer (ps)
r.loadObstacleModel ("iai_maps","kitchen_area","kitchen_area") # visual kitchen
r.loadObstacleModel ("iai_maps","floor","floor")
q1 = robot.getCurrentConfig ()
q1 [0:2] = [-3.4, -6]
q1 [robot.rankInConfiguration ['torso_lift_joint']] = 0.13
q1 [robot.rankInConfiguration ['l_shoulder_pan_joint']] = 0.5 # ecarte les bras
q1 [robot.rankInConfiguration ['r_shoulder_pan_joint']] = -0.5
q1 [robot.rankInConfiguration ['l_upper_arm_roll_joint']] = 1.57 # tourne bras sur lui-mm
q1 [robot.rankInConfiguration ['r_upper_arm_roll_joint']] = -1.57
q1 [robot.rankInConfiguration ['l_elbow_flex_joint']] = -0.49 # plier coude
q1 [robot.rankInConfiguration ['r_elbow_flex_joint']] = -0.50
q1 [robot.rankInConfiguration ['l_wrist_flex_joint']] = -0.2 # plie poignet
q1 [robot.rankInConfiguration ['r_wrist_flex_joint']] = -0.2
q1 [robot.rankInConfiguration ['l_gripper_r_finger_joint']] = 0.09 # ouvrir pince
q1 [robot.rankInConfiguration ['l_gripper_l_finger_joint']] = 0.12
q1 [robot.rankInConfiguration ['r_gripper_r_finger_joint']] = 0.13
q1 [robot.rankInConfiguration ['r_gripper_l_finger_joint']] = 0.095
r(q1)
from hpp.corbaserver.pr2 import Robot
robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
vf (q_init)
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf (q_goal)
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
ps.setInitialConfig (q_init)
from hpp.corbaserver.pr2 import Robot
from hpp.gepetto import PathPlayer
from math import pi
robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3,-2,2,-2,2])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
#/usr/bin/env python
# Script which goes with puzzle_description package.
# RViz command : roslaunch puzzle_description puzzle.launch
# Easy way to test planning algo (no internal DoF) on SO3 joint.
# b hpp::core::pathOptimization::GradientBased::initialize (const PathVectorPtr_t& path)
from hpp.corbaserver.puzzle import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
import math
robot = Robot ('puzzle')
ps = ProblemSolver (robot)
cl = robot.client
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
#r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy")
robot.setJointBounds('base_joint_xyz', [-2.2, +2.2, -0.2, 2.2, -0.5, 0.5])
Q = []
Q.append ( [0, 0, 0, 1.0, 0.0, 0.0, 0.0])
"""
Q.append ( [0.0, 0.0, 0.0, 0.8573289792052967, 0.5110043077178016, 0.0474382998474562, 0.04014009987092448])
Q.append ( [0.0, 0.0, 0.0, 0.47002595717039686, 0.8761976030104256, 0.08134045836690892, 0.06882654169507678])
Q.append ( [0.0, 0.0, 0.0, -0.05139523108351973, 0.9913748854243123, 0.09203276443212992, 0.07787387759641762]) # !
Q.append ( [0.0, 0.0, 0.0, -0.5581511991721069, 0.8236712340507641, 0.07646425360117025, 0.06470052227791329])
Q.append ( [0.0, 0.0, 0.0, -0.9056431645733478, 0.420939551154707, 0.03907727653904272, 0.033065387840728454]) # ! one turn
from hpp.corbaserver.sphere import Robot
#from hpp.corbaserver.ant import Robot
#from hpp.corbaserver.ant_sphere import Robot
#from hpp.corbaserver.monopod_mesh import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints
from parseLog import parseNodes, parseIntersectionConePlane, parseAlphaAngles
from parabola_plot_tools import parabPlotDoubleProjCones, parabPlotOriginCones
import math
import numpy as np
robot = Robot ('robot')
#robot.setJointBounds('base_joint_xyz', [-3.9, 3.9, -3.9, 3.9, 1, 12]) # plane
robot.setJointBounds('base_joint_xyz', [-6, 6, -6, 6, 2, 9]) # environment_3d
ps = ProblemSolver (robot)
cl = robot.client
#cl.obstacle.loadObstacleModel('animals_description','inclined_plane_3d','inclined_plane_3d')
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
#q1 = [-1.5, -1.5, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]; q2 = [2.6, 3.7, 3.41, 0, 0, 0, 1, 0, 0, 1, 0]
#q11 = [2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta = Pi
#q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, 0, 0, 1, 0] # plane with theta ~= 0
#q11 = [-2.5, 3, 4, 0, 0, 0, 1,-0.1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 8, 0, 0, 0, 1, -0.1, 0, 0, 1, 0] # plane with theta ~= 0 MONOPOD
#q11 = [-2.5, 3, 4, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [2.5, 2.7, 9, 0, 0, 0, 1, 0, 0, 1, 0] # multiple-planes (3 planes)
q11 = [-5, 3.1, 4.2, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [5.2, -5.2, 4, 0, 0, 0, 1, 0, 0, 1, 0] # environment_3d
#r(q22)
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps)
pp = PathPlayer (robot.client, r)
from hpp.corbaserver.pr2 import Robot
robot = Robot('pr2')
robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
from hpp.gepetto import ViewerFactory
r = ViewerFactory(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['torso_lift_joint']
q_init[rank] = 0.2
r(q_init)
q_goal[0:2] = [-3.2, -4]
rank = robot.rankInConfiguration['l_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['l_elbow_flex_joint']
q_goal[rank] = -0.5
rank = robot.rankInConfiguration['r_shoulder_lift_joint']
q_goal[rank] = 0.5
rank = robot.rankInConfiguration['r_elbow_flex_joint']
q_goal[rank] = -0.5
r(q_goal)
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([-4., 4., -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, -10., 10., -aMax, aMax, -aMax, aMax, -10., 10.])
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)
# 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)
rbprmBuilder.setFilter(rbprmBuilder.urdfNameRom)
for rom in rbprmBuilder.urdfNameRom:
rbprmBuilder.setAffordanceFilter(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.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)
# 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
from hpp.corbaserver.pr2 import Robot
from hpp.gepetto import PathPlayer
from math import pi
robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3,-2,2,-2,2])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
# Benchmark4. Script which goes with animals_description package.
# The script launches a point-robot and the scene_jump_harder (mesh version 10) environment.
# It defines init and final configs, and solve them for .. couples of mu / vmax (parabola constraints).
from hpp.corbaserver.sphere import Robot
#from hpp.corbaserver.sphere_mesh import Robot
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
from viewer_display_library import normalizeDir, plotCone, plotFrame, plotThetaPlane, shootNormPlot, plotStraightLine, plotConeWaypoints, plotSampleSubPath, contactPosition, addLight, plotSphere, plotSpheresWaypoints, plotConesRoadmap
import math
import numpy as np
robot = Robot ('robot')
robot.setJointBounds('base_joint_xyz', [-6, 6.8, -2.5, 3.2, 0, 8])
ps = ProblemSolver (robot)
cl = robot.client
# Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta]
q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0]
from hpp.gepetto import Viewer, PathPlayer
r = Viewer (ps); gui = r.client.gui
pp = PathPlayer (robot.client, r)
r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder")
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
#/usr/bin/env python
# Script which goes with gravity.launch, to simulate Hrp2 in the space with a spaceship from a movie and an emu character from Nasa.gov. See gravity_description package.
from hpp.gepetto import Viewer, PathPlayer
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import Client
#Robot.urdfSuffix = '_capsule'
#Robot.srdfSuffix= '_capsule'
robot = Robot ('hrp2_14')
#robot.setJointBounds('base_joint_xyz', [-5, 10, -10, 10, -5, 5])
robot.setJointBounds('base_joint_xyz', [-3, 10, -4, 4, -3, 5])
ps = ProblemSolver (robot)
cl = robot.client
r = Viewer (ps)
pp = PathPlayer (cl, r)
#r.loadObstacleModel ("gravity_description","emu","emu")
r.loadObstacleModel ("gravity_description","gravity_decor","gravity_decor")
# Difficult init config
q1 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]
q2 = [6.55, -2.91, 1.605, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0]
robot.isConfigValid(q1)
robot.isConfigValid(q2)
# qf should be invalid
Robot.srdfSuffix = '_capsule'
robot = Robot('hrp2_14')
robot.setTranslationBounds(-3, 3, -3, 3, 0, 1)
cl = robot.client
r = ScenePublisher(robot)
q0 = robot.getInitialConfig()
r(q0)
# Add constraints
wcl = WsClient()
wcl.problem.addStaticStabilityConstraints("balance", q0, robot.leftAnkle,
robot.rightAnkle)
ps = ProblemSolver(robot)
ps.setNumericalConstraints("balance", [
"balance/relative-com", "balance/relative-orientation",
"balance/relative-position", "balance/orientation-left-foot",
"balance/position-left-foot"
])
# lock hands in closed position
lockedDofs = robot.leftHandClosed()
for name, value in lockedDofs.iteritems():
ps.lockDof(name, value, 0, 0)
lockedDofs = robot.rightHandClosed()
for name, value in lockedDofs.iteritems():
ps.lockDof(name, value, 0, 0)
from hpp.corbaserver.pr2 import Robot
robot = Robot('pr2')
robot.setJointBounds("base_joint_x", [-4, -3])
robot.setJointBounds("base_joint_y", [-5, -3])
from hpp_ros import ScenePublisher
r = ScenePublisher(robot)
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver(robot)
q_init = [
-3.2, -4, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2,
0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0
]
r(q_init)
q_goal = [
-3.2, -4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0, 0.0, 0.0, 0.0, 0.5, 0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.5, 0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0
]
r(q_goal)
ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "")
ps.selectPathOptimizer("None")
ps.selectPathValidation("Dichotomy", 0)
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
#original from antonio
q1=[0,1.5,0.64870180180254433111, 0.9249114088877176,0,0,-0.38018270043406405,0,0,0,0,0.26179900000000000393,0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393,-0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268,0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268,0.87266500000000002402,-0.41887900000000000134,0]
#q1=[0,1.5,0.64870180180254433111, 1,0,0,0 ,0,0,0,0,0.26179900000000000393,0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393,-0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268,0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268,0.87266500000000002402,-0.41887900000000000134,0]
#original from antonio
q2=[0,-2.5,0.64870180180254433111, 0.7101853756232854, 0, 0, -0.7040147244559684, 0,0,0,0,0.26179900000000000393, 0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393, -0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0]
#q2=[0,-2.5,0.64870180180254433111, 1,0,0,0, 0,0,0,0,0.26179900000000000393, 0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393, -0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0]
publisher = ScenePublisher(robot)
publisher(q2)
time.sleep(1)
publisher(q1)
## Add constraints
solver = ProblemSolver (robot)
print "diffusing planner (general RRT implementation)"
solver.selectPathPlanner("DiffusingPlanner")
solver.loadObstacleFromUrdf("hpp_ros","wall")
solver.setInitialConfig (q1)
solver.addGoalConfig (q2)
#pc.invokeIrreduciblePlanner()
#mpc = MPClient()
#pc = mpc.precomputation
#cnames = pc.getNumericalConstraints (robot.getInitialConfig())
#solver.setNumericalConstraints ("stability-constraints", cnames)
## add obstacles
print "solve"
start_time = float(time.time())
