Commit 16ca6f24 authored by olivier stasse's avatar olivier stasse
Browse files

Add proper registration to the pool of classes.

This is mandatory to use the device inside the python interpreter.
In addition this commit provides debugging messages when
compiled with VP_DEBUG and when VP_DEBUG_MODE is greater than 25.
parent 18702203
......@@ -16,7 +16,7 @@
*/
#include <sot/core/debug.hh>
#include <sot/core/exception-abstract.hh>
#include "sot-hrp2-controller.hh"
......@@ -30,8 +30,9 @@ SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
device_(RobotName)
{
std::cout << __FILE__ << ":" << __FUNCTION__ <<"(#"
<< __LINE__ << " )" << std::endl;
sotDEBUG(25) << __FILE__ << ":"
<< __FUNCTION__ <<"(#"
<< __LINE__ << " )" << std::endl;
}
......@@ -62,7 +63,22 @@ cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
void SoTHRP2Controller::
getControl(map<string,dgsot::ControlValues> &controlOut)
{
device_.getControl(controlOut);
try
{
sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
device_.getControl(controlOut);
sotDEBUG(25) << __FILE__ << __FUNCTION__ << "(#" << __LINE__ << ")" << endl;
}
catch ( dynamicgraph::sot::ExceptionAbstract & err)
{
std::cout << __FILE__ << " "
<< __FUNCTION__ << " ("
<< __LINE__ << ") "
<< err.getStringMessage()
<< endl;
throw err;
}
}
void SoTHRP2Controller::
......
......@@ -8,30 +8,50 @@
* 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
* that you either purchased the proper license to have access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#include <fstream>
#include <map>
#include <sot/core/debug.hh>
#include "sot-hrp2-device.hh"
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
using namespace std;
const std::string SoTHRP2Device::CLASS_NAME = "Device";
const double SoTHRP2Device::TIMESTEP_DEFAULT = 0.005;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(SoTHRP2Device,"Device");
SoTHRP2Device::SoTHRP2Device(std::string RobotName):
dgsot::Device(RobotName),
timestep_(TIMESTEP_DEFAULT),
previousState_ (),
robotState_ ("StackOfTasks(" + RobotName + ")::output(vector)::robotState")
{
sotDEBUGIN(25) ;
signalRegistration (robotState_);
using namespace dynamicgraph::command;
std::string docstring;
/* Command increment. */
docstring =
"\n"
" Integrate dynamics for time step provided as input\n"
"\n"
" take one floating point number as input\n"
"\n";
addCommand("increment",
makeCommandVoid1((Device&)*this,
&Device::increment, docstring));
sotDEBUGOUT(25);
}
SoTHRP2Device::~SoTHRP2Device()
......@@ -39,7 +59,7 @@ SoTHRP2Device::~SoTHRP2Device()
void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
sotDEBUGIN(25) ;
vector<double> anglesIn = SensorsIn["joints"].getValues();
// Read state from motor command
......@@ -65,11 +85,13 @@ void SoTHRP2Device::setupSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
}
updateRobotState(anglesIn);
sotDEBUGOUT(25);
}
void SoTHRP2Device::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn)
{
sotDEBUGIN(25) ;
// Read angular values.
vector<double> anglesIn = SensorsIn["joints"].getValues();
updateRobotState(anglesIn);
......@@ -85,11 +107,12 @@ void SoTHRP2Device::nominalSetSensors(map<string,dgsot::SensorValues> &SensorsIn
forcesSOUT[i]->setConstant (mlforces);
forcesSOUT[i]->setTime(t+1);
}
sotDEBUGOUT(25) ;
}
void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsIn)
{
sotDEBUGIN(25) ;
vector<double> anglesIn = SensorsIn["joints"].getValues();
updateRobotState(anglesIn);
......@@ -101,11 +124,13 @@ void SoTHRP2Device::cleanupSetSensors(map<string, dgsot::SensorValues> &SensorsI
mlforces(j) = 0.0;
forcesSOUT[i]->setConstant (mlforces);
}
sotDEBUGOUT(25) ;
}
void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
{
sotDEBUGIN(25) ;
vector<double> anglesOut;
anglesOut.resize(state_.size());
......@@ -166,17 +191,18 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
baseff[i * 4 + j] = freeFlyerPose () (i, j);
controlOut["baseff"].setValues(baseff);
sotDEBUGOUT(25) ;
}
void SoTHRP2Device::updateRobotState(vector<double> &anglesIn)
{
sotDEBUGIN(25) ;
ml::Vector robotState (anglesIn.size () + 6);
for (unsigned i = 0; i < 6; ++i)
robotState (i) = 0.;
for (unsigned i = 0; i < anglesIn.size(); ++i)
robotState (i + 6) = anglesIn[i];
robotState_.setConstant(robotState);
sotDEBUGOUT(25) ;
}
......@@ -73,4 +73,4 @@ protected:
};
#endif _SOT_HRP2Device_H_
#endif /* _SOT_HRP2Device_H_*/
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