Commit 9ca80276 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub

Merge pull request #3 from florent-lamiraux/master

Benchmark run after refactoring constraints
parents 70fde169 796beead
Benchmark run after refactoring constraints
- Error of explicit relative poses are now evaluated with the implicit
formulation to make the value consistent between edges and nodes in the
manipulation constraint graph.
\ No newline at end of file
0:00:04.149720
Number nodes: 70
0:00:05.873612
Number nodes: 111
0:00:09.476805
Number nodes: 179
0:00:10.643584
Number nodes: 215
0:00:04.112079
Number nodes: 86
0:00:01.821590
Number nodes: 27
0:00:03.584685
Number nodes: 76
0:00:03.601712
Number nodes: 69
0:00:13.781280
Number nodes: 247
0:00:01.941743
Number nodes: 40
0:00:05.161452
Number nodes: 92
0:00:06.473669
Number nodes: 110
0:00:04.012887
Number nodes: 71
0:00:02.914368
Number nodes: 62
0:00:02.028876
Number nodes: 58
0:00:07.662905
Number nodes: 149
0:00:06.452738
Number nodes: 122
0:00:07.880368
Number nodes: 143
0:00:05.027291
Number nodes: 95
0:00:02.606938
Number nodes: 43
Average time: 5.4604151
Average number nodes: 103.25
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.baxter import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
ConstraintGraphFactory, Constraints, Rule
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import Color, PathPlayer
from math import sqrt
# nbBoxes
K = 2
nBoxPerLine = 2
# grippers = [ "baxter/r_gripper",]
grippers = [ "baxter/r_gripper" , "baxter/l_gripper"]
# Box i will be at box goal[i] place at the end
#goal = [1, 2, 0]
goal = [1, 0]
# Load robot and object. {{{3
# Define classes for the objects {{{4
class Table (object):
rootJointType = "anchor"
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'table'
urdfSuffix = ""
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 0, 0, 0, 1])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
boxes.append ("box" + str(i))
vf.loadObjectModel (Box, boxes[i])
robot.setJointBounds (boxes[i]+ '/root_joint', [-1,0.5,-1,2,0.6,1.9,-1,1,-1,1,-1,1,-1,1])
def setBoxColors (gui):
c = Color()
for i in xrange(K):
gui.setColor (boxes[i], c[i])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB = list()
for i in xrange(K):
rankB.append (robot.rankInConfiguration [boxes[i] + '/root_joint'])
bb = [-0.3, -0.4, 0.7, 0.9]
c = sqrt (2) / 2
xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0])
nbCols = int(K * 1. / nBoxPerLine + 0.5)
ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2])
for i in xrange(K):
iL = i % nBoxPerLine
iC = (i - iL) / nBoxPerLine
x = bb[0] + xstep * iL
y = bb[2] + xstep * iC
q_init [rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, 0, -c, c]
q_goal = q_init [::]
for i in xrange(K):
r = rankB[i]
rn = rankB[goal[i]]
q_goal[r:r+7] = q_init[rn:rn+7]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
ps.selectPathValidation ('Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.2)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
for n in jointNames['all']:
if n.startswith ("baxter"):
if n.startswith ("baxter/left_"):
jointNames['baxterLeftSide'].append (n)
if n.startswith ("baxter/right_"):
jointNames['baxterRightSide'].append (n)
# 4}}}
# Locks joints that are not used for this problem {{{4
lockFingers = ["r_gripper_l_finger",
"r_gripper_r_finger",
"l_gripper_l_finger",
"l_gripper_r_finger",
]
for side in ["r", "l", ]:
ps.createLockedJoint(side + "_gripper_l_finger", "baxter/" + side + "_gripper_l_finger_joint", [ 0.02,])
ps.createLockedJoint(side + "_gripper_r_finger", "baxter/" + side + "_gripper_r_finger_joint", [-0.02,])
lockHead = ['head_pan',]
ps.createLockedJoint ('head_pan', 'baxter/head_pan',
[q_init[robot.rankInConfiguration['baxter/head_pan']]])
for n in jointNames["baxterRightSide"]:
ps.createLockedJoint (n, n, [0,])
for n in jointNames["baxterLeftSide"]:
ps.createLockedJoint (n, n, [0,])
lockAll = lockFingers + lockHead
# 4}}}
# 3}}}
handlesPerObject = list ()
handles = list ()
objContactSurfaces = list ()
for i in xrange(K):
handlesPerObject.append ([boxes[i] + "/handle2"])
handles.append (boxes[i] + "/handle2")
objContactSurfaces .append ([boxes[i] + "/box_surface"])
# Build rules
rules = [Rule ([".*"], [".*"], True)]
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (['table/pancake_table_table_top'])
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = lockAll))
cg.initialize ()
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_init)
if not res[0]:
raise Exception ('Init configuration could not be projected.')
q_init_proj = res [1]
res = ps.client.manipulation.problem.applyConstraints (cg.nodes['free'], q_goal)
if not res[0]:
raise Exception ('Goal configuration could not be projected.')
q_goal_proj = res [1]
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init_proj)
ps.addGoalConfig (q_goal_proj)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = ps.numberNodes ()
totalNumberNodes += n
print ("Number nodes: " + str(n))
if N != 0:
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float(N)))
#v = vf.createViewer ()
#pp = PathPlayer (v)
baxter_common: 6c4b0f375fe4e356a3b12df26ef7c0d5e58df86e
collada-dom: a03897f736f9132a6d7c0c943a78d62e66ffd11a
gepetto-viewer: b0886e5bd3166bd4c9fa629f109501c5436c2ca2
gepetto-viewer-corba: 3ec2798c4411af46da9e7f05c4dd2ed5921da905
hpp-baxter: 3acb7dc48216f6848328c0fe726f66444561ef96
hpp_benchmark: 70fde16988b80aa07978ca902973005e11ad78da
hpp-constraints: d141da01b945dd36c51adaf8f21a91cdfb6eb835
hpp-corbaserver: 866fde6eb341805a7d8ceec0f97912d5592f6441
hpp-core: 978b5967db14faf1f35bd9f08203cc0bf8c2dbe0
hpp-doc: a37f17406f7677ddb83894fdd6fe11466fecfcdf
hpp-environments: c24c45f2c4ad00fd29a5afa3e789c91890d1cac7
hpp-fcl: 21c3a589b129c64b5c370dd15b58fb00c9a2bb99
hpp-gepetto-viewer: 6f28e28100e9184ff514bba2f84d925b12ac2a36
hpp-gui: 4820283646fb4e02fa27f57a56b68ffbd97e87d0
hpp-hrp2: f1655b73ef2a76976003b124e810c7dd391f5a73
hpp-manipulation: 6bedc67eba71f0f4287f688528e05607e9fc5fb0
hpp-manipulation-corba: b5112346654f22531e0eacb974f93f5fd619ad47
hpp-manipulation-urdf: a0b063e8cf1e905100f9c66b0f0404e7dd3b1b2e
hpp-pinocchio: 6822839c32fcb00cccd7ce808525e35ebd5606aa
hpp-plot: a27ae9beaafe45ed226da46b6af48398d71e17d9
hpp_romeo: a543048ba13d21d8dfc850d44c9d1023aac1a461
hpp-statistics: 1b3fb2456d8eb37b1d6580a026bac007171f966b
hpp-template-corba: b62e77273aecf583637444dd0deb44b670bafc59
hpp-tools: 1dcbde657e82c49426d15a91b556553942abffc4
hpp_tutorial: 90383e4f91b634dc77be755185bfef9fdde5e9b0
hpp_universal_robot: ff81c3a89e558751b89b6d0e2873a6606766ec67
hpp-util: 692575ff0893f9d0c25da9030658e15da023bdba
hpp-walkgen: 4968aa2b4de12d3b5226574243d16d6309062297
hpp-wholebody-step: 2accb4ed20c4926361c27f61db6a7a427d39069b
hpp-wholebody-step-corba: 2b7f6c0164aa97bdf4974d76df463d16d514e644
hrp2-14-description: 71423ed7ed6e303e82940d8889fd102c3e7a8526
iai_maps: 2add6b526f9d61d3dcbc4f3c02e0e9d34e30e769
OpenSceneGraph-dae-plugin: 64ad7a5f44837c067f42c0b83ccb70c8eae7ac36
pinocchio: 92ffa68b15d1948bf763846f287ef75b1629c580
pythonqt: 73ae333f80fd7fb7952713b138f942d18890951e
qgv: bf9497f3c3691e257c3f229cde93700eb90fe207
qpOASES: a4f413fefcd5ced70bfd22ec6dd9f9808b08d254
roboptim-core: 27205035697382cbf172d7488196a2b7451c7ad1
roboptim-trajectory: e13630d601b5d47308a6b68a2ecb4eeb34ea0ad7
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
romeo: 12c3d1e3822c39c3af67d4b7054380deea01505b
universal_robot: b466dadacc4e7a4ce23db127db33e219ae0c97ce
0:00:19.991395
Number nodes: 15
0:00:06.268456
Number nodes: 20
0:00:05.556285
Number nodes: 15
0:00:15.497764
Number nodes: 13
0:00:15.414143
Number nodes: 14
0:00:22.439701
Number nodes: 11
0:00:04.268345
Number nodes: 11
0:00:00.543420
Number nodes: 12
0:00:22.606587
Number nodes: 18
0:00:00.688967
Number nodes: 14
0:00:16.686236
Number nodes: 12
0:00:48.943416
Number nodes: 32
0:00:00.488916
Number nodes: 13
0:00:01.764369
Number nodes: 20
0:00:00.672495
Number nodes: 13
0:00:00.380909
Number nodes: 12
0:00:00.923306
Number nodes: 19
0:00:11.295675
Number nodes: 12
0:00:14.085788
Number nodes: 14
0:00:15.208887
Number nodes: 14
Average time: 11.186253
Average number nodes: 53.05
#
# Copyright (2017) CNRS
#
# Author: Florent Lamiraux
#
import re, os
from math import pi, fabs
import hpp
from hpp.corbaserver.manipulation import ConstraintGraphFactory, Rule
from setup import ConstraintGraph, Constraints, grippers, handlesPerObjects, nCylinder, nSphere, objects, ps, robot, shapesPerObject, vf
from hpp.gepetto import PathPlayer
from state_name import StateName
from visibility_prm import VisibilityPRM
import time
def cleanPaths (ps, solutions) :
offset = 0
for s, i in zip (solutions, xrange (100000)):
for j in range (s - offset):
ps.erasePath (i)
offset += 1
offset += 1
## Write log in a file
#
devel_dir = os.getenv ('DEVEL_DIR')
def makeRule (grasps):
'''
Build a rule that will generate a state where each specified gripper will
grasp the specify handle.
This rule is used in the construction of the constraint graph to generate the
corresponding state
'''
_grippers = list ()
_handles = list ()
for (g,h) in grasps:
_grippers.append (g)
_handles.append (h)
for g in grippers:
if not g in _grippers:
_grippers.append (g)
_handles += (len (_grippers) - len (_handles)) * ['^$']
return Rule (grippers = _grippers, handles = _handles, link = True)
def getTransitionConnectingStates (graph, s1, s2):
'''
Get transition edge connecting two states of the constraint graph
Input:
- graph: the manipulation constraint graph,
- s1, s2: states of the constraint graph
'''
# Filter out waypoint and levelset edges from the constraint graph
p = re.compile ('\|.*_.*')
_edges = set (filter (lambda s : p.search (s) is None, graph.edges.keys ()))
for edge in _edges:
node1, node2 = cg.getNodesConnectedByEdge (edge)
if StateName (node1) == s1 and StateName (node2) == s2:
return edge
def getEdges (graph, nodes, exploreNodes):
'''
Build list of edges linking a list of nodes
Input:
- graph: the manipulation constraint graph,
- nodes: list of nodes the solution path should visit,
- exploreNodes: states in which to perform path planning to cross an edge
this list contains the same number of element as the
resulting list of edges.
Return
- edges: list of edges the resulting path should cross,
- loops: list of loop transitions of each element of list "exploreNodes".
These transitions are used to perform exploration in order to cross
edges of list "edges".
'''
edges = list ()
loops = list ()
# Filter out waypoint and levelset edges from the constraint graph
p = re.compile ('\|.*_.*')
_edges = set (filter (lambda s : p.search (s) is None, graph.edges.keys ()))
for n1, n2, n3 in zip (nodes, nodes [1:], exploreNodes):
edgeFound = False
loopFound = False
for edge in _edges:
node1, node2 = cg.getNodesConnectedByEdge (edge)
if StateName (node1) == n1 and StateName (node2) == n2:
edges.append (edge)
edgeFound = True
if StateName (node1) == n3 and StateName (node2) == n3:
loops.append (edge)
loopFound = True
if edgeFound and loopFound:
break
if not edgeFound : raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n2))
if not loopFound : raise RuntimeError \
('cannot find edge from node "{0}" to "{1}"'.format (n1, n1))
return edges, loops
# infinite norm between vectors
dC = lambda q1,q2: reduce (lambda x,y : x if fabs (y [0]- y [1]) < x \
else fabs (y [0]- y [1]), zip (q1, q2), 0)
display = False
if display:
v = vf.createViewer ()
pp = PathPlayer (v)
else:
v = lambda x: None
## Initial configuration of manipulator arms
q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0,]
q0_r1 = q0_r0 [::]
## Generate initial configurations of spheres
q0_spheres = list ()
i = 0
y = -0.04
while i < nSphere:
q0_spheres.append ([-0.45 - .1*(i/2), y, 0.025, 0, 0, 0, 1])
i+=1; y = -y
## Generate initial configurations of cylinders
q0_cylinders = list ()
i = 0
y = -0.04
while i < nCylinder:
q0_cylinders.append ([0.45 + .1*(i/2), y, 0.025, 0, 0, 0, 1])
i+=1; y = -y
q0 = q0_r0 + q0_r1 + sum (q0_spheres, []) + sum (q0_cylinders, [])
if display:
v (q0)
# List of nodes composing the assembly sequence
nodes = list ()
# List of rules that define all the nodes
rules = list ()
# List of grasps for each node. From any node to the next one, one grasp is
# added or removed
grasps = set ()
# list of nodes that are explored to cross each edge
exploreNodes = list ()
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
# grasp sphere0
grasps.add (('r0/gripper', 'sphere0/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# grasp cylinder0
grasps.add (('r1/gripper', 'cylinder0/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# assemble cylinder0 and sphere0
grasps.add (('cylinder0/magnet0', 'sphere0/magnet'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# release sphere0
grasps.remove (('r0/gripper', 'sphere0/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-1])
# grasp sphere1
grasps.add (('r0/gripper', 'sphere1/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# assemble sphere1
grasps.add (('cylinder0/magnet1', 'sphere1/magnet'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# release sphere1
grasps.remove (('r0/gripper', 'sphere1/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
# release cylinder0 : put assembly on the ground
grasps.remove (('r1/gripper', 'cylinder0/handle'))
nodes.append (StateName (grasps))
rules.append (makeRule (grasps = grasps))
exploreNodes.append (nodes [-2])
cg = ConstraintGraph (robot, 'assembly')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
#factory.environmentContacts (['table/pancake_table_table_top'])
factory.setObjects (objects, handlesPerObjects, shapesPerObject)
factory.setRules (rules)
factory.generate ()
cg.initialize ()
edges, loops = getEdges (graph = cg, nodes = nodes, exploreNodes = exploreNodes)
ps.selectPathProjector ('Progressive', .05)
## Add a node to move robots in initial configurations
nodes.append (nodes [-1])
edges.append (getTransitionConnectingStates (graph = cg, s1 = nodes [-2],
s2 = nodes [-1]))
loops.append (getTransitionConnectingStates (graph = cg, s1 = nodes [-1],
s2 = nodes [-1]))
exploreNodes.append (nodes [-1])
def generateSubGoals (q0, edges):
'''
Generate a list of subgoals
Input:
- q0: initial configuration
- edges: list of transitions
Return
- a list of subgoal configurations
The first subgoal is the initial configuration q0. The following ones are
generated by projecting random configurations on the destination node of
each transition of the list. Each subgoal is reachable from the previous one
by a path of the transition.
'''
subgoals = list ()
node = 'free'
res, q_init, err = cg.applyNodeConstraints (node, q0)
if not res:
raise RuntimeError ('Failed to project configuration on node ' + node)
subgoals.append (q_init [::])
## Starting from initial configuration, iteratively produce random
# configurations on each successive node in such a way that each new
# configuration is reachable from the previous one through the transition
# linking the states of the configurations.
q_init = subgoals [0]
for edge in edges [:-1]:
edgeSuccess = False
for i in range (400):
if i == 0:
q = q_init
else:
q = robot.shootRandomConfig ()
res, q1, err = cg.generateTargetConfig (edge, q_init, q)
if not res: continue
res, msg = robot.isConfigValid (q1)
if not res: continue
v (q1)
ps.addConfigToRoadmap (q1)
subgoals.append (q1 [::])
q_init = q1 [::]
edgeSuccess = True
break
if not edgeSuccess:
raise RuntimeError ('Failed to generate a subgoal through edge ' + edge)
## Generate last sub goal configuration to move back manipulator arms in
# initial configurations
q_goal = subgoals [-1] [::]
q_goal [0:6] = q0_r0; q_goal [6:12] = q0_r1
subgoals.append (q_goal)
return subgoals
## Display sequence of states in file
def displayGraph ():
with open ('/tmp/states.dot', 'w') as f1:
f1.write ('digraph CD {\n rankdir = BT\n compound=true\n\n')
for n in nodes:
f1.write (' "{0}" [shape = box]\n'.format (n))
for n0, n1, e in zip (nodes, nodes [1:], edges):
f1.write (' "{0}" [shape = oval]\n'.format (e))
f1.write (' "{0}" -> "{1}"\n'.format (n0, e))
f1.write (' "{0}" -> "{1}"\n'.format (e, n1))
f1.write ('}')
## Try to connect successive configurations using Visibility PRM planner
def solve ():
for p0, p1, i in zip (subgoals, subgoals [1:], xrange (100000)):
planner = VisibilityPRM (cg = cg, ps = ps, q_init = p0, q_goal = p1,
state = exploreNodes [i],
loopTransition = loops [i], logStream = None)
planner.solve ()
#ps.addPathOptimizer ('Graph-RandomShortcut')
#ps.addPathOptimizer ('SplineGradientBased_bezier1')
solutions = list ()
# Solve 20 times the problem
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
numberNodes = 0
# Try 20 times to solve the problem and stop at first success
t1 = dt.datetime.now ()
for i in range (20):
try:
subgoals = generateSubGoals (q0 = q0, edges = edges)
ps.clearRoadmap ()
ps.setInitialConfig (subgoals [0])
q_goal = subgoals [-1]
ps.resetGoalConfigs ()
ps.addGoalConfig (q_goal)
solve ()
ps.solve ()
n = ps.numberNodes ()
numberNodes += n
solutions.append (ps.numberPaths () -1)
break;
except RuntimeError:
n = ps.numberNodes ()
numberNodes += n
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
print ("Number nodes: " + str(n))
totalNumberNodes += numberNodes
if N != 0:
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float(N)))
from math import pi
from hpp.corbaserver.manipulation import Client, ConstraintGraph, \
Constraints, ProblemSolver
from hpp.corbaserver.manipulation.ur5 import Robot
from hpp.gepetto.manipulation import ViewerFactory
from hpp.gepetto import PathPlayer
Client ().problem.resetProblem ()
Robot.urdfName = "ur3_gripper"
Robot.urdfSuffix = ""
Robot.srdfSuffix = ""
class Cylinder_08 (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
urdfName = 'construction_set/cylinder_08'
urdfSuffix = ""
srdfSuffix = ""
class Cylinder_13 (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
urdfName = 'construction_set/cylinder_13'
urdfSuffix = ""
srdfSuffix = ""
class Sphere (object):
rootJointType = 'freeflyer'
packageName = 'hpp_environments'
urdfName = 'construction_set/sphere'
urdfSuffix = ""
srdfSuffix = ""
class Ground (object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'construction_set/ground'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('2ur5-sphere', 'r0')
robot.setJointPosition ('r0/root_joint',