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
Humanoid Path Planner
hpp-practicals
Commits
ff7a9093
Commit
ff7a9093
authored
Nov 25, 2016
by
Florent Lamiraux
Committed by
Florent Lamiraux florent@laas.fr
Nov 30, 2016
Browse files
Update script/manipulation.py to topic/pinocchio branch.
parent
3984a273
Changes
3
Hide whitespace changes
Inline
Side-by-side
script/manipulation.py
View file @
ff7a9093
...
...
@@ -5,11 +5,13 @@ from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripp
vf
.
loadEnvironmentModel
(
Ground
,
'ground'
)
vf
.
loadObjectModel
(
Pokeball
,
'pokeball'
)
robot
.
setJointBounds
(
'pokeball/base_joint_xyz'
,
[
-
.
4
,.
4
,
-
.
4
,.
4
,
-
.
1
,
1.
])
robot
.
setJointBounds
(
'pokeball/root_joint'
,
[
-
.
4
,.
4
,
-
.
4
,.
4
,
-
.
1
,
1.
,
-
1.0001
,
1.0001
,
-
1.0001
,
1.0001
,
-
1.0001
,
1.0001
,
-
1.0001
,
1.0001
,])
r
=
vf
.
createViewer
()
q1
=
[
0
,
-
1.57
,
1.57
,
0
,
0
,
0
,
.
3
,
0
,
0.025
,
1
,
0
,
0
,
0
]
q1
=
[
0
,
-
1.57
,
1.57
,
0
,
0
,
0
,
.
3
,
0
,
0.025
,
0
,
0
,
0
,
1
]
r
(
q1
)
## Create constraint graph
...
...
@@ -17,7 +19,7 @@ graph = ConstraintGraph (robot, 'graph')
## Create constraint of relative position of the ball in the gripper when ball
## is grasped
ballInGripper
=
[
0
,
0
,
0.
05
5
,
0
,
-
sqrt
(
2
)
/
2
,
-
sqrt
(
2
)
/
2
,
0
]
ballInGripper
=
[
0
,
.
137
,
0
,
0.5
,
0
.5
,
-
0.5
,
0
.5
]
ps
.
createTransformationConstraint
(
'grasp'
,
gripperName
,
ballName
,
ballInGripper
,
6
*
[
True
,])
...
...
@@ -33,9 +35,10 @@ graph.createEdge ('grasp', 'placement', 'release-ball', 1, 'grasp')
## Create transformation constraint : ball is in horizontal plane with free
## rotation around z
ps
.
createTransformationConstraint
(
'placement'
,
ballName
,
''
,
[
0
,
0
,
0.025
,
1
,
0
,
0
,
0
],
[
False
,
False
,
True
,
True
,
True
,
False
,])
ps
.
createTransformationConstraint
(
'placement'
,
''
,
ballName
,
[
0
,
0
,
0.025
,
0
,
0
,
0
,
1
],
[
False
,
False
,
True
,
True
,
True
,
False
,])
# Create complement constraint
ps
.
createTransformationConstraint
(
'placement/complement'
,
ballName
,
''
,
[
0
,
0
,
0.025
,
1
,
0
,
0
,
0
],
[
True
,
True
,
False
,
False
,
False
,
True
,])
ps
.
createTransformationConstraint
(
'placement/complement'
,
''
,
ballName
,
[
0
,
0
,
0.025
,
0
,
0
,
0
,
1
],
[
True
,
True
,
False
,
False
,
False
,
True
,])
ps
.
setConstantRightHandSide
(
'placement'
,
True
)
ps
.
setConstantRightHandSide
(
'placement/complement'
,
False
)
...
...
script/manipulation/common.py
View file @
ff7a9093
...
...
@@ -37,5 +37,5 @@ robot.client.basic.problem.setMaxIterations (40)
ps
=
ProblemSolver
(
robot
)
vf
=
ViewerFactory
(
ps
)
gripperName
=
'ur5/wrist_3_
link-tool0_fixed_
joint'
ballName
=
'pokeball/
base
_joint
_SO3
'
gripperName
=
'ur5/wrist_3_joint'
ballName
=
'pokeball/
root
_joint'
script/manipulation_box.py
View file @
ff7a9093
...
...
@@ -5,17 +5,19 @@ from manipulation import robot, vf, ps, Ground, Box, Pokeball, PathPlayer, gripp
vf
.
loadEnvironmentModel
(
Ground
,
'ground'
)
vf
.
loadEnvironmentModel
(
Box
,
'box'
)
vf
.
moveObstacle
(
'box/base_link_0'
,
[
0.3
+
0.04
,
0
,
0.04
,
1
,
0
,
0
,
0
])
vf
.
moveObstacle
(
'box/base_link_1'
,
[
0.3
-
0.04
,
0
,
0.04
,
1
,
0
,
0
,
0
])
vf
.
moveObstacle
(
'box/base_link_2'
,
[
0.3
,
0.04
,
0.04
,
1
,
0
,
0
,
0
])
vf
.
moveObstacle
(
'box/base_link_3'
,
[
0.3
,
-
0.04
,
0.04
,
1
,
0
,
0
,
0
])
vf
.
moveObstacle
(
'box/base_link_0'
,
[
0.3
+
0.04
,
0
,
0.04
,
0
,
0
,
0
,
1
])
vf
.
moveObstacle
(
'box/base_link_1'
,
[
0.3
-
0.04
,
0
,
0.04
,
0
,
0
,
0
,
1
])
vf
.
moveObstacle
(
'box/base_link_2'
,
[
0.3
,
0.04
,
0.04
,
0
,
0
,
0
,
1
])
vf
.
moveObstacle
(
'box/base_link_3'
,
[
0.3
,
-
0.04
,
0.04
,
0
,
0
,
0
,
1
])
vf
.
loadObjectModel
(
Pokeball
,
'pokeball'
)
robot
.
setJointBounds
(
'pokeball/base_joint_xyz'
,
[
-
.
4
,.
4
,
-
.
4
,.
4
,
-
.
1
,
1.
])
robot
.
setJointBounds
(
'pokeball/root_joint'
,
[
-
.
4
,.
4
,
-
.
4
,.
4
,
-
.
1
,
1.
,
-
1.0001
,
1.0001
,
-
1.0001
,
1.0001
,
-
1.0001
,
1.0001
,
-
1.0001
,
1.0001
,])
r
=
vf
.
createViewer
()
q1
=
[
0
,
-
1.57
,
1.57
,
0
,
0
,
0
,
.
3
,
0
,
0.025
,
1
,
0
,
0
,
0
]
q1
=
[
0
,
-
1.57
,
1.57
,
0
,
0
,
0
,
.
3
,
0
,
0.025
,
0
,
0
,
0
,
1
]
r
(
q1
)
## Create graph
...
...
@@ -31,7 +33,7 @@ res, q_goal, error = graph.applyNodeConstraints ('placement', q2)
ps
.
setInitialConfig
(
q_init
)
ps
.
addGoalConfig
(
q_goal
)
ps
.
selectPathValidation
(
"Di
chotomy
"
,
0
)
ps
.
selectPathValidation
(
"Di
scretized
"
,
0
.01
)
ps
.
selectPathProjector
(
"Progressive"
,
0.1
)
pp
=
PathPlayer
(
ps
.
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