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
9b77c6ed
Commit
9b77c6ed
authored
Mar 06, 2015
by
Florent Lamiraux
Browse files
Merge remote-tracking branch 'origin/joseph'
parents
5fed9624
aabfe903
Changes
4
Hide whitespace changes
Inline
Side-by-side
cmake
@
59b1f75f
Compare
e31b581c
...
59b1f75f
Subproject commit
e31b581cd45f102
d2
b
9b
28e182f096e4b51696a7
Subproject commit
59b1f75fc529cc5474c4154
d29b
1a7e28f1feee3
script/
test
_constraints.py
→
script/
balance
_constraints
_success_statistics
.py
View file @
9b77c6ed
...
...
@@ -22,8 +22,7 @@ balanceConstraints = [constraintName + "/relative-com",
constraintName
+
"/orientation-left-foot"
,
constraintName
+
"/position-left-foot"
]
for
a
in
[
"x"
,
"y"
,
"z"
]:
robot
.
setJointBounds
(
"base_joint_"
+
a
,
[
-
4
,
4
])
robot
.
setJointBounds
(
"base_joint_xyz"
,
[
-
4
,
4
,
-
4
,
4
,
-
4
,
4
])
def
testConstraint
(
constraints
,
nbIter
=
100
):
success
=
0
...
...
script/test_graph_two_hands.py
View file @
9b77c6ed
# vim: foldmethod=marker foldlevel=2
from
hpp.corbaserver.manipulation.pr2
import
Robot
from
hpp.corbaserver.manipulation
import
ProblemSolver
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'
,
rootJointType
=
"anchor"
)
robot
=
Robot
(
'pr2-box'
,
'pr2'
,
rootJointType
=
"anchor"
)
ps
=
ProblemSolver
(
robot
)
r
=
ViewerFactory
(
ps
)
robot
.
client
.
manipulation
.
robot
.
setRootJointPosition
(
'pr2'
,
(
-
3.2
,
-
4
,
0
,
1
,
0
,
0
,
0
))
r
obot
.
loadObjectModel
(
'box'
,
'freeflyer'
,
'hpp_tutorial'
,
'box'
,
''
,
'
'
)
r
obot
.
buildCompositeRobot
(
'pr2-box'
,
[
'pr2'
,
'box'
])
r
obot
.
client
.
manipulation
.
robot
.
loadEnvironmentModel
(
"iai_maps"
,
"kitchen_area"
,
''
,
''
,
""
)
r
.
loadObjectModel
(
Box
,
'box
'
)
r
.
buildCompositeRobot
([
'pr2'
,
'box'
])
r
.
loadEnvironmentModel
(
Environment
,
"kitchen_area"
)
robot
.
setJointBounds
(
"box/base_joint_xyz"
,
[
-
3
,
-
2
,
-
5
,
-
3
,
0.7
,
1
])
# 3}}}
from
hpp_ros.manipulation
import
ScenePublisher
r
=
ScenePublisher
(
robot
)
# Define configurations. {{{3
q_init
=
robot
.
getCurrentConfig
()
rank
=
robot
.
rankInConfiguration
[
'pr2/r_gripper_l_finger_joint'
]
...
...
@@ -33,13 +51,11 @@ q_init [rank:rank+3] = [-2.5, -3.6, 0.76]
rank
=
robot
.
rankInConfiguration
[
'box/base_joint_SO3'
]
c
=
sqrt
(
2
)
/
2
q_init
[
rank
:
rank
+
4
]
=
[
c
,
0
,
c
,
0
]
r
(
q_init
)
rank
=
robot
.
rankInConfiguration
[
'box/base_joint_xyz'
]
q_goal
[
rank
:
rank
+
3
]
=
[
-
2.5
,
-
4.4
,
0.76
]
rank
=
robot
.
rankInConfiguration
[
'box/base_joint_SO3'
]
q_goal
[
rank
:
rank
+
4
]
=
[
c
,
0
,
-
c
,
0
]
r
(
q_goal
)
del
c
q_inter
=
[
-
0.8017105239677402
,
-
0.5977125025958312
,
-
0.796440524800078
,
0.6047168680102916
,
-
0.4879605145323316
,
0.8728657034488995
,
-
0.988715265911429
,
0.1498069522875767
,
-
0.012487804646172175
,
-
0.9999220243274567
,
-
0.9479271854727237
,
-
0.31848712853388694
,
0.06700549180802896
,
-
0.9977526066453368
,
0.15793164217459785
,
0.9874500475467277
,
0.9804271799015071
,
-
0.19688205837601827
,
0.06981400674906149
,
0.9975600254930236
,
0.8026666074307995
,
-
0.5964279649006498
,
-
0.8558688410761539
,
-
0.5171929300318802
,
0.07633365848037467
,
2.5514381844999448
,
1.1951774265118278
,
-
0.5864281075389233
,
0.24807300661555917
,
1.0730239901832896
,
-
0.9931474461781542
,
0.5380253563010143
,
-
0.8429286541440898
,
0.0
,
-
0.9291311234626017
,
0.36975039609596555
,
0.5
,
0.07192830903452277
,
0.0516497980242827
,
0.5
,
0.2978673015357309
,
0.011873305063635719
,
0.2207828272342602
,
0.9968680838221816
,
-
1.1330407965502385
,
0.1474961939381539
,
-
0.9059397450606351
,
-
0.9591666722669869
,
0.8241613711518598
,
-
0.5663550426199861
,
-
2.094
,
0.7924979452316735
,
0.6098745828476339
,
0.5
,
0.35889873246983567
,
0.10845342945887403
,
0.5
,
0.02916333341652683
,
0.025597731231524482
,
0.16145134862579935
,
-
2.785939904956431
,
-
4.563075760833335
,
0.8958690128585236
,
-
0.19634763254425533
,
-
0.7205092487114027
,
0.6650461296003519
,
0.005260724207565836
]
...
...
@@ -49,16 +65,14 @@ robot.client.basic.problem.resetRoadmap ()
robot
.
client
.
basic
.
problem
.
selectPathOptimizer
(
'None'
)
robot
.
client
.
basic
.
problem
.
setErrorThreshold
(
1e-3
)
robot
.
client
.
basic
.
problem
.
setMaxIterations
(
40
)
from
hpp.corbaserver.manipulation
import
ProblemSolver
p
=
ProblemSolver
(
robot
)
ps
.
selectPathProjector
(
'Progressive'
,
0.2
)
# Create constraints. {{{3
from
hpp.corbaserver.manipulation
import
ConstraintGraph
cg
=
ConstraintGraph
(
robot
,
'graph'
)
robot
.
client
.
manipulation
.
problem
.
createPlacementConstraint
(
'box_placement'
,
'box'
,
'box/base_joint_SO3'
,
'box_surface'
,
'pancake_table_table_top'
)
'box_placement'
,
'box'
,
'box/base_joint_SO3'
,
'box_surface'
,
'
kitchen_area/
pancake_table_table_top'
)
jointNames
=
dict
()
jointNames
[
'all'
]
=
robot
.
getJointNames
()
...
...
@@ -69,23 +83,24 @@ for n in jointNames['all']:
jointNames
[
'pr2'
].
append
(
n
)
if
not
n
.
startswith
(
"pr2/l_gripper"
):
jointNames
[
'allButPR2LeftArm'
].
append
(
n
)
ps
.
addPassiveDofs
(
'pr2'
,
jointNames
[
'pr2'
])
cg
.
createGrasp
(
'l_grasp'
,
'pr2/l_gripper'
,
'box/handle'
,
jointNames
[
'pr2'
]
)
cg
.
createGrasp
(
'l_grasp'
,
'pr2/l_gripper'
,
'box/handle'
,
'pr2'
)
cg
.
createGrasp
(
'r_grasp'
,
'pr2/r_gripper'
,
'box/handle2'
,
jointNames
[
'pr2'
]
)
cg
.
createGrasp
(
'r_grasp'
,
'pr2/r_gripper'
,
'box/handle2'
,
'pr2'
)
cg
.
createPreGrasp
(
'l_pregrasp'
,
'pr2/l_gripper'
,
'box/handle'
)
cg
.
createPreGrasp
(
'r_pregrasp'
,
'pr2/r_gripper'
,
'box/handle2'
)
lockbox
=
p
.
lockFreeFlyerJoint
(
'box/base_joint'
,
'box_lock'
,
parametric
=
True
)
lockbox
=
p
s
.
lockFreeFlyerJoint
(
'box/base_joint'
,
'box_lock'
,
compType
=
'Equality'
)
locklhand
=
[
'l_l_finger'
,
'l_r_finger'
];
p
.
createLocked
DofConstra
int
(
'l_l_finger'
,
'pr2/l_gripper_l_finger_joint'
,
0.5
,
0
,
0
)
p
.
createLocked
DofConstra
int
(
'l_r_finger'
,
'pr2/l_gripper_r_finger_joint'
,
0.5
,
0
,
0
)
p
s
.
createLocked
Jo
int
(
'l_l_finger'
,
'pr2/l_gripper_l_finger_joint'
,
[
0.5
]
)
p
s
.
createLocked
Jo
int
(
'l_r_finger'
,
'pr2/l_gripper_r_finger_joint'
,
[
0.5
]
)
lockrhand
=
[
'r_l_finger'
,
'r_r_finger'
];
p
.
createLocked
DofConstra
int
(
'r_l_finger'
,
'pr2/r_gripper_l_finger_joint'
,
0.5
,
0
,
0
)
p
.
createLocked
DofConstra
int
(
'r_r_finger'
,
'pr2/r_gripper_r_finger_joint'
,
0.5
,
0
,
0
)
p
s
.
createLocked
Jo
int
(
'r_l_finger'
,
'pr2/r_gripper_l_finger_joint'
,
[
0.5
]
)
p
s
.
createLocked
Jo
int
(
'r_r_finger'
,
'pr2/r_gripper_r_finger_joint'
,
[
0.5
]
)
lockhands
=
[
'l_l_finger'
,
'l_r_finger'
,
'r_l_finger'
,
'r_r_finger'
];
...
...
@@ -173,13 +188,13 @@ cg.client.graph.setLevelSetConstraints (cg.edges['r_keep_grasp_ls'], [], lockbox
cg
.
setConstraints
(
graph
=
True
,
lockDof
=
lockhands
)
# 3}}}
res
=
p
.
client
.
manipulation
.
problem
.
applyConstraints
(
cg
.
nodes
[
'free'
],
q_init
)
res
=
p
s
.
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
=
p
.
client
.
manipulation
.
problem
.
applyConstraints
(
cg
.
nodes
[
'free'
],
q_goal
)
res
=
p
s
.
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
]
p
.
setInitialConfig
(
q_init_proj
)
p
.
addGoalConfig
(
q_goal_proj
)
p
s
.
setInitialConfig
(
q_init_proj
)
p
s
.
addGoalConfig
(
q_goal_proj
)
script/test_simple_graph.py
View file @
9b77c6ed
...
...
@@ -60,19 +60,20 @@ for n in jointNames['all']:
graph
.
createGrasp
(
'l_grasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
,
jointNames
[
'hrp2'
])
graph
.
createPreGrasp
(
'l_pregrasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
)
lockscrewgun
=
ps
.
lockFreeFlyerJoint
(
'screw_gun/base_joint'
,
'screwgun_lock'
)
lockscrewgun
=
ps
.
lockFreeFlyerJoint
(
'screw_gun/base_joint'
,
'screwgun_lock'
,
compType
=
'Equality'
)
locklhand
=
[
'larm_6'
,
'lhand_0'
,
'lhand_1'
,
'lhand_2'
,
'lhand_3'
,
'lhand_4'
]
ps
.
createLockedJoint
Constraint
(
'larm_6'
,
'hrp2/LARM_JOINT6'
,
[
q_init
[
ilh
],])
ps
.
createLockedJoint
Constraint
\
ps
.
createLockedJoint
(
'larm_6'
,
'hrp2/LARM_JOINT6'
,
[
q_init
[
ilh
],])
ps
.
createLockedJoint
\
(
'lhand_0'
,
'hrp2/LHAND_JOINT0'
,
[
q_init
[
ilh
+
1
],])
ps
.
createLockedJoint
Constraint
\
ps
.
createLockedJoint
\
(
'lhand_1'
,
'hrp2/LHAND_JOINT1'
,
[
q_init
[
ilh
+
2
],])
ps
.
createLockedJoint
Constraint
\
ps
.
createLockedJoint
\
(
'lhand_2'
,
'hrp2/LHAND_JOINT2'
,
[
q_init
[
ilh
+
3
],])
ps
.
createLockedJoint
Constraint
\
ps
.
createLockedJoint
\
(
'lhand_3'
,
'hrp2/LHAND_JOINT3'
,
[
q_init
[
ilh
+
4
],])
ps
.
createLockedJoint
Constraint
\
ps
.
createLockedJoint
\
(
'lhand_4'
,
'hrp2/LHAND_JOINT4'
,
[
q_init
[
ilh
+
5
],])
ps
.
createStaticStabilityConstraints
(
"balance"
,
q_init
)
...
...
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