Unverified Commit 56304929 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #35 from nim65s/topic/delete-shell

Remove shell
parents 4d0349ba d5062fa6
Pipeline #1209 passed with stage
in 9 minutes and 37 seconds
......@@ -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_;
......
......@@ -67,10 +67,10 @@
#endif
namespace dynamicgraph {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace command {
class SetFile;
class CreateOpPoint;
......@@ -78,9 +78,9 @@ namespace dynamicgraph {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup signals
\brief This class provides an inverse dynamic model of the robot.
More precisely it wraps the newton euler algorithm implemented
......@@ -92,8 +92,8 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
friend class sot::command::SetFile;
friend class sot::command::CreateOpPoint;
// friend class sot::command::InitializeRobot;
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DYNAMIC_GRAPH_ENTITY_DECL();
......@@ -110,15 +110,15 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg::SignalTimeDependent< dg::Matrix,int >&
createJacobianSignal( const std::string& signame, const std::string& );
void destroyJacobianSignal( const std::string& signame );
dg::SignalTimeDependent< MatrixHomogeneous,int >&
createPositionSignal( const std::string&,const std::string& );
void destroyPositionSignal( const std::string& signame );
dg::SignalTimeDependent< dg::Vector,int >&
createVelocitySignal( const std::string&,const std::string& );
void destroyVelocitySignal( const std::string& signame );
dg::SignalTimeDependent< dg::Vector,int >&
createAccelerationSignal( const std::string&, const std::string& );
void destroyAccelerationSignal( const std::string& signame );
......@@ -137,7 +137,7 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg::SignalPtr<dg::Vector,int> freeFlyerVelocitySIN;
dg::SignalPtr<dg::Vector,int> jointAccelerationSIN;
dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN;
dg::SignalTimeDependent<dg::Vector,int> pinocchioPosSINTERN;
dg::SignalTimeDependent<dg::Vector,int> pinocchioVelSINTERN;
dg::SignalTimeDependent<dg::Vector,int> pinocchioAccSINTERN;
......@@ -151,17 +151,17 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
int& computeForwardKinematics(int& dummy,const int& time );
int& computeCcrba( int& dummy,const int& time );
int& computeJacobians( int& dummy,const int& time );
dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
dg::SignalTimeDependent<dg::Matrix,int> JcomSOUT;
dg::SignalTimeDependent<dg::Vector,int> comSOUT;
dg::SignalTimeDependent<dg::Matrix,int> inertiaSOUT;
dg::SignalTimeDependent<dg::Matrix,int>& jacobiansSOUT( const std::string& name );
dg::SignalTimeDependent<MatrixHomogeneous,int>& positionsSOUT( const std::string& name );
dg::SignalTimeDependent<dg::Vector,int>& velocitiesSOUT( const std::string& name );
dg::SignalTimeDependent<dg::Vector,int>& accelerationsSOUT( const std::string& name );
dg::SignalTimeDependent<double,int> footHeightSOUT;
dg::SignalTimeDependent<dg::Vector,int> upperJlSOUT;
dg::SignalTimeDependent<dg::Vector,int> lowerJlSOUT;
......@@ -184,14 +184,14 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
/* --- MODEL CREATION --- */
void displayModel() const
void displayModel() const
{ assert(m_model); std::cout<<(*m_model)<<std::endl; };
void setModel(se3::Model*);
void setData(se3::Data*);
/* --- GETTERS --- */
/// \brief Get joint position lower limits
......@@ -228,7 +228,7 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
const int jointId,
dg::Matrix& res,const int& time );
MatrixHomogeneous& computeGenericPosition(const bool isFrame,
MatrixHomogeneous& computeGenericPosition(const bool isFrame,
const int jointId,
MatrixHomogeneous& res,const int& time );
dg::Vector& computeGenericVelocity(const int jointId,dg::Vector& res,const int& time );
......@@ -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 );
......@@ -258,11 +255,11 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
private:
/// \brief map of joints in construction.
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
dg::Vector& getPinocchioPos(dg::Vector& q,const int& time);
dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
dg::Vector& getPinocchioAcc(dg::Vector& a, const int&time);
//\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
std::vector<int> sphericalJoints;
......
......@@ -43,12 +43,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (force_compensation_EXPORTS)
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
# else
# else
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTFORCECOMPENSATION_EXPORT
#endif
......@@ -70,12 +70,12 @@ namespace dynamicgraph { namespace sot {
public:
ForceCompensation( void );
static MatrixForce& computeHandXworld(
static MatrixForce& computeHandXworld(
const MatrixRotation & worldRhand,
const dynamicgraph::Vector & transSensorCom,
MatrixForce& res );
static MatrixForce& computeHandVsensor( const MatrixRotation & sensorRhand,
MatrixForce& res );
static MatrixForce& computeSensorXhand( const MatrixRotation & sensorRhand,
......@@ -106,7 +106,7 @@ namespace dynamicgraph { namespace sot {
static dynamicgraph::Vector& computeDeadZone( const dynamicgraph::Vector& torqueInput,
const dynamicgraph::Vector& deadZoneLimit,
dynamicgraph::Vector& res );
public: // CALIBRATION
std::list<dynamicgraph::Vector> torsorList;
......@@ -115,15 +115,15 @@ namespace dynamicgraph { namespace sot {
void clearCalibration( void );
void addCalibrationValue( const dynamicgraph::Vector& torsor,
const MatrixRotation & worldRhand );
dynamicgraph::Vector calibrateTransSensorCom( const dynamicgraph::Vector& gravity,
const MatrixRotation& handRsensor );
dynamicgraph::Vector calibrateGravity( const MatrixRotation& handRsensor,
bool precompensationCalibration = false,
const MatrixRotation& hand0Rsensor = I3 );
};
/* --------------------------------------------------------------------- */
......@@ -147,50 +147,43 @@ namespace dynamicgraph { namespace sot {
public: /* --- SIGNAL --- */
/* --- INPUTS --- */
dg::SignalPtr<dynamicgraph::Vector,int> torsorSIN;
dg::SignalPtr<MatrixRotation,int> worldRhandSIN;
dg::SignalPtr<dynamicgraph::Vector,int> torsorSIN;
dg::SignalPtr<MatrixRotation,int> worldRhandSIN;
/* --- CONSTANTS --- */
dg::SignalPtr<MatrixRotation,int> handRsensorSIN;
dg::SignalPtr<dynamicgraph::Vector,int> translationSensorComSIN;
dg::SignalPtr<dynamicgraph::Vector,int> gravitySIN;
dg::SignalPtr<dynamicgraph::Vector,int> precompensationSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> gainSensorSIN;
dg::SignalPtr<dynamicgraph::Vector,int> deadZoneLimitSIN;
dg::SignalPtr<dynamicgraph::Vector,int> transSensorJointSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaJointSIN;
dg::SignalPtr<dynamicgraph::Vector,int> velocitySIN;
dg::SignalPtr<dynamicgraph::Vector,int> accelerationSIN;
dg::SignalPtr<MatrixRotation,int> handRsensorSIN;
dg::SignalPtr<dynamicgraph::Vector,int> translationSensorComSIN;
dg::SignalPtr<dynamicgraph::Vector,int> gravitySIN;
dg::SignalPtr<dynamicgraph::Vector,int> precompensationSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> gainSensorSIN;
dg::SignalPtr<dynamicgraph::Vector,int> deadZoneLimitSIN;
dg::SignalPtr<dynamicgraph::Vector,int> transSensorJointSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaJointSIN;
dg::SignalPtr<dynamicgraph::Vector,int> velocitySIN;
dg::SignalPtr<dynamicgraph::Vector,int> accelerationSIN;
/* --- INTERMEDIATE OUTPUTS --- */
dg::SignalTimeDependent<MatrixForce,int> handXworldSOUT;
dg::SignalTimeDependent<MatrixForce,int> handVsensorSOUT;
dg::SignalPtr<dynamicgraph::Vector,int> torsorDeadZoneSIN;
dg::SignalTimeDependent<MatrixForce,int> handXworldSOUT;
dg::SignalTimeDependent<MatrixForce,int> handVsensorSOUT;
dg::SignalPtr<dynamicgraph::Vector,int> torsorDeadZoneSIN;
dg::SignalTimeDependent<MatrixForce,int> sensorXhandSOUT;
//dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> momentumSOUT;
dg::SignalPtr<dynamicgraph::Vector,int> momentumSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> momentumSOUT;
dg::SignalPtr<dynamicgraph::Vector,int> momentumSIN;
/* --- OUTPUTS --- */
dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorCompensatedSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorCompensatedSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> torsorDeadZoneSOUT;
typedef int sotDummyType;
dg::SignalTimeDependent<sotDummyType,int> calibrationTrigerSOUT;
dg::SignalTimeDependent<sotDummyType,int> calibrationTrigerSOUT;
public: /* --- COMMANDLINE --- */
sotDummyType& calibrationTriger( sotDummyType& dummy,int time );
virtual void commandLine( const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os );
};
......
......@@ -44,12 +44,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (integrator_force_exact_EXPORTS)
# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllexport)
# else
# else
# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTINTEGRATORFORCEEXACT_EXPORT
#endif
......@@ -81,12 +81,6 @@ class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeVelocityExact( dynamicgraph::Vector& res,
const int& time );
/* public: /\* --- PARAMS --- *\/ */
/* virtual void commandLine( const std::string& cmdLine, */
/* std::istringstream& cmdArgs, */
/* std::ostream& os ); */
};
......
......@@ -44,12 +44,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (integrator_force_rk4_EXPORTS)
# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllexport)
# else
# else
# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTINTEGRATORFORCERK4_EXPORT
#endif
......@@ -81,12 +81,6 @@ class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeDerivativeRK4( dynamicgraph::Vector& res,
const int& time );
/* public: /\* --- PARAMS --- *\/ */
/* virtual void commandLine( const std::string& cmdLine, */
/* std::istringstream& cmdArgs, */
/* std::ostream& os ); */
};
......
......@@ -43,12 +43,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (integrator_force_EXPORTS)
# define SOTINTEGRATORFORCE_EXPORT __declspec(dllexport)
# else
# else
# define SOTINTEGRATORFORCE_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTINTEGRATORFORCE_EXPORT
#endif
......@@ -80,18 +80,18 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce
public: /* --- SIGNAL --- */
dg::SignalPtr<dynamicgraph::Vector,int> forceSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> massInverseSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> frictionSIN;
dg::SignalPtr<dynamicgraph::Vector,int> forceSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> massInverseSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> frictionSIN;
/* Memory of the previous iteration. The sig is fed by the previous
* computations. */
dg::SignalPtr<dynamicgraph::Vector,int> velocityPrecSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> velocityDerivativeSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> velocitySOUT;
dg::SignalPtr<dynamicgraph::Vector,int> velocityPrecSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> velocityDerivativeSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> velocitySOUT;
dg::SignalPtr<dynamicgraph::Matrix,int> massSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> massInverseSOUT;
dg::SignalPtr<dynamicgraph::Matrix,int> massSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> massInverseSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeDerivative( dynamicgraph::Vector& res,
......@@ -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 );
};
......
......@@ -42,12 +42,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (waist_attitude_from_sensor_EXPORTS)
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllexport)
# else
# else
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT
#endif
......@@ -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 );
};
......
......@@ -42,12 +42,12 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
#if defined (WIN32)
# if defined (zmpreffromcom_EXPORTS)
# define SOTZMPREFFROMCOM_EXPORT __declspec(dllexport)
# else
# else
# define SOTZMPREFFROMCOM_EXPORT __declspec(dllimport)
# endif
# endif
#else
# define SOTZMPREFFROMCOM_EXPORT
#endif
......@@ -69,7 +69,7 @@ class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
double dt;
const static double DT_DEFAULT; // = 5e-3; // 5ms
double footHeight;
const static double FOOT_HEIGHT_DEFAULT; // = .105;
const static double FOOT_HEIGHT_DEFAULT; // = .105;
public: /* --- CONSTRUCTION --- */
......@@ -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 );
};
......
......@@ -315,7 +315,7 @@ computeWaistWorldPosition( MatrixHomogeneous& res,
if( fromSensor_ )
{
const MatrixRotation & Rflex = flexibilitySOUT( time ); // footRleg
MatrixHomogeneous footMleg;
MatrixHomogeneous footMleg;
footMleg.linear() = Rflex; footMleg.translation().setZero();
tmpRes = footMleg*legMwaist;
......@@ -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); }
}
......@@ -331,7 +331,7 @@ computeHandXworld( const MatrixRotation & worldRhand,
sotDEBUG(25) << "wRrh = " << worldRhand <<std::endl;
sotDEBUG(25) << "SC = " << transSensorCom <<std::endl;
MatrixHomogeneous scRw;
MatrixHomogeneous scRw;
scRw.linear()=worldRhand.transpose(); scRw.translation()=transSensorCom;
sotDEBUG(25) << "scMw = " << scRw <<std::endl;
......@@ -344,7 +344,7 @@ computeHandXworld( const MatrixRotation & worldRhand,
_t(2), 0, -_t(0),
-_t(1), _t(0), 0;
Eigen::Matrix3d sk; sk = Tx*R;
res.block<3,3>(0,0) = R;
res.block<3,3>(0,3).setZero();
res.block<3,3>(3,0) = sk;
......@@ -395,7 +395,7 @@ computeSensorXhand( const MatrixRotation & /*handRsensor*/,
Tx << 0, -transJointSensor(2), transJointSensor(1),
transJointSensor(2), 0, -transJointSensor(0),
-transJointSensor(1), transJointSensor(0), 0;
res.block<3,3>(0,0).setIdentity();
res.block<3,3>(0,3).setZero();
res.block<3,3>(3,0) = Tx;
......@@ -444,7 +444,7 @@ computeTorsorCompensated( const dynamicgraph::Vector& torqueInput,
sotDEBUG(25) << "t_nc = " << torqueInput;
dynamicgraph::Vector torquePrecompensated(6);
//if( usingPrecompensation )
torquePrecompensated = torqueInput+torquePrecompensation;
torquePrecompensated = torqueInput+torquePrecompensation;
//else { torquePrecompensated = torqueInput; }
sotDEBUG(25) << "t_pre = " << torquePrecompensated;
......@@ -496,7 +496,7 @@ crossProduct_V_F( const dynamicgraph::Vector& velocity,
}
return res;
}
dynamicgraph::Vector& ForceCompensation::
computeMomentum( const dynamicgraph::Vector& velocity,
......@@ -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 )
// {