Commit 5f21c47f authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Set default values for the force sensors.

parent b3e13023
...@@ -85,6 +85,10 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName): ...@@ -85,6 +85,10 @@ SoTTalosDevice::SoTTalosDevice(std::string RobotName):
accelerometerSOUT_.setConstant (data); accelerometerSOUT_.setConstant (data);
gyrometerSOUT_.setConstant (data); gyrometerSOUT_.setConstant (data);
baseff_.resize(7); baseff_.resize(7);
dg::Vector dataForces(6); dataForces.setZero ();
for(int i=0;i<4;i++)
forcesSOUT[i]->setConstant(dataForces);
using namespace dynamicgraph::command; using namespace dynamicgraph::command;
std::string docstring; std::string docstring;
/* Command increment. */ /* Command increment. */
...@@ -105,6 +109,7 @@ SoTTalosDevice::~SoTTalosDevice() ...@@ -105,6 +109,7 @@ SoTTalosDevice::~SoTTalosDevice()
void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t) void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, int t)
{ {
sotDEBUGIN(15);
map<string,dgsot::SensorValues>::iterator it; map<string,dgsot::SensorValues>::iterator it;
it = SensorsIn.find("forces"); it = SensorsIn.find("forces");
if (it!=SensorsIn.end()) if (it!=SensorsIn.end())
...@@ -114,17 +119,17 @@ void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn, ...@@ -114,17 +119,17 @@ void SoTTalosDevice::setSensorsForce(map<string,dgsot::SensorValues> &SensorsIn,
const vector<double>& forcesIn = it->second.getValues(); const vector<double>& forcesIn = it->second.getValues();
for(int i=0;i<4;++i) for(int i=0;i<4;++i)
{ {
std::cout << "Force sensor " << i << std::endl; sotDEBUG(15) << "Force sensor " << i << std::endl;
for(int j=0;j<6;++j) for(int j=0;j<6;++j)
{ {
dgforces_(j) = forcesIn[i*6+j]; dgforces_(j) = forcesIn[i*6+j];
std::cout << "Force value " << j << ":" << dgforces_(j) << std::endl; sotDEBUG(15) << "Force value " << j << ":" << dgforces_(j) << std::endl;
} }
forcesSOUT[i]->setConstant(dgforces_); forcesSOUT[i]->setConstant(dgforces_);
forcesSOUT[i]->setTime (t); forcesSOUT[i]->setTime (t);
} }
} }
else std::cout << "No forces in the map" << std::endl; sotDEBUGIN(15);
} }
void SoTTalosDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn, int t) void SoTTalosDevice::setSensorsIMU(map<string,dgsot::SensorValues> &SensorsIn, int t)
...@@ -368,3 +373,30 @@ void SoTTalosDevice::getControl(map<string,dgsot::ControlValues> &controlOut) ...@@ -368,3 +373,30 @@ void SoTTalosDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
ODEBUG5FULL("end"); ODEBUG5FULL("end");
sotDEBUGOUT(25) ; sotDEBUGOUT(25) ;
} }
using namespace dynamicgraph::sot;
namespace dynamicgraph { namespace sot {
#ifdef WIN32
const char * DebugTrace::DEBUG_FILENAME_DEFAULT = "c:/tmp/sot-core-traces.txt";
#else // WIN32
const char * DebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/sot-core-traces.txt";
#endif // WIN32
#ifdef VP_DEBUG
# ifdef WIN32
std::ofstream debugfile("C:/tmp/sot-core-traces.txt",
std::ios::trunc&std::ios::out);
# else // WIN32
std::ofstream debugfile("/tmp/sot-core-traces.txt",
std::ios::trunc&std::ios::out);
# endif // WIN32
#else // VP_DEBUG
std::ofstream debugfile;
#endif // VP_DEBUG
} /* namespace sot */} /* namespace dynamicgraph */
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