Commit 2d06bd38 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr

New benchmarks with romeo and construction set.

parent d867354c
devel branches as of 2018-03-07
- new benchmark with Romeo and construction set,
- benchmarks run on florent-laptop.
\ No newline at end of file
0:00:02.114809
Number nodes: 57
0:00:04.871240
Number nodes: 140
0:00:02.438279
Number nodes: 67
0:00:03.131689
Number nodes: 85
0:00:02.705720
Number nodes: 77
0:00:05.258558
Number nodes: 156
0:00:03.284864
Number nodes: 88
0:00:08.579428
Number nodes: 221
0:00:02.453221
Number nodes: 68
0:00:01.684416
Number nodes: 45
0:00:03.673817
Number nodes: 105
0:00:02.950398
Number nodes: 73
0:00:02.640406
Number nodes: 69
0:00:03.172492
Number nodes: 80
0:00:05.789657
Number nodes: 165
0:00:07.858056
Number nodes: 213
0:00:04.388711
Number nodes: 117
0:00:00.689034
Number nodes: 17
0:00:06.246648
Number nodes: 163
0:00:05.246649
Number nodes: 142
Average time: 3.9589046
Average number nodes: 107.4
# 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: f182ed101a77ce01a1a7b7c32feb9cbcf4be8b22
gepetto-viewer-corba: 2daf7c2dc2741a9f59cf5afd9b66a41c8f100808
hpp-baxter: 08a9aefcf9c5e6d7e4d5ce320e838d9a81f9a69c
hpp_benchmark: d867354c5f5cab535d06099af79a78761bd4ba53
hpp-constraints: 98c23f3c66d3704bae787e5f53603e9dabfe2811
hpp-corbaserver: eeeafd23d59259a22b571c179e7bded602b981a8
hpp-core: 7a8b2bad5a8c328a94d59b99f9bb7cae0d81852e
hpp-doc: 5225e3093f531dd769f6a9b0c25092d02fb0a7d7
hpp-environments: d42603a08b9abdfb93690209d2d180e0d7c3e55d
hpp-fcl: 2fb20e168a1e32fa0efcec633f1f01f6c9ed8c54
hpp-gepetto-viewer: 29e3acdb58a36b163c20d8d5a75bfe3f533bd43f
hpp-gui: 6b190b9f461e225a09f2a6c25cdd18036594911c
hpp-hrp2: 4bff1683e8de56969c0d2e9f0649a7d132323b6b
hpp-manipulation: 24ac8e141f897b98cb6d0a10599b8a4f919b4deb
hpp-manipulation-corba: e2ab7f57c1e335a57eb901037e5654e3d716882c
hpp-manipulation-urdf: ed9096b11eb7f8171f61362ad45fe53d2b85cc20
hpp-pinocchio: 118bcd1dc2dc96cdec32c424b9d43c99df25a787
hpp-plot: 999e3b160430461a62aa4469c6135851e3d1294e
hpp_romeo: 0bb624e71273aae9a4cf43eb973d8ca4315f0251
hpp-statistics: fe1a714fd90d4d7cc21350dd1daa0fa855e966cd
hpp-template-corba: 532f8b49c1fcc91a8cce7a743258b6789066f9da
hpp_tutorial: 6a778c041c2cb5213e7190f0bb7a5929d2b297de
hpp_universal_robot: d51c00b4828000f669d07add174a8587846801aa
hpp-util: 12a147fc4e6dc2bf1d1f03d332fb4fd25f56253d
hpp-walkgen: ad3e9f78fc2db24704f5834b717b5e8b9c768015
hpp-wholebody-step: 644fe3d90bfb305087e402d89f9a53636dd2f30d
hpp-wholebody-step-corba: cdf5866ed3048d31b2bc7c46d5d1c2c163b9aa5d
hrp2: bd1a22f86a83d0a5ee0166caa9290d42af7f2a76
iai_maps: dbc724906b803f77c97771a5227da0cce1ee1e23
OpenSceneGraph-dae-plugin: f15a82f547728f17c85777be99d23676262a6dc5
pinocchio: 9243a5cafd37946eb995e99a0e41d47eca301bd4
pythonqt: 794ea38316b8c8d99d40e20263e32debfe2efab5
qgv: bf9497f3c3691e257c3f229cde93700eb90fe207
qpOASES: c7e5ff50a770cf3a2b1b56eb375941be6088b26b
robot_capsule_urdf: 7ca48311a16b05b639682d1d780daa18049ec06f
robot_model_py: ba8d4de4cd811374e490bb93c825677f7590eb06
robot_state_chain_publisher: be765c4bdb361202b172dbad1bb66d03c04b9fd6
romeo: 12c3d1e3822c39c3af67d4b7054380deea01505b
universal_robot: 75cd154762b04d1132f7419d6ca21e639fa9b8d1
0:00:15.603637
Number nodes: 15
0:00:04.743334
Number nodes: 16
0:00:04.236818
Number nodes: 15
0:00:06.193526
Number nodes: 16
0:00:00.438788
Number nodes: 12
0:00:00.464755
Number nodes: 14
0:00:00.385792
Number nodes: 14
0:00:00.247398
Number nodes: 13
0:00:00.331478
Number nodes: 13
0:00:00.749391
Number nodes: 17
0:00:00.491593
Number nodes: 12
0:00:26.694656
Number nodes: 14
0:00:00.234224
Number nodes: 10
0:00:03.143240
Number nodes: 11
0:00:00.394450
Number nodes: 13
0:00:08.717837
Number nodes: 14
0:00:00.487341
Number nodes: 12
0:00:18.854063
Number nodes: 11
0:00:00.654115
Number nodes: 14
0:00:02.117639
Number nodes: 18
Average time: 4.75920375
Average number nodes: 33.9
#
# 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, PathPlayerGui
from state_name import StateName
from visibility_prm import VisibilityPRM
import time
## 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', [-.25, 0, 0, 0, 0, 0, 1])
ps = ProblemSolver (robot)
ps.setErrorThreshold (1e-4)
ps.setMaxIterProjection (40)
vf = ViewerFactory (ps)
#
# Load robots and objets
# - 2 UR3,
# - 4 spheres,
# - 4 short cylinders,
vf.loadRobotModel (Robot, "r1")
robot.setJointPosition ('r1/root_joint', [.25, 0, 0, 0, 0, 1, 0])