Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
test-hpp
Commits
890fc317
Commit
890fc317
authored
Sep 25, 2014
by
Joseph Mirabel
Committed by
Joseph Mirabel
Nov 18, 2014
Browse files
Update script using the constraint graph
parent
06867178
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/test_graph.py
View file @
890fc317
...
...
@@ -2,7 +2,6 @@ from hpp.corbaserver.manipulation.hrp2 import Robot
from
hpp.corbaserver.manipulation
import
ProblemSolver
from
scene_publisher3b
import
ScenePublisher
as
MultiRobotPub
#Robot.urdfSuffix = '_capsule_mesh_hands_fixed'
Robot
.
urdfSuffix
=
'_capsule_mesh'
Robot
.
srdfSuffix
=
'_mathieu'
...
...
script/test_simple_graph.py
View file @
890fc317
...
...
@@ -30,6 +30,7 @@ qinit=[6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.
p
=
ProblemSolver
(
robot
)
p
.
createGrasp
(
'left-hand-grasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
)
p
.
createGrasp
(
'left-hand-grasp-passive'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
)
p
.
createPreGrasp
(
'pregrasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
)
rankcfg
=
1
;
rankvel
=
0
;
lockscrewgun
=
list
();
for
axis
in
[
'x'
,
'y'
,
'z'
]:
p
.
createLockedDofConstraint
(
'screwgun_lock_'
+
axis
,
'screw_gun/base_joint_'
+
axis
,
0
,
0
,
0
)
...
...
@@ -109,6 +110,8 @@ id["free" ] = graph.createNode (id["subgraph"], 'free')
id
[
"ungrasp"
]
=
graph
.
createEdge
(
id
[
"screwgun"
],
id
[
"free"
],
"ungrasp"
,
1
,
False
)
id
[
"grasp"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"screwgun"
],
"grasp"
,
10
,
True
)
id
[
"grasp_w"
],
id
[
"grasp_w_node"
]
=
graph
.
addWaypoint
(
id
[
"grasp"
],
"grasp_w"
)
id
[
"move_free"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"free"
],
"move_free"
,
1
,
False
)
id
[
"keep_grasp"
]
=
graph
.
createEdge
(
id
[
"screwgun"
],
id
[
"screwgun"
],
"keep_grasp"
,
10
,
False
)
...
...
@@ -117,8 +120,8 @@ graph.setNumericalConstraintsForPath (id["screwgun"], ['left-hand-grasp-passive'
graph
.
setLockedDofConstraints
(
id
[
"move_free"
],
lockscrewgun
)
graph
.
setLockedDofConstraints
(
id
[
"grasp"
],
lockscrewgun
)
graph
.
setLockedDofConstraints
(
id
[
"ungrasp"
],
lockscrewgun
)
#
graph.set
LockedDof
Constraints (id["grasp
"], sum([lockscrewgun, lockbottompart],[])
)
#
graph.setLockedDofConstraints (id["
un
grasp"],
sum([
lockscrewgun
, lockbottompart],[])
)
graph
.
set
Numerical
Constraints
(
id
[
"grasp
_w_node"
],
[
'pregrasp'
,
'pregrasp/ineq_0'
,
'pregrasp/ineq_0.1'
]
)
graph
.
setLockedDofConstraints
(
id
[
"grasp
_w
"
],
lockscrewgun
)
graph
.
setNumericalConstraints
(
id
[
"graph"
],
p
.
balanceConstraints
())
graph
.
setLockedDofConstraints
(
id
[
"graph"
],
locklhand
)
...
...
@@ -128,4 +131,4 @@ p.setInitialConfig (q1)
p
.
addGoalConfig
(
q2
)
# This projector tends to fail with probability 0.6 (with random configuration)
manip
.
graph
.
statOnConstraint
(
[
id
[
"grasp"
]
]
)
manip
.
graph
.
statOnConstraint
(
id
[
"grasp"
])
script/test_ur5.py
View file @
890fc317
...
...
@@ -6,7 +6,7 @@ Robot.packageName = "fiad_description"
Robot
.
urdfName
=
"ur5"
Robot
.
urdfSuffix
=
''
Robot
.
srdfSuffix
=
'_ee'
with
Pregrasp
=
True
with
Waypoint
=
True
robot
=
Robot
(
'ur5'
)
robot
.
client
.
basic
.
problem
.
selectPathPlanner
(
"M-RRT"
)
...
...
@@ -16,7 +16,6 @@ for a in ["x","y","z"]:
robot
.
setJointBounds
(
"box/base_joint_"
+
a
,
[
-
1
,
1
])
robot
.
client
.
basic
.
problem
.
resetRoadmap
()
robot
.
client
.
basic
.
problem
.
selectPathOptimizer
(
'None'
)
#robot.client.basic.problem.setErrorThreshold (1e-3)
robot
.
client
.
basic
.
problem
.
setMaxIterations
(
40
)
r
=
MultiRobotPub
(
robot
)
...
...
@@ -28,7 +27,7 @@ qgoal=[0,0,0,0,0,0,0,-0.5,0,1,0,0,0]
p
=
ProblemSolver
(
robot
)
p
.
createGrasp
(
'grasp'
,
'ur5/ee'
,
'box/handle'
)
p
.
createGrasp
(
'grasp-passive'
,
'ur5/ee'
,
'box/handle'
)
if
with
Pregrasp
:
if
with
Waypoint
:
p
.
createPreGrasp
(
'pregrasp'
,
'ur5/ee'
,
'box/handle'
)
p
.
createLockedDofConstraint
(
'box_lock_x'
,
'box/base_joint_x'
,
0
,
0
,
0
)
p
.
createLockedDofConstraint
(
'box_lock_y'
,
'box/base_joint_y'
,
0
,
0
,
0
)
...
...
@@ -56,48 +55,30 @@ graph = robot.client.manipulation.graph
id
=
dict
()
id
[
"graph"
]
=
graph
.
createGraph
(
'ur5-box'
)
id
[
"subgraph"
]
=
graph
.
createSubGraph
(
'ee'
)
if
withPregrasp
:
id
[
"box"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'box'
)
id
[
"prebox"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'prebox'
)
id
[
"free"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'free'
)
else
:
id
[
"box"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'box'
)
id
[
"free"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'free'
)
id
[
"box"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'box'
)
id
[
"free"
]
=
graph
.
createNode
(
id
[
"subgraph"
],
'free'
)
if
withPregrasp
:
id
[
"unpregrasp"
]
=
graph
.
createEdge
(
id
[
"prebox"
],
id
[
"free"
],
"unpregrasp"
,
1
,
False
)
id
[
"pregrasp"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"prebox"
],
"pregrasp"
,
10
,
True
)
id
[
"ungrasp"
]
=
graph
.
createEdge
(
id
[
"box"
],
id
[
"prebox"
],
"ungrasp"
,
1
,
False
)
id
[
"grasp"
]
=
graph
.
createEdge
(
id
[
"prebox"
],
id
[
"box"
],
"grasp"
,
10
,
True
)
id
[
"ungrasp"
]
=
graph
.
createEdge
(
id
[
"box"
],
id
[
"free"
],
"ungrasp"
,
1
,
False
)
id
[
"grasp"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"box"
],
"grasp"
,
10
,
True
)
id
[
"move_free"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"free"
],
"move_free"
,
1
,
False
)
id
[
"keep_grasp"
]
=
graph
.
createEdge
(
id
[
"box"
],
id
[
"box"
],
"keep_grasp"
,
1
,
False
)
id
[
"keep_align"
]
=
graph
.
createEdge
(
id
[
"prebox"
],
id
[
"prebox"
],
"keep_align"
,
0
,
False
)
else
:
id
[
"ungrasp"
]
=
graph
.
createEdge
(
id
[
"box"
],
id
[
"free"
],
"ungrasp"
,
1
,
False
)
id
[
"grasp"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"box"
],
"grasp"
,
1
,
True
)
id
[
"move_free"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"free"
],
"move_free"
,
1
,
False
)
id
[
"keep_grasp"
]
=
graph
.
createEdge
(
id
[
"box"
],
id
[
"box"
],
"keep_grasp"
,
1
,
False
)
id
[
"move_free"
]
=
graph
.
createEdge
(
id
[
"free"
],
id
[
"free"
],
"move_free"
,
1
,
False
)
id
[
"
keep_grasp"
]
=
graph
.
createEdge
(
id
[
"box"
],
id
[
"box"
],
"keep_grasp"
,
1
,
False
)
if
withWaypoint
:
id
[
"
grasp_w"
],
id
[
"grasp_w_node"
]
=
graph
.
addWaypoint
(
id
[
"grasp"
],
"grasp_w"
)
graph
.
setNumericalConstraints
(
id
[
"box"
],
[
'grasp'
])
graph
.
setNumericalConstraintsForPath
(
id
[
"box"
],
[
'grasp-passive'
])
graph
.
setLockedDofConstraints
(
id
[
"move_free"
],
lockbox
)
graph
.
setLockedDofConstraints
(
id
[
"grasp"
],
lockbox
)
graph
.
setLockedDofConstraints
(
id
[
"ungrasp"
],
lockbox
)
if
withPregrasp
:
graph
.
setNumericalConstraints
(
id
[
"prebox"
],
[
'pregrasp'
,
'pregrasp/ineq_0'
,
'pregrasp/ineq_0.1'
])
#graph.setNumericalConstraints (id["prebox"], ['pregrasp'])
graph
.
setNumericalConstraintsForPath
(
id
[
"prebox"
],
[
'pregrasp'
])
graph
.
setLockedDofConstraints
(
id
[
"keep_align"
],
lockbox
)
graph
.
setLockedDofConstraints
(
id
[
"pregrasp"
],
lockbox
)
graph
.
setLockedDofConstraints
(
id
[
"unpregrasp"
],
lockbox
)
if
withWaypoint
:
graph
.
setNumericalConstraints
(
id
[
"grasp_w_node"
],
[
'pregrasp'
,
'pregrasp/ineq_0'
,
'pregrasp/ineq_0.1'
])
graph
.
setLockedDofConstraints
(
id
[
"grasp_w"
],
lockbox
)
manip
=
robot
.
client
.
manipulation
#p.setInitialConfig (qinit)
#p.addGoalConfig (qgoal)
p
.
setInitialConfig
(
qgoal
)
p
.
addGoalConfig
(
qinit
)
p
.
setInitialConfig
(
qinit
)
p
.
addGoalConfig
(
qgoal
)
manip
.
graph
.
statOnConstraint
(
id
[
"grasp"
])
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment