Add damping to SoT solvers and fix high gain.

sot_hpp/factory.py

@@ -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

@@ -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)