Commit d5062fa6 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files
parent 4d0349ba
......@@ -114,9 +114,6 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator
public: /* --- PARAMS --- */
void fromSensor(const bool& fs) { fromSensor_ = fs; }
bool fromSensor() const { return fromSensor_; }
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
private:
bool fromSensor_;
......
......@@ -246,9 +246,6 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg::Vector& computeTorqueDrift( dg::Vector& res,const int& time );
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
void cmd_createOpPointSignals ( const std::string& sig,const std::string& j );
void cmd_createJacobianWorldSignal ( const std::string& sig,const std::string& j );
void cmd_createJacobianEndEffectorSignal( const std::string& sig,const std::string& j );
......
......@@ -184,13 +184,6 @@ namespace dynamicgraph { namespace sot {
sotDummyType& calibrationTriger( sotDummyType& dummy,int time );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
......
......@@ -82,12 +82,6 @@ class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
dynamicgraph::Vector& computeVelocityExact( dynamicgraph::Vector& res,
const int& time );
/* public: /\* --- PARAMS --- *\/ */
/* virtual void commandLine( const std::string& cmdLine, */
/* std::istringstream& cmdArgs, */
/* std::ostream& os ); */
};
......
......@@ -82,12 +82,6 @@ class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4
dynamicgraph::Vector& computeDerivativeRK4( dynamicgraph::Vector& res,
const int& time );
/* public: /\* --- PARAMS --- *\/ */
/* virtual void commandLine( const std::string& cmdLine, */
/* std::istringstream& cmdArgs, */
/* std::ostream& os ); */
};
......
......@@ -102,13 +102,6 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce
dynamicgraph::Matrix& computeMassInverse( dynamicgraph::Matrix& res,
const int& time );
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
......
......@@ -81,13 +81,6 @@ class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistAttitudeFromSensor
dg::SignalPtr<MatrixHomogeneous,int> positionSensorSIN;
dg::SignalTimeDependent<VectorRollPitchYaw,int> attitudeWaistSOUT;
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
......@@ -120,13 +113,6 @@ class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistPoseFromSensorAndContact
dg::SignalPtr<MatrixHomogeneous,int> positionContactSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> positionWaistSOUT;
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
......
......@@ -86,13 +86,6 @@ class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
dg::SignalPtr<dynamicgraph::Vector,int> dcomSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> zmprefSOUT;
public: /* --- PARAMS --- */
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
......
......@@ -384,31 +384,3 @@ compute_qdotSOUT( dynamicgraph::Vector& res,
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
void AngleEstimator::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
if( cmdLine == "help" )
{
Entity::commandLine(cmdLine,cmdArgs,os);
}
else if( cmdLine == "fromSensor" )
{
std::string val; cmdArgs>>val;
if( ("true"==val)||("false"==val) )
{
fromSensor_ = ( val=="true" );
} else {
os << "fromSensor = " << (fromSensor_?"true":"false") << std::endl;
}
}
else { Entity::commandLine( cmdLine,cmdArgs,os); }
}
......@@ -553,115 +553,3 @@ calibrationTriger( ForceCompensationPlugin::sotDummyType& dummy,int /*time*/ )
// sotDEBUGOUT(45);
return dummy=1;
}
/* --- COMMANDLINE ---------------------------------------------------------- */
/* --- COMMANDLINE ---------------------------------------------------------- */
/* --- COMMANDLINE ---------------------------------------------------------- */
void ForceCompensationPlugin::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
if( "help"==cmdLine )
{
os << "ForceCompensation: "
<< " - clearCalibration" << std::endl
<< " - {start|stop}Calibration [wait <time_sec>]" << std::endl
<< " - calibrateGravity\t[only {x|y|z}]" << std::endl
<< " - calibratePosition" << std::endl
<< " - precomp [{true|false}]: get/set the "
<< "precompensation due to sensor calib." << std::endl;
}
// else if( "clearCalibration" == cmdLine )
// {
// clearCalibration();
// }
// else if( "startCalibration" == cmdLine )
// {
// calibrationStarted = true;
// cmdArgs >> std::ws;
// if( cmdArgs.good() )
// {
// std::string cmdWait; cmdArgs>>cmdWait>>std::ws;
// if( (cmdWait == "wait")&&(cmdArgs.good()) )
// {
// double timeSec; cmdArgs >> timeSec;
// unsigned int timeMSec= (unsigned int)(round(timeSec*1000*1000));
// sotDEBUG(15) << "Calibration: wait for " << timeMSec << "us."<<std::endl;
// usleep( timeMSec );
// calibrationStarted = false;
// }
// }
// }
// else if( "stopCalibration" == cmdLine )
// {
// calibrationStarted = false;
// }
// else if( "calibrateGravity" == cmdLine )
// {
// if( calibrationStarted )
// {
// os<< "Calibration phase is on, stop it first."<<std::endl;
// return;
// }
// dynamicgraph::Vector grav = calibrateGravity( handRsensorSIN.accessCopy(),
// usingPrecompensation );
// cmdArgs >> std::ws;
// if( cmdArgs.good() )
// {
// std::string cmdOnly; cmdArgs>>cmdOnly>>std::ws;
// if( (cmdOnly == "only")&&(cmdArgs.good()) )
// {
// std::string xyz; cmdArgs >> xyz;
// if( "x"==xyz ) { grav(1)=grav(2)=0.; }
// else if( "y"==xyz ) { grav(0)=grav(2)=0.; }
// else if( "z"==xyz ) { grav(0)=grav(1)=0.; }
// }
// }
// gravitySIN = grav;
// }
// else if( "calibratePosition" == cmdLine )
// {
// if( calibrationStarted )
// {
// return;
// os<< "Calibration phase is on, stop it first."<<std::endl;
// }
// dynamicgraph::Vector position(3);
// position = calibrateTransSensorCom( gravitySIN.accessCopy(),
// handRsensorSIN.accessCopy() );
// transSensorComSIN = position;
// }
else if( "precomp" == cmdLine )
{
cmdArgs>>std::ws;
if( cmdArgs.good() )
{ cmdArgs >> usingPrecompensation; }
else { os << "precompensation = " << usingPrecompensation <<std::endl; }
}
else if( "compensateMomentum" == cmdLine )
{
cmdArgs>>std::ws;
if( cmdArgs.good() )
{
bool use; cmdArgs >> use;
if( use ) momentumSIN.plug( &momentumSOUT );
else
{
dynamicgraph::Vector m(6); m.resize(0); momentumSIN = m;
}
}
else
{
os << "compensateMomentum = " << (momentumSIN.getPtr()!=&momentumSIN)
<<std::endl;
}
}
else{ Entity::commandLine( cmdLine,cmdArgs,os ); }
}
......@@ -230,22 +230,3 @@ computeVelocityExact( dynamicgraph::Vector& res,
sotDEBUGOUT(15);
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
// void IntegratorForceExact::
// commandLine( const std::string& cmdLine,
// std::istringstream& cmdArgs,
// std::ostream& os )
// {
// sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
// if( cmdLine == "help" )
// {
// os << "IntegratorForceExact: " << std::endl;
// }
// else { IntegratorForce::commandLine( cmdLine,cmdArgs,os); }
// }
......@@ -116,22 +116,3 @@ computeDerivativeRK4( dynamicgraph::Vector& res,
sotDEBUGOUT(15);
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
// void IntegratorForceRK4::
// commandLine( const std::string& cmdLine,
// std::istringstream& cmdArgs,
// std::ostream& os )
// {
// sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
// if( cmdLine == "help" )
// {
// os << "IntegratorForceRK4: " << std::endl;
// }
// else { IntegratorForce::commandLine( cmdLine,cmdArgs,os); }
// }
......@@ -144,33 +144,3 @@ computeMassInverse( dynamicgraph::Matrix& res,
sotDEBUGOUT(15);
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
void IntegratorForce::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
if( cmdLine == "help" )
{
os << "IntegratorForce: "
<< " - dt [<time>]: get/set the timestep value. " << std::endl;
}
else if( cmdLine == "dt" )
{
cmdArgs >>std::ws;
if( cmdArgs.good() )
{
double val; cmdArgs>>val;
if( val >0. ) timeStep = val;
}
else { os << "TimeStep = " << timeStep << std::endl;}
}
else { Entity::commandLine( cmdLine,cmdArgs,os); }
}
......@@ -1048,117 +1048,6 @@ accelerationsSOUT( const std::string& name )
/*-------------------------------------------------------------------------*/
/* --- PARAMS --------------------------------------------------------------- */
void DynamicPinocchio::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUG(25) << "# In { Cmd " << cmdLine <<std::endl;
std::string filename;
if( cmdLine == "displayModel" ) {
displayModel();
}
else if( cmdLine == "createJacobian" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
createJacobianSignal(signame,jointName);
}
else if( cmdLine == "destroyJacobian" ) {
std::string Jname; cmdArgs >> Jname;
destroyJacobianSignal(Jname);
}
else if( cmdLine == "createPosition" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
createPositionSignal(signame,jointName);
}
else if( cmdLine == "destroyPosition" ) {
std::string Jname; cmdArgs >> Jname;
destroyPositionSignal(Jname);
}
else if( cmdLine == "createVelocity" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
createVelocitySignal(signame,jointName);
}
else if( cmdLine == "destroyVelocity" ) {
std::string Jname; cmdArgs >> Jname;
destroyVelocitySignal(Jname);
}
else if( cmdLine == "createAcceleration" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
createAccelerationSignal(signame,jointName);
}
else if( cmdLine == "destroyAcceleration" ) {
std::string Jname; cmdArgs >> Jname;
destroyAccelerationSignal(Jname);
}
else if( cmdLine == "createEndeffJacobian" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
createEndeffJacobianSignal(signame,jointName);
}
else if( cmdLine == "createOpPoint" ) {
std::string signame; cmdArgs >> signame;
std::string jointName; cmdArgs >> jointName;
createEndeffJacobianSignal(std::string("J")+signame,jointName);
createPositionSignal(signame,jointName);
sotDEBUG(15)<<std::endl;
}
else if( cmdLine == "destroyOpPoint" ) {
std::string Jname; cmdArgs >> Jname;
destroyJacobianSignal(std::string("J")+Jname);
destroyPositionSignal(Jname);
}
else if( cmdLine == "ndof" ) {
os << "Number of Degree of freedom:" <<m_data->J.cols() << std::endl;
return;
}
else if( cmdLine == "help" ) {
os << "Dynamics:"<<std::endl
<< " - displayModel\t:display the current model configuration" <<std::endl
<< " - createJacobian <name> <point>:create a signal named <name> " << std::endl
<< " - destroyJacobian <name>\t:delete the jacobian signal <name>" << std::endl
<< " - createEndeffJacobian <name> <point>:create a signal named <name> "
<< "forwarding the jacobian computed at <point>." <<std::endl
<< " - {create|destroy}Position\t:handle position signals." <<std::endl
<< " - {create|destroy}OpPoint\t:handle Operation Point (ie pos+jac) signals." <<std::endl
<< " - {create|destroy}Acceleration\t:handle acceleration signals." <<std::endl
/*TODO: Put these flags for computations (copied from humanoid_robot.py):
def setProperties(self, model):
model.setProperty('TimeStep', str(self.timeStep))
model.setProperty('ComputeAcceleration', 'false')
model.setProperty('ComputeAccelerationCoM', 'false')
model.setProperty('ComputeBackwardDynamics', 'false')
model.setProperty('ComputeCoM', 'false')
model.setProperty('ComputeMomentum', 'false')
model.setProperty('ComputeSkewCom', 'false')
model.setProperty('ComputeVelocity', 'false')
model.setProperty('ComputeZMP', 'false')
model.setProperty('ComputeAccelerationCoM', 'true')
model.setProperty('ComputeCoM', 'true')
model.setProperty('ComputeVelocity', 'true')
model.setProperty('ComputeZMP', 'true')
if self.enableZmpComputation:
model.setProperty('ComputeBackwardDynamics', 'true')
model.setProperty('ComputeAcceleration', 'true')
model.setProperty('ComputeMomentum', 'true')
*/
// << " - {get|set}Property <name> [<val>]: set/get the property." <<std::endl
// << " - displayProperties: print the prop-val couples list." <<std::endl
<< " - ndof\t\t\t: display the number of DOF of the robot."<< std::endl;
Entity::commandLine(cmdLine,cmdArgs,os);
}
else {
Entity::commandLine( cmdLine,cmdArgs,os); }
sotDEBUGOUT(15);
}
//jointName is either a fixed-joint (pinocchio operational frame) or a
//movable joint (pinocchio joint-variant).
......
......@@ -80,26 +80,6 @@ computeAttitudeWaist( VectorRollPitchYaw & res,
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
void WaistAttitudeFromSensor::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
if( cmdLine == "help" )
{
Entity::commandLine(cmdLine,cmdArgs,os);
}
else { Entity::commandLine( cmdLine,cmdArgs,os); }
}
/* === WaistPoseFromSensorAndContact ===================================== */
/* === WaistPoseFromSensorAndContact ===================================== */
/* === WaistPoseFromSensorAndContact ===================================== */
......@@ -195,35 +175,3 @@ computePositionWaist( dynamicgraph::Vector& res,
sotDEBUGOUT(15);
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
void WaistPoseFromSensorAndContact::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
if( cmdLine == "help" )
{
os <<"WaistPoseFromSensorAndContact:"<<std::endl
<<" - fromSensor [true|false]: get/set the flag." << std::endl;
WaistAttitudeFromSensor::commandLine(cmdLine,cmdArgs,os);
}
else if( cmdLine == "fromSensor" )
{
std::string val; cmdArgs>>val;
if( ("true"==val)||("false"==val) )
{
fromSensor_ = ( val=="true" );
} else {
os << "fromSensor = " << (fromSensor_?"true":"false") << std::endl;
}
}
else { WaistAttitudeFromSensor::commandLine( cmdLine,cmdArgs,os); }
}
......@@ -84,39 +84,3 @@ computeZmpref( dynamicgraph::Vector& res,
sotDEBUGOUT(15);
return res;
}
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
void ZmprefFromCom::
commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
sotDEBUG(25) << "Cmd " << cmdLine <<std::endl;
if( cmdLine == "help" )
{
os<<"zmprefFromCom:" << std::endl
<<" - dt [<value>]: get/set the dt value." << std::endl
<<" - footHeight [<value>]: "
<<"get/set the height of the foot ("
<< FOOT_HEIGHT_DEFAULT <<" by default)." <<std::endl;
Entity::commandLine(cmdLine,cmdArgs,os);
}
if( cmdLine == "dt" )
{
cmdArgs>>std::ws;
if( cmdArgs.good() ) cmdArgs>>dt; else os <<"dt = " << dt<<std::endl;
}
if( cmdLine == "footHeight" )
{
cmdArgs>>std::ws;
if( cmdArgs.good() ) cmdArgs>>footHeight;
else os <<"footHeight = " << footHeight<<std::endl;
}
else { Entity::commandLine( cmdLine,cmdArgs,os); }
}
......@@ -63,8 +63,6 @@ int main(int argc, char * argv[])
istringstream iss;
string help("help");
// Commented out to make this sample a unit test
// dyn->commandLine(help,iss,cout);
delete dyn;
return 0;
......
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