diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py index 16fd1f8ad811de9b07be63d88cc4808d55ad2689..8cce5eb56892d7a27ebdb12f348e1d76cb2fd868 100755 --- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py +++ b/src/dynamic_graph/sot/dynamics/humanoid_robot.py @@ -288,6 +288,14 @@ class AbstractHumanoidRobot (object): task.controlGain.value = gain return (feature, task) + def createBalanceTask (self, taskName, gain = 1.): + task = Task (taskName) + task.add (self.featureCom.name) + task.add (self.leftAnkle.name) + task.add (self.rightAnkle.name) + task.controlGain.value = gain + return task + def createFrame(self, frameName, transformation, operationalPoint): frame = OpPointModifier(frameName) frame.setTransformation(transformation) @@ -363,6 +371,10 @@ class AbstractHumanoidRobot (object): memberName += i.capitalize() setattr(self, memberName, self.features[op]) + # --- balance task --- # + self.tasks ['balance'] =\ + self.createBalanceTask ('{0}_task_balance'.format (self.name)) + # --- additional frames --- self.frames = dict() frameName = 'rightHand'