...
 
Commits (12)
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
This diff is collapsed.
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])
# Change bounds of robots to increase workspace and avoid some collisions
robot.setJointBounds ('r0/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds ('r1/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds ('r0/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds ('r1/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds ('r0/elbow_joint', [-2.6, 2.6])
robot.setJointBounds ('r1/elbow_joint', [-2.6, 2.6])
vf.loadEnvironmentModel (Ground, 'ground')
nSphere = 2
nCylinder = 2
objects = list ()
for i in range (nSphere):
vf.loadObjectModel (Sphere, 'sphere{0}'.format (i))
robot.setJointBounds ('sphere{0}/root_joint'.format (i),
[-1.,1.,-1.,1.,-.1,1.,-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
objects.append ('sphere{0}'.format (i))
for i in range (nCylinder):
vf.loadObjectModel (Cylinder_08, 'cylinder{0}'.format (i))
robot.setJointBounds ('cylinder{0}/root_joint'.format (i),
[-1.,1.,-1.,1.,-.1,1.,-1.0001, 1.0001,-1.0001, 1.0001,
-1.0001, 1.0001,-1.0001, 1.0001,])
objects.append ('cylinder{0}'.format (i))
r_joints = []
for i in range (2):
r_joints.append (['r{0}/shoulder_pan_joint'.format (i),
'r{0}/shoulder_lift_joint'.format (i),
'r{0}/elbow_joint'.format (i),
'r{0}/wrist_1_joint'.format (i),
'r{0}/wrist_2_joint'.format (i),
'r{0}/wrist_3_joint'.format (i),])
## Gripper
#
grippers = ['cylinder{0}/magnet0'.format (i) for i in range (nCylinder)]
grippers += ['cylinder{0}/magnet1'.format (i) for i in range (nCylinder)]
grippers += ["r{0}/gripper".format (i) for i in range (2)]
## Handles
#
handlesPerObjects = [['sphere{0}/handle'.format (i),
'sphere{0}/magnet'.format (i)] for i in range (nSphere)]
handlesPerObjects += [['cylinder{0}/handle'.format (i)] for i in
range (nCylinder)]
## Contact surfaces
shapesPerObject = [[] for o in objects]
## Constraints
#
for i in range (nSphere):
placementName = "place_sphere{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"sphere{0}/root_joint".format (i),
[0, 0, 0.025, 0, 0, 0, 1],
[False, False, True, True, True, False])
for i in range (nCylinder):
placementName = "place_cylinder{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"cylinder{0}/root_joint".format (i),
[0, 0, 0.025, 0, 0, 0, 1],
[False, False, True, True, True, False])
for i in range (nSphere):
placementName = "preplace_sphere{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"sphere{0}/root_joint".format (i),
[0, 0, 0.075, 0, 0, 0, 1],
[False, False, True, True, True, False])
for i in range (nCylinder):
placementName = "preplace_cylinder{0}".format (i)
ps.createTransformationConstraint (placementName, "",
"cylinder{0}/root_joint".format (i),
[0, 0, 0.075, 0, 0, 0, 1],
[False, False, True, True, True, False])
#
# Copyright (2017) CNRS
#
# Author: Florent Lamiraux
#
class StateName (object):
"""
Handle permutations in state names
State names are built according to the following pattern:
'gripper name grasps handle name' separated by ':'
A given state may therefore have a name difficult to predict since the order
of the above sentences may vary.
This class handles the variation.
"""
noGrasp = 'free'
def __init__ (self, grasps) :
"""
Node names is defined by a set of pairs (gripper, handle)
"""
if isinstance (grasps, set):
self.grasps = grasps.copy ()
elif isinstance (grasps, str):
if grasps == self.noGrasp:
self.grasps = set ()
else:
g1 = map (lambda s: s.strip (' '), grasps.split (':'))
self.grasps = set (map (lambda s: tuple (s.split (' grasps ')), g1))
else:
raise TypeError ('expecting a set of pairs (gripper, handle) or a string')
def __str__ (self) :
if self.grasps == set ():
return 'free'
res = ""
for g in self.grasps:
res += g [0] + " grasps " + g [1] + " : "
return res [:-3]
def __eq__ (self, other):
return self.grasps == other.grasps
def __ne__ (self, other):
return not self.__eq__ (other)
#
# Copyright (2017) CNRS
#
# Author: Florent Lamiraux
#
from state_name import StateName
class Edge (object):
'''
Store an edge as a triple (n0, n1, pathId)
Input
- n0: initial node,
- n1 : goal node,
- pathId: id of a path going from "n0" to "n1".
'''
def __init__ (self, n0, n1, pathId):
self.n0 = n0
self.n1 = n1
self.pathId = pathId
class VisibilityPRM (object):
'''
Run visibility PRM in a State of a manipulation constraint graph in order
to connect two configurations
'''
@staticmethod
def getState (cg, nodeName) :
for n in cg.nodes:
if StateName (n) == nodeName : return n
raise KeyError (nodeName)
maxIter = 100
def __init__ (self, cg, ps, q_init, q_goal, state, loopTransition,
logStream):
'''
Constructor
Input
- cg: the constraint graph that contains the state and edge,
- ps: manipulation ProblemSolver instance,
- q_init, q_goal: the initial and goal configurations. Both should lie
in the input state,
- state: state of the constraint graph in which the roadmap is built,
- loopTransition: the transition linking the state with itsef,
- logStream: output stream to write log
'''
self.cg = cg
self.ps = ps
self.state = self.getState (cg = cg, nodeName = state)
self.loopTransition = loopTransition
res, err = cg.getConfigErrorForNode (self.state, q_init)
if not res:
raise RuntimeError ("q_init = " + str (q_init) + " does not satisfy"
+ " constraints of state " + self.state)
self.q_init = q_init
self.q_goal = q_goal
self.ps.addConfigToRoadmap (q_init)
self.ps.addConfigToRoadmap (q_goal)
res, pid, msg = self.ps.directPath (q_init, q_goal, True)
if res and self.ps.pathLength (pid) > 0:
self.ps.addEdgeToRoadmap (q_init, q_goal, pid, True)
self.solved = True
else:
self.solved = False
self.isSolved ()
self.logStream = logStream
# build list of nodes from current roadmap that lie in the right state
nodes = filter (lambda q : self.cg.getConfigErrorForEdgeLeaf \
(self.loopTransition, self.q_init, q) [0],
self.ps.nodes ())
nodes.append (self.q_init)
nodes.append (self.q_goal)
self.nodes = set (map (tuple, nodes))
def writeLog (self, s):
if self.logStream: self.logStream.write (s)
def isSolved (self):
'''
Compute whether problem is solved
'''
if self.solved == True: return True
for i in range (self.ps.numberConnectedComponents ()):
nodes = self.ps.nodesConnectedComponent (i)
if self.q_init in nodes and self.q_goal in nodes:
self.solved = True
return self.solved
self.solved = False
return self.solved
def solve (self):
'''
Solve path planning problem by running a Visibility PRM algorithm
'''
self.nIter = 0
while not self.isSolved ():
self.oneStep ()
self.nIter += 1
self.writeLog ("nIter = " + str (self.nIter))
if self.nIter > self.maxIter :
self.writeLog ("Maximal number of iterations reached")
self.writeLog (' between\n')
self.writeLog ('q_init = {0} and\n'.format (self.q_init))
self.writeLog ('q_goal = {0}\n'.format (self.q_goal))
raise RuntimeError ("Maximal number of iterations reached.")
self.writeLog ('successfully solved path planning between\n')
self.writeLog ('q_init = {0} and\n'.format (self.q_init))
self.writeLog ('q_goal = {0}\n'.format (self.q_goal))
def oneStep (self):
'''
Run one step of Visibility PRM
'''
# Generate a random configuration
for i in range (50):
q = self.ps.robot.shootRandomConfig ()
res, q_rand, err = self.cg.generateTargetConfig \
(self.loopTransition, self.q_init, q)
if not res: continue
res, msg = self.ps.robot.isConfigValid (q_rand)
if res: break
if i == 49:
self.writeLog \
("Failed to generate a random valid configuration.")
raise RuntimeError \
("Failed to generate a random valid configuration.")
# try to connect to other connected components
edges = list ()
for i in range (self.ps.numberConnectedComponents ()):
edge = self.connect (q_rand, i)
if edge and self.ps.pathLength (edge.pathId) > 0:
edges.append (edge)
if len (edges) != 1:
self.ps.addConfigToRoadmap (q_rand)
self.nodes.add (tuple (q_rand))
#self.writeLog ('Added q_rand = {0} in roadmap and connected it with\n'.format (q_rand))
for e in edges:
self.ps.addEdgeToRoadmap (e.n0, e.n1, e.pathId, True)
#self.writeLog ('n1 = {0}\n'.format (e.n1))
#self.writeLog ('pathId = {0}\n'.format (e.pathId))
def connect (self, config, i):
'''
Try to connect random configuration to connected component of roadmap
Input
- config: random configuration in search state,
- i: id of the connected component of the roadmap
'''
# Consider only nodes in the state that are reachable from q_init
nodes = self.nodes & set (map (tuple,
self.ps.nodesConnectedComponent (i)))
# sort by increasing distance to input configuration
nodesAndDistances = list ()
for n in nodes:
res, pid, msg = self.ps.directPath (config, n, False)
if not res:
d = float('+inf')
else:
d = self.ps.pathLength (pid)
nodesAndDistances.append ((n, d))
nodesAndDistances.sort (key = lambda x : x [1])
# Try to connect input configuration to each node in order of increasing
# distance.
for n, d in nodesAndDistances:
if d == float('+inf'):
res = False; pid = -1;
else:
res, pid, msg = self.ps.directPath (config, n, True)
if res:
return Edge (n0 = config, n1 = n, pathId = pid)
return None
cat /proc/cpuinfo
processor : 0
vendor_id : GenuineIntel
cpu family : 6
model : 58
model name : Intel(R) Core(TM) i7-3540M CPU @ 3.00GHz
stepping : 9
microcode : 0x20
cpu MHz : 1447.331
cache size : 4096 KB
physical id : 0
siblings : 4
core id : 0
cpu cores : 2
apicid : 0
initial apicid : 0
fpu : yes
fpu_exception : yes
cpuid level : 13
wp : yes
flags : fpu vme de pse tsc msr pae mce cx8 apic sep mtrr pge mca cmov pat pse36 clflush dts acpi mmx fxsr sse sse2 ss ht tm pbe syscall nx rdtscp lm constant_tsc arch_perfmon pebs bts rep_good nopl xtopology nonstop_tsc cpuid aperfmperf pni pclmulqdq dtes64 monitor ds_cpl vmx smx est tm2 ssse3 cx16 xtpr pdcm pcid sse4_1 sse4_2 x2apic popcnt tsc_deadline_timer aes xsave avx f16c rdrand lahf_lm cpuid_fault epb pti ssbd ibrs ibpb stibp tpr_shadow vnmi flexpriority ept vpid fsgsbase smep erms xsaveopt dtherm ida arat pln pts flush_l1d
bugs : cpu_meltdown spectre_v1 spectre_v2 spec_store_bypass l1tf
bogomips : 5980.77
clflush size : 64
cache_alignment : 64
address sizes : 36 bits physical, 48 bits virtual
power management:
processor : 1
vendor_id : GenuineIntel
cpu family : 6
model : 58
model name : Intel(R) Core(TM) i7-3540M CPU @ 3.00GHz
stepping : 9
microcode : 0x20
cpu MHz : 1248.631
cache size : 4096 KB
physical id : 0
siblings : 4
core id : 1
cpu cores : 2
apicid : 2
initial apicid : 2
fpu : yes
fpu_exception : yes
cpuid level : 13
wp : yes
flags : fpu vme de pse tsc msr pae mce cx8 apic sep mtrr pge mca cmov pat pse36 clflush dts acpi mmx fxsr sse sse2 ss ht tm pbe syscall nx rdtscp lm constant_tsc arch_perfmon pebs bts rep_good nopl xtopology nonstop_tsc cpuid aperfmperf pni pclmulqdq dtes64 monitor ds_cpl vmx smx est tm2 ssse3 cx16 xtpr pdcm pcid sse4_1 sse4_2 x2apic popcnt tsc_deadline_timer aes xsave avx f16c rdrand lahf_lm cpuid_fault epb pti ssbd ibrs ibpb stibp tpr_shadow vnmi flexpriority ept vpid fsgsbase smep erms xsaveopt dtherm ida arat pln pts flush_l1d
bugs : cpu_meltdown spectre_v1 spectre_v2 spec_store_bypass l1tf
bogomips : 5980.77
clflush size : 64
cache_alignment : 64
address sizes : 36 bits physical, 48 bits virtual
power management:
processor : 2
vendor_id : GenuineIntel
cpu family : 6
model : 58
model name : Intel(R) Core(TM) i7-3540M CPU @ 3.00GHz
stepping : 9
microcode : 0x20
cpu MHz : 1281.416
cache size : 4096 KB
physical id : 0
siblings : 4
core id : 0
cpu cores : 2
apicid : 1
initial apicid : 1
fpu : yes
fpu_exception : yes
cpuid level : 13
wp : yes
flags : fpu vme de pse tsc msr pae mce cx8 apic sep mtrr pge mca cmov pat pse36 clflush dts acpi mmx fxsr sse sse2 ss ht tm pbe syscall nx rdtscp lm constant_tsc arch_perfmon pebs bts rep_good nopl xtopology nonstop_tsc cpuid aperfmperf pni pclmulqdq dtes64 monitor ds_cpl vmx smx est tm2 ssse3 cx16 xtpr pdcm pcid sse4_1 sse4_2 x2apic popcnt tsc_deadline_timer aes xsave avx f16c rdrand lahf_lm cpuid_fault epb pti ssbd ibrs ibpb stibp tpr_shadow vnmi flexpriority ept vpid fsgsbase smep erms xsaveopt dtherm ida arat pln pts flush_l1d
bugs : cpu_meltdown spectre_v1 spectre_v2 spec_store_bypass l1tf
bogomips : 5980.77
clflush size : 64
cache_alignment : 64
address sizes : 36 bits physical, 48 bits virtual
power management:
processor : 3
vendor_id : GenuineIntel
cpu family : 6
model : 58
model name : Intel(R) Core(TM) i7-3540M CPU @ 3.00GHz
stepping : 9
microcode : 0x20
cpu MHz : 1344.030
cache size : 4096 KB
physical id : 0
siblings : 4
core id : 1
cpu cores : 2
apicid : 3
initial apicid : 3
fpu : yes
fpu_exception : yes
cpuid level : 13
wp : yes
flags : fpu vme de pse tsc msr pae mce cx8 apic sep mtrr pge mca cmov pat pse36 clflush dts acpi mmx fxsr sse sse2 ss ht tm pbe syscall nx rdtscp lm constant_tsc arch_perfmon pebs bts rep_good nopl xtopology nonstop_tsc cpuid aperfmperf pni pclmulqdq dtes64 monitor ds_cpl vmx smx est tm2 ssse3 cx16 xtpr pdcm pcid sse4_1 sse4_2 x2apic popcnt tsc_deadline_timer aes xsave avx f16c rdrand lahf_lm cpuid_fault epb pti ssbd ibrs ibpb stibp tpr_shadow vnmi flexpriority ept vpid fsgsbase smep erms xsaveopt dtherm ida arat pln pts flush_l1d
bugs : cpu_meltdown spectre_v1 spectre_v2 spec_store_bypass l1tf
bogomips : 5980.77
clflush size : 64
cache_alignment : 64
address sizes : 36 bits physical, 48 bits virtual
power management:
0:00:00.786844
Number nodes: 16
0:00:00.114218
Number nodes: 4
0:00:01.780934
Number nodes: 21
0:00:01.560022
Number nodes: 42
0:00:00.413963
Number nodes: 7
0:00:01.826668
Number nodes: 25
0:00:25.316482
Number nodes: 13
0:00:06.767772
Number nodes: 59
0:00:00.783896
Number nodes: 24
0:00:02.653402
Number nodes: 64
0:00:00.112649
Number nodes: 4
0:00:07.316409
Number nodes: 137
0:00:07.609740
Number nodes: 56
0:00:01.232759
Number nodes: 21
0:00:01.351558
Number nodes: 30
0:00:04.126923
Number nodes: 68
0:00:00.642520
Number nodes: 16
0:00:00.732020
Number nodes: 20
0:00:00.784013
Number nodes: 21
0:00:00.364944
Number nodes: 10
Average time: 3.3138868
Average number nodes: 32.9
#/usr/bin/env python
from hpp.corbaserver.hrp2 import Robot
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver.wholebody_step.client import Client as WsClient, Problem
from math import pi
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
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints ("balance", q0, robot.leftAnkle, robot.rightAnkle,"",Problem.SLIDING)
ps = ProblemSolver (robot)
ps.addNumericalConstraints ("balance", ["balance/relative-com",
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-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)
lockedJoints.append (ljName)
lockedjointDict = robot.rightHandClosed ()
for name, value in lockedjointDict.iteritems ():
ljName = "locked_" + name
ps.createLockedJoint (ljName, name, value)
lockedJoints.append (ljName)
ps.setLockedJointConstraints ("locked-hands", lockedJoints)
q1 = [0.0, 0.0, 0.705, 0., 0., 0., 1.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q1)
if res [0]:
q1proj = res [1]
else:
raise RuntimeError ("Failed to apply constraint.")
q2 = [0.0, 0.0, 0.705, 0., 0., 0., 1., 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
res = ps.applyConstraints (q2)
if res[0]:
q2proj = res[1]
else:
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)
#v = vf.createViewer()
from hpp.gepetto import PathPlayer
#pp = PathPlayer (v, robot.client)
cat /proc/meminfo
MemTotal: 8039160 kB
MemFree: 4244932 kB
MemAvailable: 6139152 kB
Buffers: 382624 kB
Cached: 1862488 kB
SwapCached: 0 kB
Active: 1857376 kB
Inactive: 1468072 kB
Active(anon): 1081600 kB
Inactive(anon): 324240 kB
Active(file): 775776 kB
Inactive(file): 1143832 kB
Unevictable: 48 kB
Mlocked: 48 kB
SwapTotal: 8258044 kB
SwapFree: 8258044 kB
Dirty: 1408 kB
Writeback: 0 kB
AnonPages: 1080380 kB
Mapped: 463812 kB
Shmem: 325508 kB
Slab: 357052 kB
SReclaimable: 279396 kB
SUnreclaim: 77656 kB
KernelStack: 9216 kB
PageTables: 34712 kB
NFS_Unstable: 0 kB
Bounce: 0 kB
WritebackTmp: 0 kB
CommitLimit: 12277624 kB
Committed_AS: 5344864 kB
VmallocTotal: 34359738367 kB
VmallocUsed: 0 kB
VmallocChunk: 0 kB
HardwareCorrupted: 0 kB
AnonHugePages: 0 kB
ShmemHugePages: 0 kB
ShmemPmdMapped: 0 kB
CmaTotal: 0 kB
CmaFree: 0 kB
HugePages_Total: 0
HugePages_Free: 0
HugePages_Rsvd: 0
HugePages_Surp: 0
Hugepagesize: 2048 kB
DirectMap4k: 157444 kB
DirectMap2M: 8101888 kB
0:00:01.168308
Number nodes: 17
0:00:01.553133
Number nodes: 25
0:00:01.798499
Number nodes: 35
0:00:01.687451
Number nodes: 24
0:00:08.570912
Number nodes: 132
0:00:01.759673
Number nodes: 27
0:00:02.849294
Number nodes: 43
0:00:02.727454
Number nodes: 46
0:00:01.170955
Number nodes: 20
0:00:09.059615
Number nodes: 140
0:00:03.746288
Number nodes: 61
0:00:01.318103
Number nodes: 21
0:00:02.181730
Number nodes: 34
0:00:01.349001
Number nodes: 19
0:00:02.325417
Number nodes: 38
0:00:01.585475
Number nodes: 28
0:00:02.689942
Number nodes: 41
0:00:07.998327
Number nodes: 120
0:00:01.595305
Number nodes: 28
0:00:03.605356
Number nodes: 51
Average time: 3.0370119
Average number nodes: 47.5
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")
ps.selectPathValidation ("Progressive", 0.025)
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)
ps.addGoalConfig (q_goal)
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)))
#v = vf.createViewer(); v (q_init)
#pp = PathPlayer (v, robot.client)
0:00:16.800932
Number nodes: 40
0:01:20.471142
Number nodes: 205
0:00:22.417580
Number nodes: 81
0:00:31.307471
Number nodes: 104
0:00:35.785126
Number nodes: 100
0:00:35.276953
Number nodes: 80
0:00:24.434607
Number nodes: 94
0:00:44.762525
Number nodes: 162
0:01:42.678984
Number nodes: 220
0:00:25.849480
Number nodes: 95
0:00:16.393529
Number nodes: 59
0:00:18.859415
Number nodes: 73
0:00:39.811198
Number nodes: 95
0:00:30.871283
Number nodes: 85
0:00:27.387882
Number nodes: 106
0:00:49.219097
Number nodes: 121
0:01:16.403090
Number nodes: 253
0:00:16.679828
Number nodes: 78
0:00:54.516062
Number nodes: 211
0:00:58.939297
Number nodes: 174
Number of successes: 20
Average time: 40.44327405
Average number nodes: 121.8
\ No newline at end of file
# Import libraries and load robots. {{{1
# Import. {{{2
from math import sqrt
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, \
ConstraintGraphFactory, Constraints, Rule
from hpp.gepetto.manipulation import ViewerFactory
from math import pi
# 2}}}
# Load PR2 and a box to be manipulated. {{{2
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createRealClient ()
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7,-2,2,-2,2] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-2,2,-2,2,-2,2,-2,2])
# 2}}}
# 2}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
robot.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
# 2}}}
# Create lists of joint names - useful to define passive joints. {{{2
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)
ps.addPassiveDofs ('pr2', jointNames['pr2'])
# 2}}}
# Generate initial and goal configuration. {{{2
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
q_init [rank] = 0.5
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
rank = robot.rankInConfiguration ['box/root_joint']
q_init [rank:rank+7] = [-2.5, -4, 0.9, 0, sqrt (2)/2, 0, sqrt (2)/2]
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['pr2/l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['pr2/r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['box/root_joint']
q_goal [rank:rank+7] = [-4.8, -4.8, 0.9, 0, sqrt (2)/2, 0, sqrt (2)/2]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [0, 0, 0, 1]
# 2}}}
# Build rules
rules = [Rule (['pr2/l_gripper'], ['box/handle'], True),
Rule ([""], [""], True)]
locklhand = ['l_l_finger','l_r_finger'];
lockrhand = ['r_l_finger','r_r_finger'];
ps.createLockedJoint ('l_l_finger', 'pr2/l_gripper_l_finger_joint', [0.5,])
ps.createLockedJoint ('l_r_finger', 'pr2/l_gripper_r_finger_joint', [0.5,])
grippers = ['pr2/l_gripper']
boxes = ['box']
handlesPerObject = [['box/handle']]
envSurfaces = ['kitchen_area/pancake_table_table_top',
'kitchen_area/white_counter_top_sink']
objContactSurfaces = [['box/box_surface']]
#envSurfaces = []
#objContactSurfaces = [[]]
# Get the built graph
cg = ConstraintGraph (robot, 'graph')
factory = ConstraintGraphFactory (cg)
factory.setGrippers (grippers)
factory.environmentContacts (envSurfaces)
factory.setObjects (boxes, handlesPerObject, objContactSurfaces)
factory.setRules (rules)
factory.generate ()
cg.addConstraints (graph = True, constraints =\
Constraints (lockedJoints = locklhand))
cg.setWeight ('Loop | f', 1)
cg.setWeight ('Loop | 0-0', 1)
cg.initialize ()
res, q_init, err = cg.applyNodeConstraints ('free', q_init)
if not res:
raise RuntimeError ("Failed to project initial configuration")
res, q_goal, err = cg.applyNodeConstraints ('free', q_goal)
if not res:
raise RuntimeError ("Failed to project initial configuration")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.setMaxIterPathPlanning (5000)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20; success = 0
for i in range (N):
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
try:
t1 = dt.datetime.now ()
ps.solve ()
success += 1
t2 = dt.datetime.now ()
totalTime += t2 - t1
print (t2-t1)
n = ps.numberNodes ()
totalNumberNodes += n
print ("Number nodes: " + str(n))
except:
print ("Failed to plan path.")
print ("Number of successes: " + str (success))
print ("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (success)))
print ("Average number nodes: " + str (totalNumberNodes/float (success)))
# 1}}}
from hpp.gepetto import PathPlayer
#v = vf.createViewer (); v (q_init)
#pp = PathPlayer (v, robot.client.basic)
# vim: foldmethod=marker foldlevel=1
0:00:07.187740
Number nodes: 29
0:00:16.232692
Number nodes: 95
0:00:09.148962
Number nodes: 39
0:00:05.083352
Number nodes: 25
0:00:07.504551
Number nodes: 37
0:00:15.886397
Number nodes: 90
0:00:03.053300
Number nodes: 10
0:00:07.430992
Number nodes: 39
0:00:09.749302
Number nodes: 50
0:00:10.283026
Number nodes: 22
0:00:10.786961
Number nodes: 43
0:00:08.340896
Number nodes: 40
0:00:04.199240
Number nodes: 17
0:00:03.184707
Number nodes: 10
0:00:20.288661
Number nodes: 74
0:00:15.810468
Number nodes: 86
0:00:07.054555
Number nodes: 20
0:00:04.314506
Number nodes: 14
0:00:05.075484
Number nodes: 20
0:00:04.636699
Number nodes: 17
Average time: 8.76262455
Average number nodes: 38.85
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.pr2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, Constraints, \
ConstraintGraph, ConstraintGraphFactory, Rule
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer
from math import sqrt
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot ('pr2-box', 'pr2') # was anchor joint
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
q_init[0:4] = [-3.2,-4,1,0] # FIX ME ! (see up )
rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/torso_lift_joint']
q_init [rank] = 0.2
q_goal = q_init [::]
rank = robot.rankInConfiguration ['box/root_joint']
c = sqrt (2) / 2
#q_init [rank:rank+4] = [c, 0, c, 0]
q_init [rank:rank+7] = [-2.5, -3.6, 0.76,0,c,0,c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
rank = robot.rankInConfiguration ['box/root_joint']
q_goal [rank:rank+7] = [-2.5, -4.4, 0.76,0,-c,0,c]
#rank = robot.rankInConfiguration ['box/base_joint_SO3']
#q_goal [rank:rank+4] = [c, 0, -c, 0]
del c
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterProjection (40)
ps.selectPathProjector ('Progressive', 0.2)
# Create constraints. {{{3
robot.client.manipulation.problem.createPlacementConstraint \
('box_placement', ['box/box_surface',],
['kitchen_area/pancake_table_table_top',])
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
if not n.startswith ("pr2/l_gripper"):
jointNames['allButPR2LeftArm'].append (n)