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
ae4ec8f2
Commit
ae4ec8f2
authored
Dec 28, 2014
by
Florent Lamiraux
Browse files
Update scripts to latest modifications in corba packages.
parent
1b626e56
Changes
2
Hide whitespace changes
Inline
Side-by-side
script/test_rrt.py
View file @
ae4ec8f2
#/usr/bin/env python
from
hpp
_ros
import
ScenePublish
er
,
PathPlayer
from
hpp
.gepetto
import
View
er
,
PathPlayer
from
hpp.corbaserver.hrp2
import
Robot
from
hpp.corbaserver
import
ProblemSolver
from
hpp.corbaserver.wholebody_step.client
import
Client
as
WsClient
...
...
@@ -10,9 +10,11 @@ Robot.srdfSuffix= '_capsule'
robot
=
Robot
(
'hrp2_14'
)
robot
.
setJointBounds
(
"base_joint_xyz"
,
[
-
3
,
3
,
-
3
,
3
,
0
,
1
])
ps
=
ProblemSolver
(
robot
)
cl
=
robot
.
client
r
=
ScenePublisher
(
robot
)
r
=
Viewer
(
ps
)
q0
=
robot
.
getInitialConfig
()
r
(
q0
)
...
...
@@ -21,7 +23,6 @@ wcl = WsClient ()
wcl
.
problem
.
addStaticStabilityConstraints
(
"balance"
,
q0
,
robot
.
leftAnkle
,
robot
.
rightAnkle
)
ps
=
ProblemSolver
(
robot
)
ps
.
setNumericalConstraints
(
"balance"
,
[
"balance/relative-com"
,
"balance/relative-orientation"
,
"balance/relative-position"
,
...
...
@@ -31,11 +32,11 @@ ps.setNumericalConstraints ("balance", ["balance/relative-com",
# lock hands in closed position
lockedDofs
=
robot
.
leftHandClosed
()
for
name
,
value
in
lockedDofs
.
iteritems
():
ps
.
lock
Joint
(
name
,
value
)
ps
.
lock
Dof
(
name
,
value
,
0
,
0
)
lockedDofs
=
robot
.
rightHandClosed
()
for
name
,
value
in
lockedDofs
.
iteritems
():
ps
.
lock
Joint
(
name
,
value
)
ps
.
lock
Dof
(
name
,
value
,
0
,
0
)
q1
=
[
0.0
,
0.0
,
0.705
,
1.0
,
0.
,
0.
,
0.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
]
...
...
script/test_simple_graph.py
View file @
ae4ec8f2
...
...
@@ -11,22 +11,22 @@ class ScrewGun (object):
urdfSuffix
=
"_massless"
srdfSuffix
=
""
Robot
.
urdfSuffix
=
''
Robot
.
urdfSuffix
=
'
_capsule_mesh
'
Robot
.
srdfSuffix
=
'_manipulation'
# Load HRP2 and a screwgun {{{3
robot
=
Robot
(
'hrp2-screw'
,
'hrp2'
)
robot
.
client
.
basic
.
problem
.
selectPathPlanner
(
"M-RRT"
)
r
=
Viewer
(
robot
)
ps
=
ProblemSolver
(
robot
)
ps
.
selectPathPlanner
(
"M-RRT"
)
r
=
Viewer
(
ps
)
r
.
loadObjectModel
(
ScrewGun
,
'screw_gun'
)
r
.
buildCompositeRobot
([
'hrp2'
,
'screw_gun'
])
for
d
in
[
"hrp2"
,
"screw_gun"
]:
robot
.
setJointBounds
(
d
+
"/base_joint_xyz"
,
[
-
4
,
4
,
-
4
,
4
,
-
4
,
4
])
robot
.
client
.
basic
.
problem
.
resetRoadmap
()
robot
.
client
.
basic
.
problem
.
selectPathOptimizer
(
'None'
)
robot
.
client
.
basic
.
problem
.
setErrorThreshold
(
1e-3
)
robot
.
client
.
basic
.
problem
.
setMaxIterations
(
40
)
ps
.
selectPathOptimizer
(
'None'
)
ps
.
setErrorThreshold
(
1e-3
)
ps
.
setMaxIterations
(
40
)
# 3}}}
# Define configurations {{{3
...
...
@@ -44,7 +44,6 @@ q_goal [-7:] = [2, -1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
# 3}}}
# Generate constraints {{{3
p
=
ProblemSolver
(
robot
)
graph
=
ConstraintGraph
(
robot
,
'graph'
)
...
...
@@ -61,17 +60,17 @@ 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
=
p
.
lockFreeFlyerJoint
(
'screw_gun/base_joint'
,
'screwgun_lock'
,
parametric
=
True
)
lockscrewgun
=
p
s
.
lockFreeFlyerJoint
(
'screw_gun/base_joint'
,
'screwgun_lock'
,
parametric
=
True
)
locklhand
=
[
'larm_6'
,
'lhand_0'
,
'lhand_1'
,
'lhand_2'
,
'lhand_3'
,
'lhand_4'
]
p
.
createLockedDofConstraint
(
'larm_6'
,
'hrp2/LARM_JOINT6'
,
q_init
[
ilh
],
0
,
0
)
p
.
createLockedDofConstraint
(
'lhand_0'
,
'hrp2/LHAND_JOINT0'
,
q_init
[
ilh
+
1
],
0
,
0
)
p
.
createLockedDofConstraint
(
'lhand_1'
,
'hrp2/LHAND_JOINT1'
,
q_init
[
ilh
+
2
],
0
,
0
)
p
.
createLockedDofConstraint
(
'lhand_2'
,
'hrp2/LHAND_JOINT2'
,
q_init
[
ilh
+
3
],
0
,
0
)
p
.
createLockedDofConstraint
(
'lhand_3'
,
'hrp2/LHAND_JOINT3'
,
q_init
[
ilh
+
4
],
0
,
0
)
p
.
createLockedDofConstraint
(
'lhand_4'
,
'hrp2/LHAND_JOINT4'
,
q_init
[
ilh
+
5
],
0
,
0
)
p
.
createStaticStabilityConstraints
(
"balance"
,
q_init
)
p
s
.
createLockedDofConstraint
(
'larm_6'
,
'hrp2/LARM_JOINT6'
,
q_init
[
ilh
],
0
,
0
)
p
s
.
createLockedDofConstraint
(
'lhand_0'
,
'hrp2/LHAND_JOINT0'
,
q_init
[
ilh
+
1
],
0
,
0
)
p
s
.
createLockedDofConstraint
(
'lhand_1'
,
'hrp2/LHAND_JOINT1'
,
q_init
[
ilh
+
2
],
0
,
0
)
p
s
.
createLockedDofConstraint
(
'lhand_2'
,
'hrp2/LHAND_JOINT2'
,
q_init
[
ilh
+
3
],
0
,
0
)
p
s
.
createLockedDofConstraint
(
'lhand_3'
,
'hrp2/LHAND_JOINT3'
,
q_init
[
ilh
+
4
],
0
,
0
)
p
s
.
createLockedDofConstraint
(
'lhand_4'
,
'hrp2/LHAND_JOINT4'
,
q_init
[
ilh
+
5
],
0
,
0
)
p
s
.
createStaticStabilityConstraints
(
"balance"
,
q_init
)
# 3}}}
# Create the graph of constraints {{{3
...
...
@@ -95,11 +94,11 @@ graph.setConstraints (edge='grasp_e0', lockDof = lockscrewgun)
graph
.
setConstraints
(
node
=
'grasp_n0'
,
pregrasp
=
'l_pregrasp'
)
graph
.
setConstraints
(
edge
=
'grasp_e1'
,
lockDof
=
lockscrewgun
)
graph
.
client
.
graph
.
setLevelSetConstraints
(
graph
.
edges
[
'keep_grasp_ls'
],
[],
lockscrewgun
)
graph
.
setConstraints
(
graph
=
True
,
lockDof
=
locklhand
,
numConstraints
=
p
.
balanceConstraints
())
graph
.
setConstraints
(
graph
=
True
,
lockDof
=
locklhand
,
numConstraints
=
p
s
.
balanceConstraints
())
# 3}}}
p
.
setInitialConfig
(
q_init
)
p
.
addGoalConfig
(
q_goal
)
p
s
.
setInitialConfig
(
q_init
)
p
s
.
addGoalConfig
(
q_goal
)
from
hpp.gepetto
import
PathPlayer
pp
=
PathPlayer
(
robot
.
client
.
basic
,
r
)
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