Skip to content
Snippets Groups Projects
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
No related branches found
No related tags found
No related merge requests found
Pipeline #1209 passed
Stage: test
Hide whitespace changes
Inline Side-by-side
Showing
with 172 additions and 634 deletions
include/sot-dynamic-pinocchio/angle-estimator.h
+ 0
− 3
View file @ 56304929
......@@ -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_;
......
include/sot-dynamic-pinocchio/dynamic-pinocchio.h
+ 20
− 23
View file @ 56304929
......@@ -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;
......
include/sot-dynamic-pinocchio/force-compensation.h
+ 29
− 36
View file @ 56304929
......@@ -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 );
};
......
include/sot-dynamic-pinocchio/integrator-force-exact.h
+ 3
− 9
View file @ 56304929
......@@ -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 ); */
};
......
include/sot-dynamic-pinocchio/integrator-force-rk4.h
+ 3
− 9
View file @ 56304929
......@@ -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 ); */
};
......
include/sot-dynamic-pinocchio/integrator-force.h
+ 11
− 18
View file @ 56304929
......@@ -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 );
};
......
include/sot-dynamic-pinocchio/waist-attitude-from-sensor.h
+ 3
− 17
View file @ 56304929
......@@ -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 );
};
......
include/sot-dynamic-pinocchio/zmpreffromcom.h
+ 4
− 11
View file @ 56304929
......@@ -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 );
};
......
src/angle-estimator.cpp
+ 1
− 29
View file @ 56304929
......@@ -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); }
}
src/force-compensation.cpp
+ 5
− 117
View file @ 56304929
......@@ -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 )
// {
// 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 ); }
}
src/integrator-force-exact.cpp
+ 22
− 41
View file @ 56304929
......@@ -28,11 +28,11 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceExact,"IntegratorForceExact");
IntegratorForceExact::
IntegratorForceExact( const std::string & name )
IntegratorForceExact( const std::string & name )
:IntegratorForce(name)
{
sotDEBUGIN(5);
velocitySOUT.
setFunction( boost::bind(&IntegratorForceExact::computeVelocityExact,
this,_1,_2));
......@@ -73,38 +73,38 @@ int geev(Matrix &a,
char jobvl='V';
char jobvr='V';
int const n = (int)a.rows();
Vector wr(n);
Vector wi(n);
double* vl_real = MRAWDATA(vl2);
const int ldvl = (int)vl2.rows();
double* vr_real = MRAWDATA(vr2);
const int ldvr = (int)vr2.rows();
// workspace query
int lwork = -1;
double work_temp;
int result=0;
dgeev_(&jobvl, &jobvr, &n,
MRAWDATA(a), &n,
MRAWDATA(wr), MRAWDATA(wi),
MRAWDATA(a), &n,
MRAWDATA(wr), MRAWDATA(wi),
vl_real, &ldvl, vr_real, &ldvr,
&work_temp, &lwork, &result);
if (result != 0)
return result;
lwork = (int) work_temp;
Vector work(lwork);
dgeev_(&jobvl, &jobvr, &n,
MRAWDATA(a), &n,
MRAWDATA(wr), MRAWDATA(wi),
MRAWDATA(a), &n,
MRAWDATA(wr), MRAWDATA(wi),
vl_real, &ldvl, vr_real, &ldvr,
MRAWDATA(work), &lwork,
&result);
for (int i = 0; i < n; i++)
w[i] = std::complex<double>(wr[i], wi[i]);
return result;
}
......@@ -117,7 +117,7 @@ static void eigenDecomp( const dynamicgraph::Matrix& M,
Eigen::VectorXcd evals(SIZE);
Matrix vl(SIZE,SIZE);
Matrix vr(SIZE,SIZE);
// const int errCode = lapack::geev(Y, evals, &vl, &vr, lapack::optimal_workspace());
const int errCode = geev(Y,evals,vl,vr);
if( errCode<0 )
......@@ -126,14 +126,14 @@ static void eigenDecomp( const dynamicgraph::Matrix& M,
else if( errCode>0 )
{ SOT_THROW ExceptionDynamic( ExceptionDynamic::INTEGRATION,
"No convergence for given matrix",""); }
P.resize(SIZE,SIZE); eig.resize(SIZE);
for( unsigned int i=0;i<SIZE;++i )
{
for( unsigned int j=0;j<SIZE;++j ){ P(i,j)=vr(i,j); }
eig(i)=evals(i).real();
if( fabsf(static_cast<float>(evals(i).imag()))>1e-5 )
{
if( fabsf(static_cast<float>(evals(i).imag()))>1e-5 )
{
SOT_THROW ExceptionDynamic( ExceptionDynamic::INTEGRATION,
"Error imaginary part not null. ",
"(at position %d: %f)",i,evals(i).imag() );
......@@ -161,7 +161,7 @@ static void expMatrix( const dynamicgraph::Matrix& MiB,
for( unsigned int j=0;j<SIZE;++j )
Pmib(i,j)*= exp( eig_mib(j) );
Mexp = Pmib*Pinv;
sotDEBUG(45) << "expMiB = " << Mexp;
return ;
}
......@@ -172,7 +172,7 @@ static void expMatrix( const dynamicgraph::Matrix& MiB,
/* The derivative of the signal is such that: M v_dot + B v = f. We deduce:
* v_dot = M^-1 (f - Bv)
* Using Exact method:
* Using Exact method:
* dv = exp( M^-1.B.t) ( v0-B^-1.f ) + B^-1.f
*/
......@@ -189,10 +189,10 @@ computeVelocityExact( dynamicgraph::Vector& res,
res.resize(nv); res.setZero();
if(! velocityPrecSIN )
{
{
dynamicgraph::Vector zero( nv ); zero.fill(0);
velocityPrecSIN = zero;
}
}
const dynamicgraph::Vector & vel = velocityPrecSIN( time );
double & dt = this->IntegratorForce::timeStep; // this is &
......@@ -205,7 +205,7 @@ computeVelocityExact( dynamicgraph::Vector& res,
dynamicgraph::Matrix MiB( nv,nv );
MiB = massInverse*friction;
sotDEBUG(25) << "MiB = " << MiB;
dynamicgraph::Matrix MiBexp( nv,nv );
MiB*=-dt; expMatrix(MiB,MiBexp);
sotDEBUG(25) << "expMiB = " << MiBexp;
......@@ -214,38 +214,19 @@ computeVelocityExact( dynamicgraph::Vector& res,
dynamicgraph::Vector Bif( nf ); Bif = Binv*force;
sotDEBUG(25) << "Binv = " << Binv;
sotDEBUG(25) << "Bif = " << Bif;
dynamicgraph::Vector v0_bif = vel;
v0_bif -= Bif;
sotDEBUG(25) << "Kst = " << v0_bif;
res.resize( MiBexp.rows() );
res = MiBexp*v0_bif;
res += Bif;
velocityPrecSIN = res ;
sotDEBUG(25) << "vfin = " << 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); }
// }
src/integrator-force-rk4.cpp
+ 9
− 28
View file @ 56304929
......@@ -28,15 +28,15 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForceRK4,"IntegratorForceRK4");
IntegratorForceRK4::
IntegratorForceRK4( const std::string & name )
IntegratorForceRK4( const std::string & name )
:IntegratorForce(name)
{
sotDEBUGIN(5);
velocityDerivativeSOUT.
setFunction( boost::bind(&IntegratorForceRK4::computeDerivativeRK4,
this,_1,_2));
sotDEBUGOUT(5);
}
......@@ -73,10 +73,10 @@ computeDerivativeRK4( dynamicgraph::Vector& res,
res.resize(nv); res.fill(0);
if(! velocityPrecSIN )
{
{
dynamicgraph::Vector zero( nv ); zero.fill(0);
velocityPrecSIN = zero;
}
}
const dynamicgraph::Vector & vel = velocityPrecSIN( time );
double & dt = this->IntegratorForce::timeStep; // this is &
......@@ -99,39 +99,20 @@ computeDerivativeRK4( dynamicgraph::Vector& res,
sotDEBUG(35) << "f"<<i<<" = " << fi;
ki = massInverse*fi;
sotDEBUG(35) << "k"<<i<<" = " << ki;
if( i+1<4 )
if( i+1<4 )
{
v[i+1] = ki; v[i+1] *= (dt/rk_fact[i+1]);
v[i+1] += vel;
v[i+1] += vel;
}
ki *= rk_fact[i];
res += ki;
sotDEBUG(35) << "sum_k"<<i<<" = " << res;
sumFact += rk_fact[i];
}
sotDEBUG(35) << "sum_ki = " << res;
res *= (1/sumFact);
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); }
// }
src/integrator-force.cpp
+ 9
− 39
View file @ 56304929
......@@ -29,14 +29,14 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(IntegratorForce,"IntegratorForce");
const double IntegratorForce:: TIME_STEP_DEFAULT = 5e-3;
IntegratorForce::
IntegratorForce( const std::string & name )
IntegratorForce( const std::string & name )
:Entity(name)
,timeStep( TIME_STEP_DEFAULT )
,forceSIN(NULL,"sotIntegratorForce("+name+")::input(vector)::force")
,massInverseSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::massInverse")
,frictionSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::friction")
,velocityPrecSIN(NULL,"sotIntegratorForce("+name+")::input(matrix)::vprec")
,velocityDerivativeSOUT
( boost::bind(&IntegratorForce::computeDerivative,this,_1,_2),
velocityPrecSIN<<forceSIN<<massInverseSIN<<frictionSIN,
......@@ -51,7 +51,7 @@ IntegratorForce( const std::string & name )
"sotIntegratorForce("+name+")::input(matrix)::massInverseOUT")
{
sotDEBUGIN(5);
signalRegistration(forceSIN);
signalRegistration(massInverseSIN);
signalRegistration(frictionSIN);
......@@ -98,15 +98,15 @@ computeDerivative( dynamicgraph::Vector& res,
dynamicgraph::Vector f_bv( force.size() );
if( velocityPrecSIN )
{
{
const dynamicgraph::Vector & vel = velocityPrecSIN( time );
sotDEBUG(15) << "vel = " << vel << std::endl;
f_bv = friction*vel; f_bv *= -1;
f_bv = friction*vel; f_bv *= -1;
} else { f_bv.fill(0) ; } // vel is not set yet.
f_bv+=force;
res = massInverse*f_bv;
sotDEBUGOUT(15);
return res;
}
......@@ -123,8 +123,8 @@ computeIntegral( dynamicgraph::Vector& res,
if( velocityPrecSIN )
{
const dynamicgraph::Vector & vel = velocityPrecSIN( time );
res += vel;
}
res += vel;
}
else { /* vprec not set yet. */ }
velocityPrecSIN = res ;
......@@ -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); }
}
src/sot-dynamic-pinocchio.cpp
+ 29
− 140
View file @ 56304929
......@@ -59,7 +59,7 @@ DynamicPinocchio( const std::string & name)
"sotDynamicPinocchio("+name+")::intern(dummy)::pinocchioPos" )
,pinocchioVelSINTERN( boost::bind(&DynamicPinocchio::getPinocchioVel,this,_1, _2),
jointVelocitySIN<<freeFlyerVelocitySIN,
"sotDynamicPinocchio("+name+")::intern(dummy)::pinocchioVel" )
"sotDynamicPinocchio("+name+")::intern(dummy)::pinocchioVel" )
,pinocchioAccSINTERN( boost::bind(&DynamicPinocchio::getPinocchioAcc,this,_1, _2),
jointAccelerationSIN<<freeFlyerAccelerationSIN,
"sotDynamicPinocchio("+name+")::intern(dummy)::pinocchioAcc" )
......@@ -94,19 +94,19 @@ DynamicPinocchio( const std::string & name)
,upperJlSOUT( boost::bind(&DynamicPinocchio::getUpperPositionLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamicPinocchio("+name+")::output(vector)::upperJl" )
,lowerJlSOUT( boost::bind(&DynamicPinocchio::getLowerPositionLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamicPinocchio("+name+")::output(vector)::lowerJl" )
,upperVlSOUT( boost::bind(&DynamicPinocchio::getUpperVelocityLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamicPinocchio("+name+")::output(vector)::upperVl" )
,upperTlSOUT( boost::bind(&DynamicPinocchio::getMaxEffortLimits,this,_1,_2),
sotNOSIGNAL,
"sotDynamicPinocchio("+name+")::output(vector)::upperTl" )
,inertiaRotorSOUT( "sotDynamicPinocchio("+name+")::output(matrix)::inertiaRotor" )
,gearRatioSOUT( "sotDynamicPinocchio("+name+")::output(matrix)::gearRatio" )
,inertiaRealSOUT( boost::bind(&DynamicPinocchio::computeInertiaReal,this,_1,_2),
......@@ -126,7 +126,7 @@ DynamicPinocchio( const std::string & name)
sotDEBUGIN(5);
//TODO-------------------------------------------
//if( build ) buildModel();
//firstSINTERN.setDependencyType(TimeDependency<int>::BOOL_DEPENDENT);
//DEBUG: Why =0? should be function. firstSINTERN.setConstant(0);
......@@ -193,29 +193,29 @@ DynamicPinocchio( const std::string & name)
addCommand("createOpPoint",
makeCommandVoid2(*this,&DynamicPinocchio::cmd_createOpPointSignals,
docstring));
docstring = docCommandVoid2("Create a jacobian (world frame) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createJacobian",
makeCommandVoid2(*this,&DynamicPinocchio::cmd_createJacobianWorldSignal,
docstring));
docstring = docCommandVoid2("Create a jacobian (endeff frame) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createJacobianEndEff",
makeCommandVoid2(*this,&DynamicPinocchio::cmd_createJacobianEndEffectorSignal,
docstring));
docstring = docCommandVoid2("Create a position (matrix homo) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createPosition",
makeCommandVoid2(*this,&DynamicPinocchio::cmd_createPositionSignal,docstring));
docstring = docCommandVoid2("Create a velocity (vector) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createVelocity",
makeCommandVoid2(*this,&DynamicPinocchio::cmd_createVelocitySignal,docstring));
docstring = docCommandVoid2("Create an acceleration (vector) signal only for one joint.",
"string (signal name)","string (joint name)");
addCommand("createAcceleration",
......@@ -225,7 +225,7 @@ DynamicPinocchio( const std::string & name)
sphericalJoints.clear();
sotDEBUG(10)<< "Dynamic class_name address"<<&CLASS_NAME<<std::endl;
sotDEBUGOUT(5);
}
......@@ -265,7 +265,7 @@ DynamicPinocchio::setModel(se3::Model* modelPtr){
void
DynamicPinocchio::setData(se3::Data* dataPtr){
this->m_data = dataPtr;
}
/*--------------------------------GETTERS-------------------------------------------*/
......@@ -419,7 +419,7 @@ dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q,const int& time)
sotDEBUG(15) <<"Position out"<<q<<std::endl;
sotDEBUGOUT(15);
return q;
return q;
}
dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const int& time)
......@@ -432,7 +432,7 @@ dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const int& time)
v << vFF,vJoints;
return v;
}
else {
else {
v = vJoints;
return v;
}
......@@ -498,7 +498,7 @@ createEndeffJacobianSignal( const std::string& signame, const std::string& joint
"sotDynamicPinocchio("+name+")::output(matrix)::"+signame );
}
else if(m_model->existJointName(jointName)) {
long int jointId = m_model->getJointId(jointName);
long int jointId = m_model->getJointId(jointName);
sig = new dg::SignalTimeDependent< dg::Matrix,int >
( boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian,this,false,jointId,_1,_2),
jacobiansSINTERN,
......@@ -526,7 +526,7 @@ createPositionSignal( const std::string& signame, const std::string& jointName)
"sotDynamicPinocchio("+name+")::output(matrixHomo)::"+signame );
}
else if(m_model->existJointName(jointName)) {
long int jointId = m_model->getJointId(jointName);
long int jointId = m_model->getJointId(jointName);
sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
( boost::bind(&DynamicPinocchio::computeGenericPosition,this,false,jointId,_1,_2),
forwardKinematicsSINTERN,
......@@ -534,7 +534,7 @@ createPositionSignal( const std::string& signame, const std::string& jointName)
}
else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
"Robot has no joint corresponding to " + jointName);
genericSignalRefs.push_back( sig );
signalRegistration( *sig );
sotDEBUGOUT(15);
......@@ -802,7 +802,7 @@ computeGenericEndeffJacobian(const bool isFrame, const int jointId,dg::Matrix& r
}
MatrixHomogeneous& DynamicPinocchio::
computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous& res, const int& time)
computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous& res, const int& time)
{
sotDEBUGIN(25);
assert(m_data);
......@@ -823,7 +823,7 @@ computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous&
}
dg::Vector& DynamicPinocchio::
computeGenericVelocity( const int jointId, dg::Vector& res,const int& time )
computeGenericVelocity( const int jointId, dg::Vector& res,const int& time )
{
sotDEBUGIN(25);
assert(m_data);
......@@ -858,7 +858,7 @@ computeNewtonEuler(int& dummy,const int& time )
const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
se3::rnea(*m_model,*m_data,q,v,a);
sotDEBUG(1)<< "pos = " <<q <<std::endl;
sotDEBUG(1)<< "vel = " <<v <<std::endl;
sotDEBUG(1)<< "acc = " <<a <<std::endl;
......@@ -868,9 +868,9 @@ computeNewtonEuler(int& dummy,const int& time )
}
dg::Matrix& DynamicPinocchio::
computeJcom( dg::Matrix& Jcom,const int& time )
computeJcom( dg::Matrix& Jcom,const int& time )
{
sotDEBUGIN(25);
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
......@@ -881,7 +881,7 @@ computeJcom( dg::Matrix& Jcom,const int& time )
}
dg::Vector& DynamicPinocchio::
computeCom( dg::Vector& com,const int& time )
computeCom( dg::Vector& com,const int& time )
{
sotDEBUGIN(25);
......@@ -893,11 +893,11 @@ computeCom( dg::Vector& com,const int& time )
}
dg::Matrix& DynamicPinocchio::
computeInertia( dg::Matrix& res,const int& time )
computeInertia( dg::Matrix& res,const int& time )
{
sotDEBUGIN(25);
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
res = se3::crba(*m_model, *m_data, q);
res = se3::crba(*m_model, *m_data, q);
res.triangularView<Eigen::StrictlyLower>() =
res.transpose().triangularView<Eigen::StrictlyLower>();
sotDEBUGOUT(25);
......@@ -905,7 +905,7 @@ computeInertia( dg::Matrix& res,const int& time )
}
dg::Matrix& DynamicPinocchio::
computeInertiaReal( dg::Matrix& res,const int& time )
computeInertiaReal( dg::Matrix& res,const int& time )
{
sotDEBUGIN(25);
......@@ -960,7 +960,7 @@ computeMomenta(dg::Vector & Momenta, const int& time)
Momenta = m_data->hg.toVector_impl();
sotDEBUGOUT(25) << "Momenta :" << Momenta ;
return Momenta;
return Momenta;
}
dg::Vector& DynamicPinocchio::
......@@ -1048,119 +1048,8 @@ 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
//jointName is either a fixed-joint (pinocchio operational frame) or a
//movable joint (pinocchio joint-variant).
void DynamicPinocchio::cmd_createOpPointSignals( const std::string& opPointName,
const std::string& jointName )
......
src/waist-attitude-from-sensor.cpp
+ 12
− 64
View file @ 56304929
......@@ -31,13 +31,13 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistAttitudeFromSensor,"WaistAttitudeFromSensor");
WaistAttitudeFromSensor::
WaistAttitudeFromSensor( const std::string & name )
WaistAttitudeFromSensor( const std::string & name )
:Entity(name)
,attitudeSensorSIN(NULL,"sotWaistAttitudeFromSensor("+name+")::input(MatrixRotation)::attitudeIN")
,positionSensorSIN(NULL,"sotWaistAttitudeFromSensor("+name+")::input(matrixHomo)::position")
,attitudeWaistSOUT( boost::bind(&WaistAttitudeFromSensor::computeAttitudeWaist,this,_1,_2),
attitudeSensorSIN<<positionSensorSIN,
"sotWaistAttitudeFromSensor("+name+")::output(RPY)::attitude" )
"sotWaistAttitudeFromSensor("+name+")::output(RPY)::attitude" )
{
sotDEBUGIN(5);
......@@ -66,7 +66,7 @@ computeAttitudeWaist( VectorRollPitchYaw & res,
const int& time )
{
sotDEBUGIN(15);
const MatrixHomogeneous & waistMchest = positionSensorSIN( time );
const MatrixRotation & worldRchest = attitudeSensorSIN( time );
......@@ -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 ===================================== */
......@@ -108,13 +88,13 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(WaistPoseFromSensorAndContact,
"WaistPoseFromSensorAndContact");
WaistPoseFromSensorAndContact::
WaistPoseFromSensorAndContact( const std::string & name )
WaistPoseFromSensorAndContact( const std::string & name )
:WaistAttitudeFromSensor(name)
,fromSensor_(false)
,positionContactSIN(NULL,"sotWaistPoseFromSensorAndContact("+name+")::input(matrixHomo)::contact")
,positionWaistSOUT( boost::bind(&WaistPoseFromSensorAndContact::computePositionWaist,this,_1,_2),
attitudeWaistSOUT<<positionContactSIN,
"sotWaistPoseFromSensorAndContact("+name+")::output(RPY+T)::positionWaist" )
"sotWaistPoseFromSensorAndContact("+name+")::output(RPY+T)::positionWaist" )
{
sotDEBUGIN(5);
......@@ -166,64 +146,32 @@ computePositionWaist( dynamicgraph::Vector& res,
const int& time )
{
sotDEBUGIN(15);
const MatrixHomogeneous& waistMcontact = positionContactSIN( time );
MatrixHomogeneous contactMwaist; contactMwaist = waistMcontact.inverse();
res.resize(6);
for( unsigned int i=0;i<3;++i )
for( unsigned int i=0;i<3;++i )
{ res(i)=contactMwaist(i,3); }
if(fromSensor_)
{
const VectorRollPitchYaw & worldrwaist = attitudeWaistSOUT( time );
for( unsigned int i=0;i<3;++i )
for( unsigned int i=0;i<3;++i )
{ res(i+3)=worldrwaist(i); }
}
else
{
VectorRollPitchYaw contactrwaist;
VectorRollPitchYaw contactrwaist;
contactrwaist = contactMwaist.linear().eulerAngles(2,1,0).reverse();
for( unsigned int i=0;i<3;++i )
for( unsigned int i=0;i<3;++i )
{ res(i+3)=contactrwaist(i); }
}
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); }
}
src/zmpreffromcom.cpp
+ 8
− 44
View file @ 56304929
......@@ -26,20 +26,20 @@ using namespace dynamicgraph;
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ZmprefFromCom,"ZmprefFromCom");
const double ZmprefFromCom::DT_DEFAULT = 5e-3;
const double ZmprefFromCom::FOOT_HEIGHT_DEFAULT = .105;
const double ZmprefFromCom::DT_DEFAULT = 5e-3;
const double ZmprefFromCom::FOOT_HEIGHT_DEFAULT = .105;
ZmprefFromCom::
ZmprefFromCom( const std::string & name )
ZmprefFromCom( const std::string & name )
:Entity(name)
,dt(DT_DEFAULT)
,dt(DT_DEFAULT)
,footHeight(FOOT_HEIGHT_DEFAULT)
,waistPositionSIN(NULL,"sotZmprefFromCom("+name+")::input(MatrixHomo)::waist")
,comPositionSIN(NULL,"sotZmprefFromCom("+name+")::input(Vector)::com")
,dcomSIN(NULL,"sotZmprefFromCom("+name+")::input(Vector)::dcom")
,zmprefSOUT( boost::bind(&ZmprefFromCom::computeZmpref,this,_1,_2),
waistPositionSIN<<comPositionSIN<<dcomSIN,
"sotZmprefFromCom("+name+")::output(RPY)::zmpref" )
"sotZmprefFromCom("+name+")::output(RPY)::zmpref" )
{
sotDEBUGIN(5);
......@@ -69,54 +69,18 @@ computeZmpref( dynamicgraph::Vector& res,
const int& time )
{
sotDEBUGIN(15);
const dynamicgraph::Vector& com = comPositionSIN( time );
const dynamicgraph::Vector& dcom = dcomSIN( time );
const MatrixHomogeneous& oTw = waistPositionSIN( time );
MatrixHomogeneous wTo = oTw.inverse();
dynamicgraph::Vector nextComRef = dcom; nextComRef*= dt;nextComRef+=com;
nextComRef(2) = -footHeight; // projection on the ground.
res = wTo.matrix()*nextComRef;
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); }
}
unitTesting/test_dyn.cpp
+ 4
− 6
View file @ 56304929
......@@ -41,15 +41,15 @@ int main(int argc, char * argv[])
cerr << " PATH_TO_LINK2JOINT_FILE: Path to the file describing the relationship of the joint and the state vector." << endl;
}
Dynamic * dyn = new Dynamic("tot");
try
try
{
dyn->setVrmlDirectory(argv[1]);
dyn->setXmlSpecificityFile(argv[3]);
dyn->setXmlRankFile(argv[4]);
dyn->setVrmlMainFile(argv[2]);
dyn->parseConfigFiles();
}
}
catch (ExceptionDynamic& e)
{
if ( !strcmp(e.what(), "Error while parsing." )) {
......@@ -60,11 +60,9 @@ int main(int argc, char * argv[])
// rethrow
throw e;
}
istringstream iss;
string help("help");
// Commented out to make this sample a unit test
// dyn->commandLine(help,iss,cout);
delete dyn;
return 0;
......
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