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
sot-hpp
Commits
29c9c516
Commit
29c9c516
authored
Mar 26, 2018
by
Joseph Mirabel
Committed by
Joseph Mirabel
Mar 26, 2018
Browse files
Add class Affordance to store data related to each (gripper, handle)
parent
4463d8da
Changes
2
Hide whitespace changes
Inline
Side-by-side
sot_hpp/factory.py
View file @
29c9c516
...
...
@@ -2,6 +2,23 @@ from hpp.corbaserver.manipulation.constraint_graph_factory import ConstraintFact
from
tools
import
Manifold
,
Grasp
,
idx
,
idx_zip
,
OpFrame
,
EEPosture
from
dynamic_graph.sot.core
import
SOT
class
Affordance
(
object
):
def
__init__
(
self
,
gripper
,
handle
,
**
kwargs
):
self
.
gripper
=
gripper
self
.
handle
=
handle
self
.
setControl
(
**
kwargs
)
def
setControl
(
self
,
refOpen
,
refClose
,
openType
=
"position"
,
closeType
=
"position"
):
if
openType
!=
"position"
or
closeType
!=
"position"
:
raise
NotImplementedError
(
"Only position control is implemented for gripper opening/closure."
)
self
.
controlType
=
{
"open"
:
openType
,
"close"
:
closeType
,
}
self
.
ref
=
{
"open"
:
refOpen
,
"close"
:
refClose
,
}
class
TaskFactory
(
ConstraintFactoryAbstract
):
gfields
=
(
'grasp'
,
'pregrasp'
,
'gripper_open'
,
'gripper_close'
)
pfields
=
()
...
...
@@ -9,12 +26,26 @@ class TaskFactory(ConstraintFactoryAbstract):
def
__init__
(
self
,
graphfactory
):
super
(
TaskFactory
,
self
).
__init__
(
graphfactory
)
def
_buildGripper
(
self
,
type
,
gripper
,
handle
):
try
:
aff
=
self
.
graphfactory
.
affordances
[(
gripper
,
handle
)]
except
KeyError
:
# If there are no affordance, do not add a task.
return
Manifold
()
gf
=
self
.
graphfactory
robot
=
gf
.
sotrobot
if
aff
.
controlType
[
type
]
==
"position"
:
return
EEPosture
(
robot
,
gf
.
gripperFrames
[
gripper
],
aff
.
ref
[
type
])
else
:
raise
NotImplementedError
(
"Only position control is implemented for gripper closure."
)
def
buildGrasp
(
self
,
g
,
h
,
otherGrasp
=
None
):
gf
=
self
.
graphfactory
if
h
is
None
:
return
{
'gripper_open'
:
EEPosture
(
gf
.
sotrobot
,
gf
.
gripperFrames
[
g
],
[
0
])
}
gripper_open
=
self
.
_buildGripper
(
"open"
,
g
,
h
)
return
{
'gripper_open'
:
gripper_open
}
gripper_close
=
EEPosture
(
gf
.
sotrobot
,
gf
.
gripperFrames
[
g
],
[
-
0.2
]
)
gripper_close
=
self
.
_buildGripper
(
"close"
,
g
,
h
)
pregrasp
=
Grasp
(
gf
.
gripperFrames
[
g
],
gf
.
handleFrames
[
h
],
otherGrasp
,
...
...
@@ -66,6 +97,7 @@ class Factory(GraphFactoryAbstract):
for
ig
,
ih
in
idx_zip
(
grasps
):
if
ih
is
not
None
:
# Add task gripper_close
self
.
manifold
+=
tasks
.
g
(
factory
.
grippers
[
ig
],
factory
.
handles
[
ih
],
'gripper_close'
)
otherGrasp
=
objectsAlreadyGrasped
.
get
(
factory
.
objectFromHandle
[
ih
])
self
.
manifold
+=
tasks
.
g
(
factory
.
grippers
[
ig
],
factory
.
handles
[
ih
],
'grasp'
,
otherGrasp
)
objectsAlreadyGrasped
[
factory
.
objectFromHandle
[
ih
]]
=
tasks
.
g
(
factory
.
grippers
[
ig
],
factory
.
handles
[
ih
],
'grasp'
,
otherGrasp
)
...
...
@@ -78,12 +110,17 @@ class Factory(GraphFactoryAbstract):
self
.
tasks
=
TaskFactory
(
self
)
self
.
hpTasks
=
supervisor
.
hpTasks
self
.
lpTasks
=
supervisor
.
lpTasks
self
.
affordances
=
dict
()
self
.
sots
=
dict
()
self
.
postActions
=
dict
()
self
.
preActions
=
dict
()
self
.
supervisor
=
supervisor
def
addAffordance
(
self
,
aff
):
assert
isinstance
(
aff
,
Affordance
)
self
.
affordances
[(
aff
.
gripper
,
aff
.
handle
)]
=
aff
def
finalize
(
self
,
hppclient
):
graph
,
elmts
=
hppclient
.
manipulation
.
graph
.
getGraph
()
ids
=
{
n
.
name
:
n
.
id
for
n
in
elmts
.
edges
}
...
...
sot_hpp/tools.py
View file @
29c9c516
...
...
@@ -177,7 +177,6 @@ class Grasp (Manifold):
self
.
tasks
=
[
self
.
graspTask
.
task
]
#self.tasks = []
self
+=
EEPosture
(
sotrobot
,
self
.
gripper
,
[
-
0.2
if
self
.
closeGripper
else
0
])
def
opPointExist
(
self
,
dyn
,
opPoint
):
sigsP
=
filter
(
lambda
x
:
x
.
getName
().
split
(
':'
)[
-
1
]
==
opPoint
,
...
...
@@ -195,16 +194,9 @@ class EEPosture (Manifold):
super
(
EEPosture
,
self
).
__init__
()
self
.
gripper
=
gripper
# Get joint position in posture
robotName
,
gripperName
=
parseHppName
(
gripper
.
name
)
self
.
jointNames
=
sotrobot
.
grippers
[
gripperName
]
pinmodel
=
sotrobot
.
dynamic
.
model
idJ
=
pinmodel
.
getJointId
(
gripper
.
sotjoint
)
assert
idJ
<
pinmodel
.
njoints
joint
=
sotrobot
.
dynamic
.
model
.
joints
[
idJ
]
assert
joint
.
nq
==
len
(
position
)
idx_q
=
joint
.
idx_q
+
1
idx_v
=
joint
.
idx_v
+
1
n
=
"eeposture"
+
Posture
.
sep
+
gripper
.
name
+
Posture
.
sep
+
str
(
position
)
...
...
@@ -214,23 +206,34 @@ class EEPosture (Manifold):
plug
(
sotrobot
.
dynamic
.
position
,
self
.
tp
.
feature
.
state
)
q
=
list
(
sotrobot
.
dynamic
.
position
.
value
)
q
[
idx_v
:
idx_v
+
joint
.
nv
]
=
position
# Define the reference and the selected DoF
rank
=
0
ranks
=
[]
for
name
in
self
.
jointNames
:
idJ
=
pinmodel
.
getJointId
(
name
)
assert
idJ
<
pinmodel
.
njoints
joint
=
pinmodel
.
joints
[
idJ
]
idx_v
=
joint
.
idx_v
nv
=
joint
.
nv
ranks
+=
range
(
idx_v
,
idx_v
+
nv
)
q
[
idx_v
:
idx_v
+
nv
]
=
position
[
rank
:
rank
+
nv
]
rank
+=
nv
assert
rank
==
len
(
position
)
self
.
tp
.
feature
.
posture
.
value
=
q
for
i
in
ranks
:
self
.
tp
.
feature
.
selectDof
(
i
,
True
)
self
.
tp
.
gain
=
GainAdaptive
(
"gain_"
+
n
)
robotDim
=
sotrobot
.
dynamic
.
getDimension
()
# for i in range (6, robotDim):
# self.tp.feature.selectDof (i, False)
# print idx_q, idx_v
for
i
in
range
(
joint
.
nv
):
self
.
tp
.
feature
.
selectDof
(
idx_v
+
i
,
True
)
self
.
tp
.
add
(
self
.
tp
.
feature
.
name
)
# Set the gain of the posture task
setGain
(
self
.
tp
.
gain
,(
4.9
,
0.9
,
0.01
,
0.9
))
plug
(
self
.
tp
.
gain
.
gain
,
self
.
tp
.
controlGain
)
plug
(
self
.
tp
.
error
,
self
.
tp
.
gain
.
error
)
self
.
tasks
=
[
self
.
tp
]
if
len
(
self
.
jointNames
)
>
0
:
self
.
tasks
=
[
self
.
tp
]
class
Foot
(
Manifold
):
def
__init__
(
self
,
footname
,
sotrobot
,
selec
=
'111111'
):
...
...
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