Move robot initialization parts to sot-application

  Some initializations that used to be performed in this package were in fact
  application dependent. These initializations have been moved to the new
  package sot-application.
  - class solver has been removed,
  - features and tasks are not created anymore in AbstractHumanoidRobot class.

CMakeLists.txt

@@ -21,6 +21,7 @@ INCLUDE(cmake/lapack.cmake)
 INCLUDE(cmake/cpack.cmake)
 
 SET(PROJECT_NAME sot-dynamic)
+
 SET(PROJECT_DESCRIPTION "jrl-dynamics bindings for dynamic-graph.")
 SET(PROJECT_URL "http://github.com/jrl-umi3218/sot-dynamic")
 

src/CMakeLists.txt

@@ -64,7 +64,6 @@ INSTALL(FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/__init__.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/humanoid_robot.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/tools.py
-  ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/solver.py
   ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dynamics/parser.py
   DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dynamics
   )

src/dynamic_graph/sot/dynamics/humanoid_robot.py

@@ -20,9 +20,7 @@ from dynamic_graph.tracer_real_time import TracerRealTime
 from dynamic_graph.tools import addTrace
 from dynamic_graph.sot.core import OpPointModifier
 from dynamic_graph.sot.core.derivator import Derivator_of_Vector
-from dynamic_graph.sot.core.feature_position import FeaturePosition
-from dynamic_graph.sot.core import RobotSimu, FeatureGeneric, \
-    FeatureJointLimits, Task, Constraint, GainAdaptive, SOT
+from dynamic_graph.sot.core import RobotSimu
 
 from dynamic_graph.sot.dynamics.parser import Parser
 from dynamic_graph.sot.dynamics import AngleEstimator
@@ -49,15 +47,6 @@ class AbstractHumanoidRobot (object):
       - rightAnkle,
       - Gaze.
 
-    Tasks are stored into 'tasks' dictionary.
-
-    For portability, some signals are accessible as attributes:
-      - zmpRef: input (vector),
-      - comRef: input (vector).
-      - com:    output (vector)
-      - comSelec input (flag)
-      - comdot: input (vector) reference velocity of the center of mass
-
     """
 
     OperationalPoints = ['left-wrist', 'right-wrist',
@@ -250,52 +239,6 @@ class AbstractHumanoidRobot (object):
         for op in self.OperationalPoints:
             model.createOpPoint(op, op)
 
-    def createCenterOfMassFeatureAndTask(self,
-                                         featureName, featureDesName,
-                                         taskName,
-                                         selec = '011',
-                                         gain = 1.):
-        self.dynamic.com.recompute(0)
-        self.dynamic.Jcom.recompute(0)
-
-        featureCom = FeatureGeneric(featureName)
-        plug(self.dynamic.com, featureCom.errorIN)
-        plug(self.dynamic.Jcom, featureCom.jacobianIN)
-        featureCom.selec.value = selec
-        featureComDes = FeatureGeneric(featureDesName)
-        featureComDes.errorIN.value = self.dynamic.com.value
-        featureCom.setReference(featureComDes.name)
-        comTask = Task(taskName)
-        comTask.add(featureName)
-        comTask.controlGain.value = gain
-        return (featureCom, featureComDes, comTask)
-
-    def createOperationalPointFeatureAndTask(self,
-                                             operationalPointName,
-                                             featureName,
-                                             taskName,
-                                             gain = .2):
-        jacobianName = 'J{0}'.format(operationalPointName)
-        self.dynamic.signal(operationalPointName).recompute(0)
-        self.dynamic.signal(jacobianName).recompute(0)
-        feature = \
-            FeaturePosition(featureName,
-                            self.dynamic.signal(operationalPointName),
-                            self.dynamic.signal(jacobianName),
-                            self.dynamic.signal(operationalPointName).value)
-        task = Task(taskName)
-        task.add(featureName)
-        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)
@@ -307,16 +250,6 @@ class AbstractHumanoidRobot (object):
         frame.jacobian.recompute(frame.jacobian.time + 1)
         return frame
 
-    def initializeSignals (self):
-        """
-        For portability, make some signals accessible as attributes.
-        """
-        self.comRef = self.featureComDes.errorIN
-        self.zmpRef = self.device.zmp
-        self.com = self.dynamic.com
-        self.comSelec = self.featureCom.selec
-        self.comdot = self.featureComDes.errordotIN
-
     def initializeRobot(self):
         """
         If the robot model is correctly loaded, this method will then
@@ -333,11 +266,12 @@ class AbstractHumanoidRobot (object):
         if not self.device:
             self.device = RobotSimu(self.name + '_device')
 
+
         # Freeflyer reference frame should be the same as global
         # frame so that operational point positions correspond to
         # position in freeflyer frame.
         self.device.set(self.halfSitting)
-        self.dynamic.position.value = self.halfSitting
+        plug(self.device.state, self.dynamic.position)
 
         if self.enableVelocityDerivator:
             self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
@@ -359,33 +293,6 @@ class AbstractHumanoidRobot (object):
 
         self.initializeOpPoints(self.dynamic)
 
-        # --- center of mass ------------
-        (self.featureCom, self.featureComDes, self.comTask) = \
-            self.createCenterOfMassFeatureAndTask(
-            '{0}_feature_com'.format(self.name),
-            '{0}_feature_ref_com'.format(self.name),
-            '{0}_task_com'.format(self.name))
-
-        # --- operational points tasks -----
-        self.features = dict()
-        self.tasks = dict()
-        for op in self.OperationalPoints:
-            (self.features[op], self.tasks[op]) = \
-                self.createOperationalPointFeatureAndTask(
-                op, '{0}_feature_{1}'.format(self.name, op),
-                '{0}_task_{1}'.format(self.name, op))
-            # define a member for each operational point
-            w = op.split('-')
-            memberName = w[0]
-            for i in w[1:]:
-                memberName += i.capitalize()
-            setattr(self, memberName, self.features[op])
-        self.tasks ['com'] = self.comTask
-
-        # --- balance task --- #
-        self.tasks ['balance'] =\
-            self.createBalanceTask ('{0}_task_balance'.format (self.name))
-
         # --- additional frames ---
         self.frames = dict()
         frameName = 'rightHand'
@@ -410,7 +317,6 @@ class AbstractHumanoidRobot (object):
                 "{0}_{1}".format(self.name, frameName),
                 transformation,
                 signalName)
-        self.initializeSignals ()
 
     def addTrace(self, entityName, signalName):
         if self.tracer:
@@ -436,15 +342,6 @@ class AbstractHumanoidRobot (object):
             for s in ['position', 'jacobian']:
                 self.addTrace(self.frames[frameName].name, s)
 
-        # Robot features
-        for s in self.OperationalPoints:
-            self.addTrace(self.features[s]._reference.name, 'position')
-            self.addTrace(self.tasks[s].name, 'error')
-
-        # Com
-        self.addTrace(self.featureComDes.name, 'errorIN')
-        self.addTrace(self.comTask.name, 'error')
-
         # Device
         for s in self.tracedSignals['device']:
             self.addTrace(self.device.name, s)
@@ -505,12 +402,10 @@ class AbstractHumanoidRobot (object):
 
         self.dynamic.com.recompute(self.device.state.time+1)
         self.dynamic.Jcom.recompute(self.device.state.time+1)
-        self.featureComDes.errorIN.value = self.dynamic.com.value
 
         for op in self.OperationalPoints:
             self.dynamic.signal(op).recompute(self.device.state.time+1)
             self.dynamic.signal('J'+op).recompute(self.device.state.time+1)
-            self.features[op].reference.value = self.dynamic.signal(op).value
 
 class HumanoidRobot(AbstractHumanoidRobot):
 

src/dynamic_graph/sot/dynamics/solver.py

-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
-#
-# This file is part of dynamic-graph.
-# dynamic-graph is free software: you can redistribute it and/or
-# modify it under the terms of the GNU Lesser General Public License
-# as published by the Free Software Foundation, either version 3 of
-# the License, or (at your option) any later version.
-#
-# dynamic-graph is distributed in the hope that it will be useful, but
-# WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-# General Lesser Public License for more details.  You should have
-# received a copy of the GNU Lesser General Public License along with
-# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
-
-from dynamic_graph import plug
-from dynamic_graph.sot.core import JointLimitator, SOT
-
-class Solver:
-    robot = None
-    sot = None
-
-    def __init__(self, robot):
-        self.robot = robot
-
-        # Make sure control does not exceed joint limits.
-        self.jointLimitator = JointLimitator('joint_limitator')
-        plug(self.robot.dynamic.position, self.jointLimitator.joint)
-        plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl)
-        plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl)
-
-        # Create the solver.
-        self.sot = SOT('solver')
-        self.sot.signal('damping').value = 1e-6
-        self.sot.setNumberDofs(self.robot.dimension)
-
-        # Plug the solver control into the filter.
-        plug(self.sot.control, self.jointLimitator.controlIN)
-
-        # Important: always use 'jointLimitator.control'
-        # and NOT 'sot.control'!
-
-        if robot.device:
-            plug(self.jointLimitator.control, robot.device.control)
-            plug(self.robot.device.state, self.robot.dynamic.position)
-
-    def push (self, task):
-        """
-        Proxy method to push a task in the sot
-        """
-        self.sot.push (task.name)
-
-    def remove (self, task):
-        """
-        Proxy method to remove a task from the sot
-        """
-        self.sot.remove (task.name)