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
efb69320
Commit
efb69320
authored
Apr 05, 2018
by
Joseph Mirabel
Committed by
Joseph Mirabel
Apr 05, 2018
Browse files
Add damping to SoT solvers and fix high gain.
parent
b1087d95
Changes
2
Hide whitespace changes
Inline
Side-by-side
sot_hpp/factory.py
View file @
efb69320
...
...
@@ -167,6 +167,7 @@ class Factory(GraphFactoryAbstract):
n
=
self
.
_loopTransitionName
(
state
.
grasps
)
sot
=
SOT
(
'sot_'
+
n
)
sot
.
setSize
(
self
.
sotrobot
.
dynamic
.
getDimension
())
sot
.
damping
.
value
=
0.001
self
.
hpTasks
.
pushTo
(
sot
)
state
.
manifold
.
pushTo
(
sot
)
...
...
@@ -208,6 +209,7 @@ class Factory(GraphFactoryAbstract):
for
n
in
ns
:
s
=
SOT
(
'sot_'
+
n
)
s
.
setSize
(
self
.
sotrobot
.
dynamic
.
getDimension
())
s
.
damping
.
value
=
0.001
self
.
hpTasks
.
pushTo
(
s
)
#if pregrasp and i == 1:
...
...
@@ -225,6 +227,7 @@ class Factory(GraphFactoryAbstract):
sot
=
SOT
(
"postAction_"
+
key
)
sot
.
setSize
(
self
.
sotrobot
.
dynamic
.
getDimension
())
sot
.
damping
.
value
=
0.001
self
.
hpTasks
.
pushTo
(
sot
)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
st
.
manifold
.
pushTo
(
sot
)
...
...
@@ -242,6 +245,7 @@ class Factory(GraphFactoryAbstract):
sot
=
SOT
(
"preAction_"
+
key
)
sot
.
setSize
(
self
.
sotrobot
.
dynamic
.
getDimension
())
sot
.
damping
.
value
=
0.001
self
.
hpTasks
.
pushTo
(
sot
)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
sf
.
manifold
.
pushTo
(
sot
)
...
...
sot_hpp/tools.py
View file @
efb69320
...
...
@@ -147,7 +147,7 @@ class Grasp (Manifold):
M
=
transformToMatrix
(
self
.
otherGripper
.
sotpose
*
self
.
otherHandle
.
sotpose
.
inverse
()
*
self
.
handle
.
sotpose
*
self
.
gripper
.
sotpose
.
inverse
())
self
.
graspTask
.
opmodif
=
matrixToTuple
(
M
)
self
.
graspTask
.
feature
.
frame
(
"current"
)
setGain
(
self
.
graspTask
.
gain
,(
100
,
0.9
,
0.01
,
0.9
))
setGain
(
self
.
graspTask
.
gain
,(
4.9
,
0.9
,
0.01
,
0.9
))
self
.
graspTask
.
task
.
setWithDerivative
(
False
)
# Sets the position and velocity of the other gripper as the goal of the first gripper pose
...
...
@@ -222,7 +222,6 @@ class EEPosture (Manifold):
self
.
tp
.
feature
.
selectDof
(
i
,
True
)
self
.
tp
.
gain
=
GainAdaptive
(
"gain_"
+
n
)
robotDim
=
sotrobot
.
dynamic
.
getDimension
()
self
.
tp
.
add
(
self
.
tp
.
feature
.
name
)
# Set the gain of the posture task
...
...
@@ -262,6 +261,7 @@ class Foot (Manifold):
def
_signalVelocityRef
(
self
):
return
self
.
taskFoot
.
featureDes
.
velocity
class
COM
(
Manifold
):
sep
=
"_com_"
def
__init__
(
self
,
comname
,
sotrobot
):
self
.
taskCom
=
MetaTaskKineCom
(
sotrobot
.
dynamic
,
name
=
COM
.
sep
+
comname
)
...
...
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