From d5062fa67a6c2cbd359b55bfa88c9f19890045af Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Mon, 13 Aug 2018 16:44:32 +0200
Subject: [PATCH] Remove shell
ref https://github.com/stack-of-tasks/sot-core/issues/58
---
.../sot-dynamic-pinocchio/angle-estimator.h | 3 -
.../sot-dynamic-pinocchio/dynamic-pinocchio.h | 43 +++--
.../force-compensation.h | 65 +++----
.../integrator-force-exact.h | 12 +-
.../integrator-force-rk4.h | 12 +-
.../sot-dynamic-pinocchio/integrator-force.h | 29 ++-
.../waist-attitude-from-sensor.h | 20 +--
include/sot-dynamic-pinocchio/zmpreffromcom.h | 15 +-
src/angle-estimator.cpp | 30 +---
src/force-compensation.cpp | 122 +------------
src/integrator-force-exact.cpp | 63 +++----
src/integrator-force-rk4.cpp | 37 +---
src/integrator-force.cpp | 48 +----
src/sot-dynamic-pinocchio.cpp | 169 +++---------------
src/waist-attitude-from-sensor.cpp | 76 ++------
src/zmpreffromcom.cpp | 52 +-----
unitTesting/test_dyn.cpp | 10 +-
17 files changed, 172 insertions(+), 634 deletions(-)
diff --git a/include/sot-dynamic-pinocchio/angle-estimator.h b/include/sot-dynamic-pinocchio/angle-estimator.h
index ba1c27a..33185c8 100644
--- a/include/sot-dynamic-pinocchio/angle-estimator.h
+++ b/include/sot-dynamic-pinocchio/angle-estimator.h
@@ -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_;
diff --git a/include/sot-dynamic-pinocchio/dynamic-pinocchio.h b/include/sot-dynamic-pinocchio/dynamic-pinocchio.h
index 48ef5dc..8dba637 100644
--- a/include/sot-dynamic-pinocchio/dynamic-pinocchio.h
+++ b/include/sot-dynamic-pinocchio/dynamic-pinocchio.h
@@ -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;
diff --git a/include/sot-dynamic-pinocchio/force-compensation.h b/include/sot-dynamic-pinocchio/force-compensation.h
index 008ab3b..6cb9739 100644
--- a/include/sot-dynamic-pinocchio/force-compensation.h
+++ b/include/sot-dynamic-pinocchio/force-compensation.h
@@ -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 );
-
-
-
};
diff --git a/include/sot-dynamic-pinocchio/integrator-force-exact.h b/include/sot-dynamic-pinocchio/integrator-force-exact.h
index c1151f9..2a8f8cc 100644
--- a/include/sot-dynamic-pinocchio/integrator-force-exact.h
+++ b/include/sot-dynamic-pinocchio/integrator-force-exact.h
@@ -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 ); */
-
};
diff --git a/include/sot-dynamic-pinocchio/integrator-force-rk4.h b/include/sot-dynamic-pinocchio/integrator-force-rk4.h
index 6a6ad20..1621976 100644
--- a/include/sot-dynamic-pinocchio/integrator-force-rk4.h
+++ b/include/sot-dynamic-pinocchio/integrator-force-rk4.h
@@ -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 ); */
-
};
diff --git a/include/sot-dynamic-pinocchio/integrator-force.h b/include/sot-dynamic-pinocchio/integrator-force.h
index 9de5303..4b7834a 100644
--- a/include/sot-dynamic-pinocchio/integrator-force.h
+++ b/include/sot-dynamic-pinocchio/integrator-force.h
@@ -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 );
-
-
};
diff --git a/include/sot-dynamic-pinocchio/waist-attitude-from-sensor.h b/include/sot-dynamic-pinocchio/waist-attitude-from-sensor.h
index f442e21..57cb9cf 100644
--- a/include/sot-dynamic-pinocchio/waist-attitude-from-sensor.h
+++ b/include/sot-dynamic-pinocchio/waist-attitude-from-sensor.h
@@ -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 );
-
-
};
diff --git a/include/sot-dynamic-pinocchio/zmpreffromcom.h b/include/sot-dynamic-pinocchio/zmpreffromcom.h
index 7a9edb9..d70a2e3 100644
--- a/include/sot-dynamic-pinocchio/zmpreffromcom.h
+++ b/include/sot-dynamic-pinocchio/zmpreffromcom.h
@@ -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 );
-
-
};
diff --git a/src/angle-estimator.cpp b/src/angle-estimator.cpp
index 8c505d8..e0a0e91 100644
--- a/src/angle-estimator.cpp
+++ b/src/angle-estimator.cpp
@@ -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); }
-}
-
diff --git a/src/force-compensation.cpp b/src/force-compensation.cpp
index 3eb469a..e952e3e 100644
--- a/src/force-compensation.cpp
+++ b/src/force-compensation.cpp
@@ -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 ); }
-
-
-}
-
diff --git a/src/integrator-force-exact.cpp b/src/integrator-force-exact.cpp
index 3b8c017..3c0ce3f 100644
--- a/src/integrator-force-exact.cpp
+++ b/src/integrator-force-exact.cpp
@@ -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); }
-// }
-
diff --git a/src/integrator-force-rk4.cpp b/src/integrator-force-rk4.cpp
index 8e654fd..6104215 100644
--- a/src/integrator-force-rk4.cpp
+++ b/src/integrator-force-rk4.cpp
@@ -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); }
-// }
-
diff --git a/src/integrator-force.cpp b/src/integrator-force.cpp
index 1b3cd74..ea4d9d7 100644
--- a/src/integrator-force.cpp
+++ b/src/integrator-force.cpp
@@ -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); }
-}
-
diff --git a/src/sot-dynamic-pinocchio.cpp b/src/sot-dynamic-pinocchio.cpp
index b7e47cb..6f2dfd4 100644
--- a/src/sot-dynamic-pinocchio.cpp
+++ b/src/sot-dynamic-pinocchio.cpp
@@ -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 )
diff --git a/src/waist-attitude-from-sensor.cpp b/src/waist-attitude-from-sensor.cpp
index 729de0c..b2cb0e6 100644
--- a/src/waist-attitude-from-sensor.cpp
+++ b/src/waist-attitude-from-sensor.cpp
@@ -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); }
-}
-
diff --git a/src/zmpreffromcom.cpp b/src/zmpreffromcom.cpp
index 040ed6e..bb81292 100644
--- a/src/zmpreffromcom.cpp
+++ b/src/zmpreffromcom.cpp
@@ -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); }
-}
-
diff --git a/unitTesting/test_dyn.cpp b/unitTesting/test_dyn.cpp
index 8b4cc4d..bc63792 100644
--- a/unitTesting/test_dyn.cpp
+++ b/unitTesting/test_dyn.cpp
@@ -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;
--
GitLab