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 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 loadAgentView(self, index, default=False): #default position or not
self.ps = self.agents[index - 1].ps
self.vf = ViewerFactory(self.ps)
# self.vf.loadObstacleModel(self.env.packageName, self.env.urdfName, self.env.name)
self.r = self.vf.createViewer()
# print '---------------->', len(self.agents[index - 1].init_config)
if default:
self.r(self.agents[index - 1].current_config)
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 createViewer(self,
ViewerClass=Viewer,
viewerClient=None,
host=None,
collisionURDF=False):
if host is not None and viewerClient is None:
from gepetto.corbaserver import Client as GuiClient
viewerClient = GuiClient(host=host)
v = Parent.createViewer(self, ViewerClass, viewerClient, host,
collisionURDF)
return v
def initScene(Robot,
envName="multicontact/ground",
genLimbsDB=True,
context=None):
"""
Create and initialize a hpp.gepetto.Viewer object with a hpp.corbaserver.rbprm.FullBody object
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param envName: the name of the environment file to load. Load a flat floor at z = 0 by default
Currently only work with environments from the hpp-environment package
:param genLimbsDB: If True (default value) generate the limbs database of the fullbody object
:param context: An optional string that give a name to a corba context instance
:return: a Fullbody object and a Viewer object
"""
from hpp.gepetto import ViewerFactory
fullBody, ps = build_fullbody(Robot, genLimbsDB, context)
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_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)
def loadRobot(self):
hppClient = self._hpp()
# Load the robot
robotName = rospy.get_param("/robot_name", "robot")
rootJointType = rospy.get_param("/robot_root_joint_type", "anchor")
urdfString = rospy.get_param("/robot_description")
srdfString = rospy.get_param("/robot_description_semantic", "")
hppClient.robot.loadRobotModelFromString(robotName, rootJointType,
urdfString, srdfString)
self.robot = hpp.corbaserver.robot.Robot(robotName,
rootJointType,
client=hppClient,
load=False)
self.ps = hpp.corbaserver.ProblemSolver(self.robot)
self.vf = ViewerFactory(self.ps)
self.vf.loadRobot(urdfString)
# Set the root joint position in the world.
base = self.robot.getLinkNames("root_joint")[0]
p, q = self.tfListener.lookupTransform(self.world_frame, base,
rospy.Time(0))
self.robot.setJointPosition("root_joint", p + q)
from hpp.environments import Buggy
from hpp.gepetto import PathPlayer
robot = Buggy("buggy")
robot.setJointBounds ("root_joint", [-5, 16, -4.5, 4.5,
-1.01, 1.01, -1.01, 1.01])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
gui = ViewerFactory (ps)
gui.loadObstacleModel ('hpp_environments', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init[0:2] = [-3.7, -4];
gui (q_init)
q_goal [0:2] = [15,2]
gui (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectSteeringMethod ("ReedsShepp")
ps.selectPathPlanner ("DiffusingPlanner")
ps.addPathOptimizer ("RandomShortcut")
t = ps.solve ()
print ("solving time", t)
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)
robot.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
# define the velocity and acceleration bounds used by the steering method. This bounds will be stastified along the whole trajectory.
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100)
# Uncomment the following line if you want to constraint the orientation of the base of the robot to follow the direction of the motion. Note that in this case the initial and final orientation are not considered.
#ps.setParameter("Kinodynamic/forceOrientation",True)
# The following line constraint the random sampling method to fix all the extraDOF at 0 during sampling. Comment it if you want to sample states with non-null velocity and acceleration. Note that it increase the complexity of the problem and greatly increase the computation time.
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:3] = [6.5,-4,0.4] #root position
q_init[3:7] = [0,0,0,1] #root rotation
#set initial velocity (along x,y,z axis) :
q_init[-6:-3]=[0,0,0]
#q_goal[0:3] = [6.5,-1,0.4] # straight line
q_goal [0:3] = [3,-4,0.4] # easy goal position
#q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
#set goal velocity (along x,y,z axis) :
q_goal[-6:-3]=[0,0,0]
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
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("package://hpp_tutorial/urdf/kitchen_area.urdf",
"kitchen")
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
lfoot = fullBody.lfoot
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)
def createRealClient(self, ViewerClass=Viewer, viewerClient=None):
v = Parent.createRealClient(self, ViewerClass, viewerClient)
return v
def __init__ (self, problemSolver) :
Parent.__init__ (self, problemSolver)
raise RuntimeError ("Failed to apply constraint.")
ps.selectPathValidation ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.client.problem.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q1proj)
ps.addGoalConfig (q2proj)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
r = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer (robot.client, r)
import datetime as dt
totalTime = dt.timedelta(0)
totalNumberNodes = 0
for i in range(args.N):
ps.client.problem.clearRoadmap()
ps.resetGoalConfigs()
ps.setInitialConfig(q1proj)
ps.addGoalConfig(q2proj)
t1 = dt.datetime.now()
ps.solve()
t2 = dt.datetime.now()
totalTime += t2 - t1
print(t2 - t1)
n = len(ps.client.problem.nodes())
totalNumberNodes += n
print("Number nodes: " + str(n))
print("Average time: " +
str((totalTime.seconds + 1e-6 * totalTime.microseconds) / float(args.N)))
print("Average number nodes: " + str(totalNumberNodes / float(args.N)))
if args.display:
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
#v = vf.createViewer()
from hpp.gepetto import PathPlayer
pp = PathPlayer(v, robot.client)
if args.run:
pp(0)
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))
print "Optimizer parameters are:"
print "alphaInit:", ps.getParameter("SplineGradientBased/alphaInit").value()
from hpp.environments import Buggy
robot = Buggy("buggy")
robot.setJointBounds ("base_joint_xy", [-5, 16, -4.5, 4.5])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
gui = ViewerFactory (ps)
gui.loadObstacleModel ('hpp_environments', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init[0:2] = [-3.7, -4];
gui (q_init)
q_goal [0:2] = [15,2]
gui (q_goal)
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.selectSteeringMethod ("ReedsShepp")
ps.selectPathPlanner ("DiffusingPlanner")
ps.addPathOptimizer ("RandomShortcut")
t = ps.solve ()
print ("solving time", t)
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)
class HppServerInitializer(HppClient):
subscribersDict = {
"hpp": {
"reset": [Bool, "reset"],
"create_viewer": [Empty, "_createViewer"],
"configure": [Empty, "configure"],
"load_environment": [String, "loadEnvironment"],
},
}
def __init__(self):
super(HppServerInitializer, self).__init__()
self.subscribers = self._createTopics("", self.subscribersDict, True)
self.setHppUrl()
self.robot = None
self.ps = None
self.tfListener = TransformListener()
self.world_frame = "/world"
def reset(self, msg):
self.initialize()
self.loadRobot()
if msg.data:
self.loadEnvironment(param="/environment/description",
paramName="/environment/name")
self.configure()
try:
self.createViewer()
except:
rospy.loginfo("Could not reach the Gepetto-viewer")
def initialize(self):
self.setHppUrl()
hpp = self._hpp()
# Reset the problem if necessary
if self.robot is not None:
hpp.problem.resetProblem()
self.robot = None
self.ps = None
self.vf = None
def loadRobot(self):
hppClient = self._hpp()
# Load the robot
robotName = rospy.get_param("/robot_name", "robot")
rootJointType = rospy.get_param("/robot_root_joint_type", "anchor")
urdfString = rospy.get_param("/robot_description")
srdfString = rospy.get_param("/robot_description_semantic", "")
hppClient.robot.loadRobotModelFromString(robotName, rootJointType,
urdfString, srdfString)
self.robot = hpp.corbaserver.robot.Robot(robotName,
rootJointType,
client=hppClient,
load=False)
self.ps = hpp.corbaserver.ProblemSolver(self.robot)
self.vf = ViewerFactory(self.ps)
self.vf.loadRobot(urdfString)
# Set the root joint position in the world.
base = self.robot.getLinkNames("root_joint")[0]
p, q = self.tfListener.lookupTransform(self.world_frame, base,
rospy.Time(0))
self.robot.setJointPosition("root_joint", p + q)
def loadEnvironment(self,
param=None,
xmlString=None,
paramName=None,
name=None):
if name is None:
name = rospy.get_param(
paramName) if paramName is not None else "obstacle"
if param is not None:
self.vf.loadObstacleModel("", rospy.get_param(param), name)
elif xmlString is not None:
self.vf.loadObstacleModel("", xmlString, name)
def configure(self):
hpp = self._hpp()
# Setup the planner
setParameters(hpp, "", rospy.get_param("/hpp/parameters", dict()))
if rospy.has_param("/hpp/path_validation/method"):
ps.selectPathValidation(
rospy.get_param("/hpp/path_validation/method"),
rospy.get_param("/hpp/path_validation/tolerance", 0.05))
self.ps.clearPathOptimizers()
if rospy.has_param("/hpp/path_optimizers"):
pathOpts = rospy.get_param("/hpp/path_optimizers",
["RandomShortcut"])
if isinstance(pathOpts, list):
for n in pathOpts:
self.ps.addPathOptimizer(n)
else:
rospy.logerr(
"Parameter /hpp/path_optimizers shoud be a list of strings."
)
def _createViewer(self, msg):
self.createViewer()
def createViewer(self, *args, **kwargs):
host = rospy.get_param("/gepetto_viewer/host", "localhost")
try:
return self.vf.createViewer(host=host, *args, **kwargs)
except:
rospy.loginfo("Could not reach the Gepetto-viewer")
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):
return
def createViewer (self, ViewerClass = Viewer, **kwargs):
return Parent.createViewer (self, ViewerClass, **kwargs)
def __init__(self, problemSolver):
Parent.__init__(self, problemSolver)
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
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("package://iai_maps/urdf/kitchen_area.urdf", "kitchen")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
from hpp.corbaserver import *
from hpp.corbaserver.nassime import RobotL
#from hpp.corbaserver.nassime import RobotCursor
robot = RobotL('robot_L', True)
robot.setJointBounds("base_joint_xyz", [-2.5, 12, -7, 7, -7, 7])
robot.tf_root = 'base_link'
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:3] = [0, 0, 0]
vf(q_init)
q_goal[0:7] = [10, 6, 0.11, 0.707460067, 0.706753314, 0, 0]
q_goal[0:7] = [9.7, 6, 0.11, 0.707460067, 0.706753314, 0, 0]
vf(q_goal)
vf.loadObstacleModel("iai_maps", "env_complexe", "simple")
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.clearRoadmap()
ps.selectPathPlanner("interactive")
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)
from hpp.corbaserver.practicals.ur5 import Robot
from hpp.corbaserver import ProblemSolver, Client
from hpp.gepetto import ViewerFactory, PathPlayer
Client().problem.resetProblem()
import readline
import rlcompleter
readline.parse_and_bind("tab: complete")
robot = Robot('ur5')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObstacleModel(
"package://hpp_practicals/urdf/ur_benchmark/obstacles.urdf", "obstacles")
vf.loadObstacleModel("package://hpp_practicals/urdf/ur_benchmark/table.urdf",
"table")
vf.loadObstacleModel("package://hpp_practicals/urdf/ur_benchmark/wall.urdf",
"wall")
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]
ps.setInitialConfig(q2)
ps.addGoalConfig(q3)
from motion_planner import MotionPlanner
m = MotionPlanner(robot, ps)
pathId = m.solveBiRRT(maxIter=1000)
from talos_rbprm.talos import Robot
from hpp.gepetto import Viewer
import time
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
import time
fullBody = Robot()
fullBody.setJointBounds("root_joint", [-5, 5, -1.5, 1.5, 0.95, 1.05])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0] * 12)
ps = ProblemSolver(fullBody)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
vf.loadObstacleModel("hpp_environments", "multicontact/table_140_70_73",
"planning")
q_ref = [
0.0,
0.0,
1.0232773,
0.0,
0.0,
0.0,
1, #Free flyer
0.0,
0.0,
-0.411354,
0.859395,
-0.448041,
# hpp-tom If not, see
# <http://www.gnu.org/licenses/>.
from math import sqrt
from hpp.corbaserver.tom import Robot
from hpp.gepetto import PathPlayer
#Robot.urdfName = "tom_full"
robot = Robot ('tom')
robot.setJointBounds ('base_joint_xy', [-1,1,-1,1])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
#r.loadObstacleModel ("tom_description", "table", "table")
vf.loadObstacleModel ("hpp_tutorial", "box", "box")
box_position = [.5,0,.5,1,0,0,0]
table_position = (-0.90, -0.91, -0.42, .5, .5, .5, .5)
#r.moveObstacle ("table/table_tom_link_0", table_position)
#r.moveObstacle ("tom/box_tom_link_0", table_position)
vf.moveObstacle ("box/base_link_0", box_position)
q_init = robot.halfSitting [::]
dict_goal = {
'l_shoulder_pan_joint': -1.62236446982,
'l_shoulder_lift_joint': -0.741943917854,
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)
self.button.connect("clicked()", launch)
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)
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)
from hpp.quadcopter import Robot
robot = Robot("quad")
robot.client.robot.setDimensionExtraConfigSpace(9)
# robot.client.robot.setDimensionExtraConfigSpace(6)
robot.setJointBounds ("base_joint_xyz", [-5, 16, -4.5, 4.5, 0, 0.6])
# robot.client.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-1,1,-1,1,-1,1,-1,1])
robot.client.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1,-1,1])
from hpp.corbaserver import ProblemSolver
ps = ProblemSolver (robot)
from hpp.gepetto import ViewerFactory, PathPlayer
vf = ViewerFactory (ps)
vf.loadObstacleModel ('hpp-quadcopter', "scene", "scene")
q_init = robot.getCurrentConfig ()
q_init[2] = 0.5
q_goal = q_init [::]
q_init [0:2] = [-3.7, -4];
vf (q_init)
q_goal [0:2] = [15,2]
vf (q_goal)
q = q_init[:]
q[0:2] = [2,0]
vf(q)
#~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "")
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)
from hpp.corbaserver.fleche import Robot
robot = Robot('fleche')
robot.setJointBounds("root_joint", [-70, 4.375, -5, 4.375, -5, 7])
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] = [-60, -4]
vf.loadObstacleModel("iai_maps", "ville1", "ville1_link")
v = vf.createViewer()
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
print(ps.solve())
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_goal)
ps.addPathOptimizer("RandomShortcut")
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.displayPath(1)
#pp (0)
import json
a = ps.getWaypoints(1)
# Creating an instance of HPP problem solver
ps = ProblemSolver(rbprmBuilder)
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound", vMax)
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setParameter("DynamicPlanner/sizeFootX", 0.2)
ps.setParameter("DynamicPlanner/sizeFootY", 0.12)
ps.setParameter("DynamicPlanner/friction", 0.5)
ps.setParameter("Kinodynamic/forceYawOrientation", True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory(ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel("hpp_environments", "multicontact/ground",
"planning", vf)
#load the viewer
try:
v = vf.createViewer(displayArrows=True)
except Exception:
print "No viewer started !"
class FakeViewer():
sceneName = ""
# will be stastified along the whole trajectory.
ps.setParameter("Kinodynamic/velocityBound", vMax)
ps.setParameter("Kinodynamic/accelerationBound", aMax)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100)
# Uncomment the following line if you want to constraint the orientation of the base of
# the robot to follow the direction of the motion. Note that in this case the initial
# and final orientation are not considered.
# ps.setParameter("Kinodynamic/forceOrientation",True)
# The following line constraint the random sampling method to fix all the extraDOF at 0
# during sampling. Comment it if you want to sample states with non-null velocity and
# acceleration. Note that it increase the complexity of the problem and greatly increase
# the computation time.
ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
vf = ViewerFactory(ps)
q_init = robot.getCurrentConfig()
q_goal = q_init[::]
q_init[0:3] = [6.5, -4, 0.4] # root position
q_init[3:7] = [0, 0, 0, 1] # root rotation
# set initial velocity (along x,y,z axis) :
q_init[-6:-3] = [0, 0, 0]
# q_goal[0:3] = [6.5,-1,0.4] # straight line
q_goal[0:3] = [3, -4, 0.4] # easy goal position
# q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position
# set goal velocity (along x,y,z axis) :
q_goal[-6:-3] = [0, 0, 0]
