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
64d37400
Commit
64d37400
authored
Mar 06, 2015
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 06, 2015
Browse files
Update scripts.
parent
bca2f759
Changes
4
Hide whitespace changes
Inline
Side-by-side
script/balance_constraints_success_statistics.py
View file @
64d37400
...
...
@@ -6,12 +6,15 @@ from hpp.corbaserver import Client, ProblemSolver
from
hpp.corbaserver.wholebody_step
import
Client
as
WSClient
from
hpp.corbaserver.wholebody_step
import
Problem
from
hpp.corbaserver.hrp2
import
Robot
from
hpp.gepetto
import
ViewerFactory
Robot
.
urdfSuffix
=
'_capsule'
Robot
.
srdfSuffix
=
'_capsule'
robot
=
Robot
(
'hrp2'
)
p
=
ProblemSolver
(
robot
)
vf
=
ViewerFactory
(
p
)
wcl
=
WSClient
()
q0
=
robot
.
getInitialConfig
()
...
...
@@ -24,6 +27,14 @@ balanceConstraints = [constraintName + "/relative-com",
constraintName
+
"/relative-position"
,
constraintName
+
"/orientation-left-foot"
,
constraintName
+
"/position-left-foot"
]
c2name
=
"stability"
wcl
.
problem
.
addStabilityConstraints
(
c2name
,
q0
,
robot
.
leftAnkle
,
robot
.
rightAnkle
,
""
,
Problem
.
ALIGNED_COM
)
b2C
=
[
c2name
+
"/com-between-feet"
,
c2name
+
"/orientation-right"
,
c2name
+
"/orientation-left"
,
c2name
+
"/position-right"
,
c2name
+
"/position-left"
]
robot
.
setJointBounds
(
"base_joint_xyz"
,
[
-
4
,
4
,
-
4
,
4
,
-
4
,
4
])
...
...
@@ -41,3 +52,8 @@ for constraint in balanceConstraints:
testConstraint
([
constraint
])
testConstraint
(
balanceConstraints
)
for
constraint
in
b2C
:
testConstraint
([
constraint
])
testConstraint
(
b2C
)
script/hrp2_walk.py
View file @
64d37400
...
...
@@ -5,18 +5,17 @@ from hpp.gepetto.manipulation import Viewer, ViewerFactory
from
hpp.gepetto
import
PathPlayer
,
PathPlayerGui
Robot
.
urdfSuffix
=
'_capsule_mesh'
Robot
.
srdfSuffix
=
'_manipulation
_moveit
'
Robot
.
srdfSuffix
=
'_manipulation'
# Load HRP2 {{{3
robot
=
Robot
(
'hrp2
-e
'
,
'hrp2'
)
robot
=
Robot
(
'hrp2'
,
'hrp2'
)
ps
=
ProblemSolver
(
robot
)
vf
=
ViewerFactory
(
ps
)
vf
.
buildCompositeRobot
([
'hrp2'
])
robot
.
setJointBounds
(
"hrp2/base_joint_xyz"
,
[
-
0.2
,
0.8
,
-
0.5
,
0.5
,
0
,
2
])
ps
.
selectPathOptimizer
(
'None'
)
ps
.
selectPathProjector
(
'Progressive'
,
0.
0
2
)
ps
.
selectPathProjector
(
'Progressive'
,
0.2
)
ps
.
setErrorThreshold
(
1e-3
)
ps
.
setMaxIterations
(
40
)
# 3}}}
...
...
@@ -32,8 +31,8 @@ q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz
=
robot
.
rankInConfiguration
[
'hrp2/base_joint_xyz'
]
q_goal
=
q_init
[::]
#
q_goal [ibjxyz:ibjxyz+2] = [0.5, 0]
q_goal
[
ibjxyz
:
ibjxyz
+
2
]
=
[
0.2
,
0
]
q_goal
[
ibjxyz
:
ibjxyz
+
2
]
=
[
0.5
,
0
]
#
q_goal [ibjxyz:ibjxyz+2] = [0.2, 0]
# 3}}}
...
...
@@ -126,7 +125,7 @@ _["right_to_rightside_ls"]=""
_
[
"rightside_to_double"
]
=
""
_
[
"right_support"
]
=
""
# 4}}}
graph
.
setTextToTeXTranslation
(
_
)
#
graph.setTextToTeXTranslation (_)
graph
.
createNode
([
"both_left"
,
"both_right"
,
"both"
,
"left"
,
"right"
])
...
...
script/test_graph_two_hands.py
View file @
64d37400
...
...
@@ -119,6 +119,27 @@ lockAll = lockhands + lockHeadAndTorso
# Create the graph. {{{3
_
=
dict
()
# Create a dictionnary to translate human readable names into LaTeX expressions.
# This goes well with option -tmath of dot2tex command line.
# {{{4
_
[
"both"
]
=
"2 grasps"
_
[
"left"
]
=
"Left grasp"
_
[
"right"
]
=
"Right grasp"
_
[
"free"
]
=
"No grasp"
# _["r_grasp"]=""
# _["l_grasp"]=""
# _["b_r_grasp"]=""
# _["b_l_grasp"]=""
# _["b_r_ungrasp"]=""
# _["b_l_ungrasp"]=""
_
[
"move_free"
]
=
""
_
[
"r_keep_grasp"
]
=
""
_
[
"l_keep_grasp"
]
=
""
# 4}}}
cg
.
setTextToTeXTranslation
(
_
)
cg
.
createNode
([
'both'
,
'right'
,
'left'
,
'free'
])
cg
.
setConstraints
(
node
=
'free'
,
numConstraints
=
[
'box_placement'
])
...
...
script/test_simple_graph.py
View file @
64d37400
# vim: foldmethod=marker foldlevel=2
from
hpp.corbaserver.manipulation.hrp2
import
Robot
from
hpp.corbaserver.manipulation
import
ProblemSolver
,
ConstraintGraph
from
hpp.gepetto.manipulation
import
Viewer
from
hpp.gepetto.manipulation
import
Viewer
,
ViewerFactory
from
hpp.gepetto
import
PathPlayer
,
PathPlayerGui
class
ScrewGun
(
object
):
rootJointType
=
'freeflyer'
packageName
=
'airbus_environment'
meshPackageName
=
'airbus_environment'
urdfName
=
'screw_gun'
urdfSuffix
=
"
_massless
"
urdfSuffix
=
""
srdfSuffix
=
""
Robot
.
urdfSuffix
=
'_capsule_mesh'
...
...
@@ -18,13 +19,13 @@ Robot.srdfSuffix = '_manipulation'
robot
=
Robot
(
'hrp2-screw'
,
'hrp2'
)
ps
=
ProblemSolver
(
robot
)
ps
.
selectPathPlanner
(
"M-RRT"
)
r
=
Viewer
(
ps
)
r
.
loadObjectModel
(
ScrewGun
,
'screw_gun'
)
r
.
buildCompositeRobot
([
'hrp2'
,
'screw_gun'
])
vf
=
ViewerFactory
(
ps
)
vf
.
loadObjectModel
(
ScrewGun
,
'screw_gun'
)
for
d
in
[
"hrp2"
,
"screw_gun"
]:
robot
.
setJointBounds
(
d
+
"/base_joint_xyz"
,
[
-
4
,
4
,
-
4
,
4
,
-
4
,
4
])
ps
.
selectPathOptimizer
(
'None'
)
ps
.
selectPathProjector
(
"Progressive"
,
0.2
)
ps
.
setErrorThreshold
(
1e-3
)
ps
.
setMaxIterations
(
40
)
# 3}}}
...
...
@@ -33,7 +34,8 @@ ps.setMaxIterations (40)
half_sitting
=
q
=
robot
.
getInitialConfig
()
q_init
=
half_sitting
[::]
# Set initial position of screw-driver
q_init
[
-
7
:]
=
[
2
,
1
,
0.65
,
0.7071067811865476
,
0
,
-
0.7071067811865475
,
0
]
# q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
q_init
[
-
7
:]
=
[
2
,
1
,
0.65
,
0.7071067811865476
,
0.5
,
-
0.5
,
0
]
# Open left hand
ilh
=
robot
.
rankInConfiguration
[
'hrp2/LARM_JOINT6'
]
q_init
[
ilh
:
ilh
+
6
]
=
[
0.75
,
-
0.75
,
0.75
,
-
0.75
,
0.75
,
-
0.75
]
...
...
@@ -57,7 +59,8 @@ for n in jointNames['all']:
if
not
n
.
startswith
(
"hrp2/LARM"
):
jointNames
[
'allButHRP2LeftArm'
].
append
(
n
)
graph
.
createGrasp
(
'l_grasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
,
jointNames
[
'hrp2'
])
ps
.
addPassiveDofs
(
'hrp2'
,
jointNames
[
'hrp2'
])
graph
.
createGrasp
(
'l_grasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
,
'hrp2'
)
graph
.
createPreGrasp
(
'l_pregrasp'
,
'hrp2/leftHand'
,
'screw_gun/handle2'
)
lockscrewgun
=
ps
.
lockFreeFlyerJoint
(
'screw_gun/base_joint'
,
'screwgun_lock'
,
...
...
@@ -76,7 +79,8 @@ ps.createLockedJoint \
ps
.
createLockedJoint
\
(
'lhand_4'
,
'hrp2/LHAND_JOINT4'
,
[
q_init
[
ilh
+
5
],])
ps
.
createStaticStabilityConstraints
(
"balance"
,
q_init
)
ps
.
addPartialCom
(
'hrp2'
,
[
'hrp2/base_joint_xyz'
])
ps
.
createStaticStabilityConstraints
(
"balance-hrp2"
,
q_init
,
'hrp2'
)
# 3}}}
# Create the graph of constraints {{{3
...
...
@@ -99,12 +103,9 @@ graph.setConstraints (edge='ungrasp_e1', lockDof = lockscrewgun)
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
.
setLevelSetConstraints
(
'keep_grasp_ls'
,
lockDof
=
lockscrewgun
)
graph
.
setConstraints
(
graph
=
True
,
lockDof
=
locklhand
,
numConstraints
=
ps
.
balanceConstraints
())
# 3}}}
ps
.
setInitialConfig
(
q_init
)
ps
.
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