Commit 351685fa authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Florent Lamiraux florent@laas.fr
Browse files

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.
parent ee8fe675
......@@ -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")
......
......@@ -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
)
......
......@@ -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):
......
#!/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)
Supports Markdown
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