Commit efb69320 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Add damping to SoT solvers and fix high gain.

parent b1087d95
......@@ -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)
......
......@@ -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)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment