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
bca2f759
Commit
bca2f759
authored
Feb 26, 2015
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 06, 2015
Browse files
Update script test_graph_two_hands.
parent
591c1370
Changes
1
Hide whitespace changes
Inline
Side-by-side
script/test_graph_two_hands.py
View file @
bca2f759
...
...
@@ -2,7 +2,7 @@
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
hpp.gepetto
import
PathPlayer
,
PathPlayerGui
from
math
import
sqrt
class
Box
(
object
):
...
...
@@ -23,12 +23,11 @@ class Environment (object):
# Load robot and object. {{{3
robot
=
Robot
(
'pr2-box'
,
'pr2'
,
rootJointType
=
"anchor"
)
ps
=
ProblemSolver
(
robot
)
r
=
ViewerFactory
(
ps
)
vf
=
ViewerFactory
(
ps
)
robot
.
client
.
manipulation
.
robot
.
setRootJointPosition
(
'pr2'
,
(
-
3.2
,
-
4
,
0
,
1
,
0
,
0
,
0
))
r
.
loadObjectModel
(
Box
,
'box'
)
r
.
buildCompositeRobot
([
'pr2'
,
'box'
])
r
.
loadEnvironmentModel
(
Environment
,
"kitchen_area"
)
robot
.
setJointPosition
(
'pr2/base_joint'
,
(
-
3.2
,
-
4
,
0
,
1
,
0
,
0
,
0
))
vf
.
loadObjectModel
(
Box
,
'box'
)
vf
.
loadEnvironmentModel
(
Environment
,
"kitchen_area"
)
robot
.
setJointBounds
(
"box/base_joint_xyz"
,
[
-
3
,
-
2
,
-
5
,
-
3
,
0.7
,
1
])
# 3}}}
...
...
@@ -72,7 +71,7 @@ from hpp.corbaserver.manipulation import ConstraintGraph
cg
=
ConstraintGraph
(
robot
,
'graph'
)
robot
.
client
.
manipulation
.
problem
.
createPlacementConstraint
(
'box_placement'
,
'box'
,
'box/base_joint_SO3'
,
'box_surface'
,
'kitchen_area/pancake_table_table_top'
)
'box_placement'
,
'box/base_joint_SO3'
,
'box
/box
_surface'
,
'kitchen_area/pancake_table_table_top'
)
jointNames
=
dict
()
jointNames
[
'all'
]
=
robot
.
getJointNames
()
...
...
@@ -102,7 +101,19 @@ lockrhand = ['r_l_finger','r_r_finger'];
ps
.
createLockedJoint
(
'r_l_finger'
,
'pr2/r_gripper_l_finger_joint'
,
[
0.5
])
ps
.
createLockedJoint
(
'r_r_finger'
,
'pr2/r_gripper_r_finger_joint'
,
[
0.5
])
lockhands
=
[
'l_l_finger'
,
'l_r_finger'
,
'r_l_finger'
,
'r_r_finger'
];
lockhands
=
lockrhand
+
locklhand
lockHeadAndTorso
=
[
'head_pan'
,
'head_tilt'
,
'torso'
,
'laser'
];
ps
.
createLockedJoint
(
'head_pan'
,
'pr2/head_pan_joint'
,
[
q_init
[
robot
.
rankInConfiguration
[
'pr2/head_pan_joint'
]]])
ps
.
createLockedJoint
(
'head_tilt'
,
'pr2/head_tilt_joint'
,
[
q_init
[
robot
.
rankInConfiguration
[
'pr2/head_tilt_joint'
]]])
ps
.
createLockedJoint
(
'torso'
,
'pr2/torso_lift_joint'
,
[
q_init
[
robot
.
rankInConfiguration
[
'pr2/torso_lift_joint'
]]])
ps
.
createLockedJoint
(
'laser'
,
'pr2/laser_tilt_mount_joint'
,
[
q_init
[
robot
.
rankInConfiguration
[
'pr2/laser_tilt_mount_joint'
]]])
lockAll
=
lockhands
+
lockHeadAndTorso
# 3}}}
...
...
@@ -164,7 +175,7 @@ cg.createEdge ('right', 'right', 'r_keep_grasp', 5)
cg
.
setConstraints
(
edge
=
'move_free'
,
lockDof
=
lockbox
)
# 4}}}
cg
.
setConstraints
(
graph
=
True
,
lockDof
=
lock
hands
)
cg
.
setConstraints
(
graph
=
True
,
lockDof
=
lock
All
)
# 3}}}
res
=
ps
.
client
.
manipulation
.
problem
.
applyConstraints
(
cg
.
nodes
[
'free'
],
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