Commit 3c6363cd authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Format

parent 1fd2005f
......@@ -28,7 +28,8 @@ install these packages at https://github.com/jrl-umi3218.
\section python_bindings Python bindings
As most packages based on the dynamic-graph framework (see https://github.com/jrl-umi3218/dynamic-graph),
the functionnality is exposed through entities. Hence python sub-modules of dynamic_graph are generated. See <a href="../sphinx-html/index.html">sphinx documentation</a> for more details.
the functionnality is exposed through entities. Hence python sub-modules of dynamic_graph are generated. See <a
href="../sphinx-html/index.html">sphinx documentation</a> for more details.
The following entities are created by this package:\n
(all entites are placed in the namespace sot::)
......
......@@ -13,14 +13,14 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (angle_estimator_EXPORTS)
# define SOTANGLEESTIMATOR_EXPORT __declspec(dllexport)
# else
# define SOTANGLEESTIMATOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(angle_estimator_EXPORTS)
#define SOTANGLEESTIMATOR_EXPORT __declspec(dllexport)
#else
# define SOTANGLEESTIMATOR_EXPORT
#define SOTANGLEESTIMATOR_EXPORT __declspec(dllimport)
#endif
#else
#define SOTANGLEESTIMATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
......@@ -30,87 +30,70 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
#include <sot/core/matrix-geometry.hh>
/* STD */
#include <string>
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTANGLEESTIMATOR_EXPORT AngleEstimator
:public dg::Entity
{
class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
virtual const std::string& getClassName(void) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
AngleEstimator( const std::string& name );
virtual ~AngleEstimator( void );
public: /* --- SIGNAL --- */
dg::SignalPtr<MatrixRotation,int> sensorWorldRotationSIN; // estimate(worldRc)
dg::SignalPtr<MatrixHomogeneous,int> sensorEmbeddedPositionSIN; // waistRchest
dg::SignalPtr<MatrixHomogeneous,int> contactWorldPositionSIN; // estimate(worldRf)
dg::SignalPtr<MatrixHomogeneous,int> contactEmbeddedPositionSIN; // waistRleg
dg::SignalTimeDependent<dynamicgraph::Vector,int> anglesSOUT; // [ flex1 flex2 yaw_drift ]
dg::SignalTimeDependent<MatrixRotation,int> flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation,int> driftSOUT; // Ryaw = worldRc est(wRc)^-1
dg::SignalTimeDependent<MatrixRotation,int> sensorWorldRotationSOUT; // worldRc
dg::SignalTimeDependent<MatrixRotation,int> waistWorldRotationSOUT; // worldRwaist
dg::SignalTimeDependent<MatrixHomogeneous,int> waistWorldPositionSOUT; // worldMwaist
dg::SignalTimeDependent<dynamicgraph::Vector,int> waistWorldPoseRPYSOUT; // worldMwaist
dg::SignalPtr<dynamicgraph::Matrix,int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Vector,int> qdotSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> xff_dotSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector,int> qdotSOUT;
AngleEstimator(const std::string& name);
virtual ~AngleEstimator(void);
public: /* --- SIGNAL --- */
dg::SignalPtr<MatrixRotation, int> sensorWorldRotationSIN; // estimate(worldRc)
dg::SignalPtr<MatrixHomogeneous, int> sensorEmbeddedPositionSIN; // waistRchest
dg::SignalPtr<MatrixHomogeneous, int> contactWorldPositionSIN; // estimate(worldRf)
dg::SignalPtr<MatrixHomogeneous, int> contactEmbeddedPositionSIN; // waistRleg
dg::SignalTimeDependent<dynamicgraph::Vector, int> anglesSOUT; // [ flex1 flex2 yaw_drift ]
dg::SignalTimeDependent<MatrixRotation, int> flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, int> driftSOUT; // Ryaw = worldRc est(wRc)^-1
dg::SignalTimeDependent<MatrixRotation, int> sensorWorldRotationSOUT; // worldRc
dg::SignalTimeDependent<MatrixRotation, int> waistWorldRotationSOUT; // worldRwaist
dg::SignalTimeDependent<MatrixHomogeneous, int> waistWorldPositionSOUT; // worldMwaist
dg::SignalTimeDependent<dynamicgraph::Vector, int> waistWorldPoseRPYSOUT; // worldMwaist
dg::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Vector, int> qdotSIN;
dg::SignalTimeDependent<dynamicgraph::Vector, int> xff_dotSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, int> qdotSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeAngles( dynamicgraph::Vector& res,
const int& time );
MatrixRotation& computeFlexibilityFromAngles( MatrixRotation& res,
const int& time );
MatrixRotation& computeDriftFromAngles( MatrixRotation& res,
const int& time );
MatrixRotation& computeSensorWorldRotation( MatrixRotation& res,
const int& time );
MatrixRotation& computeWaistWorldRotation( MatrixRotation& res,
const int& time );
MatrixHomogeneous& computeWaistWorldPosition( MatrixHomogeneous& res,
const int& time );
dynamicgraph::Vector& computeWaistWorldPoseRPY( dynamicgraph::Vector& res,
const int& time );
dynamicgraph::Vector& compute_xff_dotSOUT( dynamicgraph::Vector& res,
const int& time );
dynamicgraph::Vector& compute_qdotSOUT( dynamicgraph::Vector& res,
const int& time );
dynamicgraph::Vector& computeAngles(dynamicgraph::Vector& res, const int& time);
MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res, const int& time);
MatrixRotation& computeDriftFromAngles(MatrixRotation& res, const int& time);
MatrixRotation& computeSensorWorldRotation(MatrixRotation& res, const int& time);
MatrixRotation& computeWaistWorldRotation(MatrixRotation& res, const int& time);
MatrixHomogeneous& computeWaistWorldPosition(MatrixHomogeneous& res, const int& time);
dynamicgraph::Vector& computeWaistWorldPoseRPY(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& compute_xff_dotSOUT(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& compute_qdotSOUT(dynamicgraph::Vector& res, const int& time);
public: /* --- PARAMS --- */
void fromSensor(const bool& fs) { fromSensor_ = fs; }
bool fromSensor() const { return fromSensor_; }
bool fromSensor() const { return fromSensor_; }
private:
bool fromSensor_;
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
#endif // #ifndef __SOT_ANGLE_ESTIMATOR_H__
......@@ -14,7 +14,6 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* STD */
#include <string>
#include <map>
......@@ -42,137 +41,126 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (dynamic_EXPORTS)
# define SOTDYNAMIC_EXPORT __declspec(dllexport)
# else
# define SOTDYNAMIC_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(dynamic_EXPORTS)
#define SOTDYNAMIC_EXPORT __declspec(dllexport)
#else
# define SOTDYNAMIC_EXPORT
#define SOTDYNAMIC_EXPORT __declspec(dllimport)
#endif
#else
#define SOTDYNAMIC_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace command {
class SetFile;
class CreateOpPoint;
}
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup signals
\brief This class provides an inverse dynamic model of the robot.
More precisely it wraps the newton euler algorithm implemented
by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
The robot is described by a VRML file.
*/
class SOTDYNAMIC_EXPORT DynamicPinocchio
:public dg::Entity {
namespace sot {
namespace dg = dynamicgraph;
namespace command {
class SetFile;
class CreateOpPoint;
} // namespace command
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/*! @ingroup signals
\brief This class provides an inverse dynamic model of the robot.
More precisely it wraps the newton euler algorithm implemented
by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
The robot is described by a VRML file.
*/
class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
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();
/* --- MODEL ATRIBUTES --- */
pinocchio::Model* m_model;
pinocchio::Data* m_data;
pinocchio::Model* m_model;
pinocchio::Data* m_data;
/* --- MODEL ATRIBUTES --- */
public:
public:
/* --- SIGNAL ACTIVATION --- */
dg::SignalTimeDependent< dg::Matrix,int >&
createEndeffJacobianSignal( const std::string& signame, const std::string&, const bool isLocal = true);
dg::SignalTimeDependent< dg::Matrix,int >&
createJacobianSignal( const std::string& signame, const std::string& );
void destroyJacobianSignal( const std::string& signame );
dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(const std::string& signame, const std::string&,
const bool isLocal = true);
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<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>& 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 );
dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(const std::string&, const std::string&);
void destroyAccelerationSignal(const std::string& signame);
/*! @} */
std::list< dg::SignalBase<int>* > genericSignalRefs;
std::list<dg::SignalBase<int>*> genericSignalRefs;
public:
public:
/* --- SIGNAL --- */
typedef int Dummy;
dg::SignalPtr<dg::Vector,int> jointPositionSIN;
dg::SignalPtr<dg::Vector,int> freeFlyerPositionSIN;
dg::SignalPtr<dg::Vector,int> jointVelocitySIN;
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;
dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
dg::SignalTimeDependent<Dummy,int> jacobiansSINTERN;
dg::SignalTimeDependent<Dummy,int> forwardKinematicsSINTERN;
dg::SignalTimeDependent<Dummy,int> ccrbaSINTERN;
int& computeNewtonEuler(int& dummy,const int& time );
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;
dg::SignalTimeDependent<dg::Vector,int> upperVlSOUT;
dg::SignalTimeDependent<dg::Vector,int> upperTlSOUT;
dg::Signal<dg::Vector,int> inertiaRotorSOUT;
dg::Signal<dg::Vector,int> gearRatioSOUT;
dg::SignalTimeDependent<dg::Matrix,int> inertiaRealSOUT;
dg::SignalTimeDependent<dg::Vector,int> MomentaSOUT;
dg::SignalTimeDependent<dg::Vector,int> AngularMomentumSOUT;
dg::SignalTimeDependent<dg::Vector,int> dynamicDriftSOUT;
public:
dg::SignalPtr<dg::Vector, int> jointPositionSIN;
dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
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;
dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;
int& computeNewtonEuler(int& dummy, const int& time);
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;
dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;
dg::Signal<dg::Vector, int> inertiaRotorSOUT;
dg::Signal<dg::Vector, int> gearRatioSOUT;
dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;
public:
/* --- CONSTRUCTOR --- */
DynamicPinocchio( const std::string& name);
virtual ~DynamicPinocchio( void );
DynamicPinocchio(const std::string& name);
virtual ~DynamicPinocchio(void);
/* --- MODEL CREATION --- */
void displayModel() const
{ assert(m_model); std::cout<<(*m_model)<<std::endl; };
void displayModel() const {
assert(m_model);
std::cout << (*m_model) << std::endl;
};
void setModel(pinocchio::Model*);
......@@ -184,77 +172,70 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getLowerPositionLimits(dg::Vector& res,const int&) const ;
dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
/// \brief Get joint position upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperPositionLimits(dg::Vector& res,const int&) const;
dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
/// \brief Get joint velocity upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperVelocityLimits(dg::Vector& res,const int&) const;
dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
/// \brief Get joint effort upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getMaxEffortLimits(dg::Vector& res,const int&) const;
dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
// dg::Vector& getAnklePositionInFootFrame() const;
protected:
dg::Matrix& computeGenericJacobian(const bool isFrame,
const int jointId,
dg::Matrix& res,const int& time );
dg::Matrix& computeGenericEndeffJacobian(const bool isFrame, const bool isLocal,
const int jointId,
dg::Matrix& res,const int& time );
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 );
dg::Vector& computeGenericAcceleration(const int jointId,dg::Vector& res,const int& time );
dg::Vector& computeZmp( dg::Vector& res,const int& time );
dg::Vector& computeMomenta( dg::Vector &res,const int& time);
dg::Vector& computeAngularMomentum( dg::Vector &res,const int& time);
dg::Matrix& computeJcom( dg::Matrix& res,const int& time );
dg::Vector& computeCom( dg::Vector& res,const int& time );
dg::Matrix& computeInertia( dg::Matrix& res,const int& time );
dg::Matrix& computeInertiaReal( dg::Matrix& res,const int& time );
double& computeFootHeight( double& res,const int& time );
dg::Vector& computeTorqueDrift( dg::Vector& res,const int& time );
protected:
dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res, const int& time);
dg::Matrix& computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId, dg::Matrix& res,
const int& time);
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);
dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time);
dg::Vector& computeZmp(dg::Vector& res, const int& time);
dg::Vector& computeMomenta(dg::Vector& res, const int& time);
dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
dg::Vector& computeCom(dg::Vector& res, const int& time);
dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
double& computeFootHeight(double& res, const int& time);
dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
public: /* --- PARAMS --- */
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 );
void cmd_createJacobianEndEffectorWorldSignal( const std::string& sig, const std::string& j );
void cmd_createPositionSignal ( const std::string& sig, const std::string& j );
void cmd_createVelocitySignal ( const std::string& sig, const std::string& j );
void cmd_createAccelerationSignal ( const std::string& sig, const std::string& j );
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);
void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig, const std::string& j);
void cmd_createPositionSignal(const std::string& sig, const std::string& j);
void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
void cmd_createAccelerationSignal(const std::string& sig, const std::string& j);
private:
/// \brief map of joints in construction.
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
dg::Vector& getPinocchioPos(dg::Vector& q,const int& time);
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);
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;
};
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} /* namespace sot */} /* namespace dynamicgraph */
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} /* namespace sot */
} /* namespace dynamicgraph */
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
......@@ -17,7 +17,6 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
......@@ -32,151 +31,124 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (force_compensation_EXPORTS)
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
# else
# define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(force_compensation_EXPORTS)
#define SOTFORCECOMPENSATION_EXPORT __declspec(dllexport)
#else
# define SOTFORCECOMPENSATION_EXPORT
#define SOTFORCECOMPENSATION_EXPORT __declspec(dllimport)
#endif
#else
#define SOTFORCECOMPENSATION_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace dynamicgraph { namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTFORCECOMPENSATION_EXPORT ForceCompensation
{
private:
static MatrixRotation I3;
protected:
bool usingPrecompensation;
public:
ForceCompensation( void );
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,
const dynamicgraph::Vector & transSensorCom,
MatrixForce& res );
/* static dynamicgraph::Matrix& computeInertiaSensor( const dynamicgraph::Matrix& inertiaJoint, */
/* const MatrixForce& sensorXhand, */
/* dynamicgraph::Matrix& res ); */
static dynamicgraph::Vector& computeTorsorCompensated( const dynamicgraph::Vector& torqueInput,