Commit e2b85995 authored by olivier stasse's avatar olivier stasse

Implement the one robot one sot-library strategy.

parent fa3f1f27
......@@ -58,27 +58,37 @@ SET(PYTHON_MODULE_BUILD_DIR
SET(PYTHON_MODULE dynamic_graph/sot/hrp2)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "hrp2.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py" )
SET(FILES __init__.py robot.py)
# Install dynamic_graph.sot.hrp2_14
SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" ${PYTHON_SITELIB})
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
# Install dynamic_graph.sot.hrp2_10
SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" ${PYTHON_SITELIB})
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
# Add the library to wrap the controller of HRP2.
ADD_LIBRARY(sot-hrp2-controller
# Add the library to wrap the controller of HRP2-JRL.
MACRO(build_hrp2_controller robotnumber)
ADD_LIBRARY(sot-hrp2-${robotnumber}-controller
SHARED
sot-hrp2-controller.cpp
sot-hrp2-${robotnumber}-controller.cpp
)
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-controller "dynamic-graph")
PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-controller "sot-core")
PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-controller "dynamic-graph-corba")
PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-${robotnumber}-controller "dynamic-graph")
PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-${robotnumber}-controller "sot-core")
PKG_CONFIG_USE_DEPENDENCY(sot-hrp2-${robotnumber}-controller "dynamic-graph-corba")
INSTALL(TARGETS sot-hrp2-${robotnumber}-controller DESTINATION lib)
ENDMACRO()
INSTALL(TARGETS sot-hrp2-controller DESTINATION lib/${ROBOTNAME})
\ No newline at end of file
build_hrp2_controller("10")
build_hrp2_controller("14")
\ No newline at end of file
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, Olivier Stasse, 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/>.
print("Prologue HRP-2")
from dynamic_graph import plug
from dynamic_graph.entity import PyEntityFactoryClass
from dynamic_graph.sot.dynamics.solver import Solver
def prologue_hrp2(hrp2class):
# Create the device.
# This entity behaves exactly like robotsimu except:
# 1. it does not provide the increment method
# 2. it forwards the robot control to the sot-abstract
# controller.
Device = PyEntityFactoryClass('Device')
# Create the robot using the device.
robot = hrp2class(name = 'robot', device = Device('robot_device'))
# Initialize the zmp signal to the current com.
_com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.)
# Create a solver.
solver = Solver(robot)
# Make sure com and feet desired positions match the current
# positions.
s = ['left-ankle', 'right-ankle']
for i in s:
robot.dynamic.signal(i).recompute(robot.dynamic.signal(i).time + 1)
robot.features[i].reference.value = \
robot.dynamic.signal(i).value
robot.features[i]._feature.selec.value = '111111'
robot.tasks[i].controlGain.value = 180.
robot.featureComDes.errorIN.value = robot.dynamic.com.value
robot.featureComDes.selec.value = '111'
robot.comTask.controlGain.value = 180.
# Push com and feet tasks.
#
# The robot is currently in half-sitting, so this script freezes com
# and feet position so that the robot will remain stable while the
# user program is starting.
solver.push(robot.comTask.name)
for i in s:
solver.push(robot.tasks[i].name)
print("Prologue ran successfully.")
# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
####################################
# --- IMPORTANT --- #
# #
# THIS FILE MUST NEVER BE CHANGED. #
# TO RUN YOUR EXPERIMENT, PLEASE #
# WRITE A SEPARATE PYTHON MODULE #
# AND LAUNCH IT USING dg-remote! #
####################################
# -*- 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/>.
print("Prologue HRP2-14")
from dynamic_graph.sot.hrp2_10.robot import *
from dynamic_graph.sot.hrp2.prologue import *
robot,solver = prologue_hrp2("Robot")
__all__ = ["robot","solver"]
####################################
# --- IMPORTANT --- #
# #
# THIS FILE MUST NEVER BE CHANGED. #
# TO RUN YOUR EXPERIMENT, PLEASE #
# WRITE A SEPARATE PYTHON MODULE #
# AND LAUNCH IT USING dg-remote! #
####################################
......@@ -47,4 +47,4 @@ class Robot (Hrp2):
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
tracer)
__all__ = [Robot]
__all__ = ["Robot"]
# -*- 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/>.
print("Prologue HRP2-14")
from dynamic_graph.sot.hrp2_14.robot import *
from dynamic_graph.sot.hrp2.prologue import *
robot,solver = prologue_hrp2("Robot")
__all__ = ["robot","solver"]
####################################
# --- IMPORTANT --- #
# #
# THIS FILE MUST NEVER BE CHANGED. #
# TO RUN YOUR EXPERIMENT, PLEASE #
# WRITE A SEPARATE PYTHON MODULE #
# AND LAUNCH IT USING dg-remote! #
####################################
......@@ -111,4 +111,4 @@ class Robot (Hrp2):
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2, tracer)
__all__ = [Robot]
__all__ = ["Robot"]
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#include <sot/core/debug.hh>
#define ROBOTNAME std::string("HRP2JRL")
#include "sot-hrp2-10-controller.hh"
const std::string SoTHRP2_10_Controller::LOG_PYTHON_10="/tmp/HRP2Controller_10_python.out";
SoTHRP2_10_Controller::SoTHRP2_10_Controller():
SoTHRP2Controller(ROBOTNAME)
{
std::cout << __FILE__ << " " << __FUNCTION__ <<":" << getClassName() << std::endl;
startupPython();
}
void SoTHRP2_10_Controller::startupPython()
{
SoTHRP2Controller::startupPython();
std::ofstream aof(LOG_PYTHON_10.c_str());
runPython
(aof,
"from dynamic_graph.sot.hrp2_14.prologue import robot, solver",
interpreter_);
interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
aof.close();
}
extern "C"
{
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
return new SoTHRP2_10_Controller;
}
}
extern "C"
{
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
{
delete p;
}
}
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#ifndef _SOT_HRP2_10_Controller_H_
#define _SOT_HRP2_10_Controller_H_
#include "sot-hrp2-controller.hh"
namespace dgsot=dynamicgraph::sot;
class SoTHRP2_10_Controller: public SoTHRP2Controller
{
public:
static const std::string LOG_PYTHON_10;
SoTHRP2_10_Controller();
virtual ~SoTHRP2_10_Controller() {};
protected:
virtual void startupPython();
};
#endif /* _SOT_HRP2Controller_H_ */
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#include <sot/core/debug.hh>
#define ROBOTNAME std::string("HRP2JRL")
#include "sot-hrp2-14-controller.hh"
const std::string SoTHRP2_14_Controller::LOG_PYTHON_14="/tmp/HRP2Controller_14_python.out";
SoTHRP2_14_Controller::SoTHRP2_14_Controller():
SoTHRP2Controller(ROBOTNAME)
{
startupPython();
}
void SoTHRP2_14_Controller::startupPython()
{
SoTHRP2Controller::startupPython();
std::ofstream aof(LOG_PYTHON_14.c_str());
runPython
(aof,
"from dynamic_graph.sot.hrp2_14.prologue import robot, solver",
interpreter_);
interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
}
extern "C"
{
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
return new SoTHRP2_14_Controller;
}
}
extern "C"
{
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
{
delete p;
}
}
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#ifndef _SOT_HRP2_14_Controller_H_
#define _SOT_HRP2_14_Controller_H_
#include "sot-hrp2-controller.hh"
namespace dgsot=dynamicgraph::sot;
class SoTHRP2_14_Controller: public SoTHRP2Controller
{
public:
static const std::string LOG_PYTHON_14;
SoTHRP2_14_Controller();
virtual ~SoTHRP2_14_Controller() {};
protected:
virtual void startupPython();
};
#endif /* _SOT_HRP2Controller_H_ */
......@@ -20,7 +20,6 @@
#include "sot-hrp2-controller.hh"
#define ENTITYNAME std::string("DLRBiped")
const std::string SoTHRP2Controller::CLASS_NAME = "Device";
const std::string SoTHRP2Controller::LOG_PYTHON="/tmp/HRP2Controller_python.out";
......@@ -28,17 +27,16 @@ const double SoTHRP2Controller::TIMESTEP_DEFAULT = 0.005;
using namespace std;
SoTHRP2Controller::SoTHRP2Controller():
dgsot::Device(ENTITYNAME),
SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
dgsot::Device(RobotName),
timestep_ (TIMESTEP_DEFAULT),
previousState_ (),
robotState_ ("StackOfTasks(" + ENTITYNAME + ")::output(vector)::robotState"),
robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState"),
interpreter_()
{
signalRegistration (robotState_);
std::cout << __FILE__ << ":" << __FUNCTION__ <<"(#" << __LINE__ << " )" << std::endl;
startupPython();
}
......@@ -212,25 +210,7 @@ void SoTHRP2Controller::startupPython()
" path.append(p)", interpreter_);
runPython (aof, "path.extend(sys.path)", interpreter_);
runPython (aof, "sys.path = path", interpreter_);
runPython
(aof,
"from dynamic_graph.sot.openhrp.prologue_dlr_biped import robot, solver",
interpreter_);
interpreter_.startCorbaServer ("openhrp", "", "stackOfTasks", "");
aof.close();
}
extern "C"
{
dgsot::AbstractSotExternalInterface * createSotExternalInterface()
{
return new SoTHRP2Controller;
}
}
extern "C"
{
void destroySotExternalInterface(dgsot::AbstractSotExternalInterface *p)
{
delete p;
}
}
......@@ -43,8 +43,8 @@ class SoTHRP2Controller: public
return CLASS_NAME;
}
SoTHRP2Controller();
~SoTHRP2Controller();
SoTHRP2Controller(std::string RobotName);
virtual ~SoTHRP2Controller();
void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
......@@ -54,7 +54,7 @@ class SoTHRP2Controller: public
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
private:
protected:
// Update output port with the control computed from the
// dynamic graph.
void updateRobotState(std::vector<double> &anglesIn);
......@@ -63,7 +63,7 @@ class SoTHRP2Controller: public
const std::string& command,
dynamicgraph::corba::Interpreter& interpreter);
void startupPython();
virtual void startupPython();
/// \brief Current integration step.
......
Markdown is supported
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