From 351685fa7cfbc17ea0b384ade80f0c1ece05ab0d Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Wed, 19 Jun 2013 13:27:34 +0200
Subject: [PATCH] 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 | 1 +
src/CMakeLists.txt | 1 -
.../sot/dynamics/humanoid_robot.py | 111 +-----------------
src/dynamic_graph/sot/dynamics/solver.py | 59 ----------
4 files changed, 4 insertions(+), 168 deletions(-)
delete mode 100644 src/dynamic_graph/sot/dynamics/solver.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7e132f8..9019c82 100644
--- a/CMakeLists.txt
+++ b/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")
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c0aa526..12d9ffa 100644
--- a/src/CMakeLists.txt
+++ b/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
)
diff --git a/src/dynamic_graph/sot/dynamics/humanoid_robot.py b/src/dynamic_graph/sot/dynamics/humanoid_robot.py
index 4e4b53a..ce5e23f 100755
--- a/src/dynamic_graph/sot/dynamics/humanoid_robot.py
+++ b/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):
diff --git a/src/dynamic_graph/sot/dynamics/solver.py b/src/dynamic_graph/sot/dynamics/solver.py
deleted file mode 100644
index a29bdd5..0000000
--- a/src/dynamic_graph/sot/dynamics/solver.py
+++ /dev/null
@@ -1,59 +0,0 @@
-#!/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)
--
GitLab