...
 
Commits (7)
# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
SET(PROJECT_NAMESPACE stack-of-tasks)
SET(PROJECT_NAME sot-dynamic-pinocchio)
......@@ -14,6 +8,12 @@ SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.")
SET(PROJECT_URL "https://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(PROJECT_SUFFIX "-v3")
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(DOXYGEN_USE_MATHJAX YES)
......@@ -26,7 +26,9 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
plugindir
)
SETUP_PROJECT()
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
......@@ -40,9 +42,9 @@ OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.4.0")
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_PATH})
ADD_REQUIRED_DEPENDENCY("eigenpy")
ENDIF(BUILD_PYTHON_INTERFACE)
......@@ -60,5 +62,3 @@ ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(doc)
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unitTesting)
SETUP_PROJECT_FINALIZE()
Subproject commit f389aae203c4d92649cd5eb66289fd6a17c03fde
Subproject commit 63efaecd7c1fa3f9c190b5365561c38ea33f3236
......@@ -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__
......@@ -17,7 +17,6 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
......@@ -33,49 +32,39 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (integrator_force_exact_EXPORTS)
# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllexport)
# else
# define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(integrator_force_exact_EXPORTS)
#define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllexport)
#else
# define SOTINTEGRATORFORCEEXACT_EXPORT
#define SOTINTEGRATORFORCEEXACT_EXPORT __declspec(dllimport)
#endif
#else
#define SOTINTEGRATORFORCEEXACT_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
:public IntegratorForce
{
class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact : public IntegratorForce {
public:
static const std::string CLASS_NAME;
protected:
public: /* --- CONSTRUCTION --- */
IntegratorForceExact( const std::string& name );
virtual ~IntegratorForceExact( void );
IntegratorForceExact(const std::string& name);
virtual ~IntegratorForceExact(void);
public: /* --- SIGNAL --- */
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeVelocityExact( dynamicgraph::Vector& res,
const int& time );
dynamicgraph::Vector& computeVelocityExact(dynamicgraph::Vector& res, const int& time);
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_SOTINTEGRATORFORCEEXACT_H__
#endif // #ifndef __SOT_SOTINTEGRATORFORCEEXACT_H__
......@@ -17,7 +17,6 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
......@@ -33,50 +32,39 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (integrator_force_rk4_EXPORTS)
# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllexport)
# else
# define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(integrator_force_rk4_EXPORTS)
#define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllexport)
#else
# define SOTINTEGRATORFORCERK4_EXPORT
#define SOTINTEGRATORFORCERK4_EXPORT __declspec(dllimport)
#endif
#else
#define SOTINTEGRATORFORCERK4_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4
:public IntegratorForce
{
class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4 : public IntegratorForce {
public:
static const std::string CLASS_NAME;
protected:
public: /* --- CONSTRUCTION --- */
IntegratorForceRK4( const std::string& name );
virtual ~IntegratorForceRK4( void );
IntegratorForceRK4(const std::string& name);
virtual ~IntegratorForceRK4(void);
public: /* --- SIGNAL --- */
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeDerivativeRK4( dynamicgraph::Vector& res,
const int& time );
dynamicgraph::Vector& computeDerivativeRK4(dynamicgraph::Vector& res, const int& time);
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_SOTINTEGRATORFORCERK4_H__
#endif // #ifndef __SOT_SOTINTEGRATORFORCERK4_H__
......@@ -17,14 +17,12 @@
/* 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>
......@@ -32,70 +30,59 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (integrator_force_EXPORTS)
# define SOTINTEGRATORFORCE_EXPORT __declspec(dllexport)
# else
# define SOTINTEGRATORFORCE_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(integrator_force_EXPORTS)
#define SOTINTEGRATORFORCE_EXPORT __declspec(dllexport)
#else
# define SOTINTEGRATORFORCE_EXPORT
#define SOTINTEGRATORFORCE_EXPORT __declspec(dllimport)
#endif
#else
#define SOTINTEGRATORFORCE_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTINTEGRATORFORCE_EXPORT IntegratorForce
:public dg::Entity
{
class SOTINTEGRATORFORCE_EXPORT IntegratorForce : 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; }
protected:
double timeStep;
static const double TIME_STEP_DEFAULT ; // = 5e-3
static const double TIME_STEP_DEFAULT; // = 5e-3
public: /* --- CONSTRUCTION --- */
IntegratorForce( const std::string& name );
virtual ~IntegratorForce( void );
IntegratorForce(const std::string& name);
virtual ~IntegratorForce(void);
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,
const int& time );
dynamicgraph::Vector& computeIntegral( dynamicgraph::Vector& res,
const int& time );
dynamicgraph::Matrix& computeMassInverse( dynamicgraph::Matrix& res,
const int& time );
dynamicgraph::Vector& computeDerivative(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computeIntegral(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res, const int& time);
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
#endif // #ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
......@@ -17,14 +17,12 @@
/* 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>
......@@ -32,53 +30,48 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (mass_apparent_EXPORTS)
# define SOTMASSAPPARENT_EXPORT __declspec(dllexport)
# else
# define SOTMASSAPPARENT_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(mass_apparent_EXPORTS)
#define SOTMASSAPPARENT_EXPORT __declspec(dllexport)
#else
# define SOTMASSAPPARENT_EXPORT
#define SOTMASSAPPARENT_EXPORT __declspec(dllimport)
#endif
#else
#define SOTMASSAPPARENT_EXPORT
#endif
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
namespace dynamicgraph { namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTMASSAPPARENT_EXPORT MassApparent
:public dg::Entity
{
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
MassApparent( const std::string& name );
virtual ~MassApparent( void );
public: /* --- SIGNAL --- */
dg::SignalPtr<dynamicgraph::Matrix,int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaInverseSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> massInverseSOUT;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> massSOUT;
dg::SignalPtr<dynamicgraph::Matrix,int> inertiaSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaInverseSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Matrix& computeMassInverse( dynamicgraph::Matrix& res,const int& time );
dynamicgraph::Matrix& computeMass( dynamicgraph::Matrix& res,const int& time );
dynamicgraph::Matrix& computeInertiaInverse( dynamicgraph::Matrix& res,const int& time );
};
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_SOTMASSAPPARENT_H__
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTMASSAPPARENT_EXPORT MassApparent : public dg::Entity {
public:
static const std::string CLASS_NAME;
virtual const std::string& getClassName(void) const { return CLASS_NAME; }
public: /* --- CONSTRUCTION --- */
MassApparent(const std::string& name);
virtual ~MassApparent(void);
public: /* --- SIGNAL --- */
dg::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Matrix, int> inertiaInverseSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix, int> massInverseSOUT;
dg::SignalTimeDependent<dynamicgraph::Matrix, int> massSOUT;
dg::SignalPtr<dynamicgraph::Matrix, int> inertiaSIN;
dg::SignalTimeDependent<dynamicgraph::Matrix, int> inertiaInverseSOUT;
public: /* --- FUNCTIONS --- */
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res, const int& time);
dynamicgraph::Matrix& computeMass(dynamicgraph::Matrix& res, const int& time);
dynamicgraph::Matrix& computeInertiaInverse(dynamicgraph::Matrix& res, const int& time);
};
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __SOT_SOTMASSAPPARENT_H__
......@@ -14,71 +14,63 @@
#include "dynamic-graph/linear-algebra.h"
#include <sot/core/matrix-geometry.hh>
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (matrix_inertia_EXPORTS)
# define SOTMATRIXINERTIA_EXPORT __declspec(dllexport)
# else
# define SOTMATRIXINERTIA_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(matrix_inertia_EXPORTS)
#define SOTMATRIXINERTIA_EXPORT __declspec(dllexport)
#else
# define SOTMATRIXINERTIA_EXPORT
#define SOTMATRIXINERTIA_EXPORT __declspec(dllimport)
#endif
#else
#define SOTMATRIXINERTIA_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
class SOTMATRIXINERTIA_EXPORT MatrixInertia
{
public:
class SOTMATRIXINERTIA_EXPORT MatrixInertia {
public:
private:
MatrixInertia( void ) {}
MatrixInertia(void) {}
void initParents( void );
void initDofTable( void );
void initParents(void);
void initDofTable(void);
public:
MatrixInertia( CjrlHumanoidDynamicRobot* aHDR );
~MatrixInertia( void );
void init( CjrlHumanoidDynamicRobot* aHDR );
MatrixInertia(CjrlHumanoidDynamicRobot* aHDR);
~MatrixInertia(void);
void init(CjrlHumanoidDynamicRobot* aHDR);
public:
void update( void );
public:
void update(void);
void computeInertiaMatrix();
void getInertiaMatrix(double* A);
const maal::boost::Matrix& getInertiaMatrix( void );
const maal::boost::Matrix& getInertiaMatrix(void);
size_t getDoF() { return joints_.size(); }
private:
CjrlHumanoidDynamicRobot* aHDR_;
dynamicsJRLJapan::HumanoidDynamicMultiBody* aHDMB_;
std::vector<CjrlJoint*> joints_;
std::vector<int> parentIndex_;
std::vector< dynamicgraph::Matrix > Ic;
std::vector< dynamicgraph::Vector > phi;
std::vector< MatrixTwist > iVpi;
std::vector< MatrixForce > iVpiT;
private:
CjrlHumanoidDynamicRobot* aHDR_;
dynamicsJRLJapan::HumanoidDynamicMultiBody* aHDMB_;
std::vector<CjrlJoint*> joints_;
std::vector<int> parentIndex_;
std::vector<dynamicgraph::Matrix> Ic;
std::vector<dynamicgraph::Vector> phi;
std::vector<MatrixTwist> iVpi;
std::vector<MatrixForce> iVpiT;
dynamicgraph::Matrix inertia_;
};
} /* namespace sot */} /* namespace dynamicgraph */
} /* namespace sot */
} /* namespace dynamicgraph */
#endif // __SOT_SOTMATRIXINERTIA_H__
#endif // __SOT_SOTMATRIXINERTIA_H__
......@@ -17,7 +17,6 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
......@@ -31,81 +30,64 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (waist_attitude_from_sensor_EXPORTS)
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllexport)
# else
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(waist_attitude_from_sensor_EXPORTS)
#define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllexport)
#else
# define SOTWAISTATTITUDEFROMSENSOR_EXPORT
#define SOTWAISTATTITUDEFROMSENSOR_EXPORT __declspec(dllimport)
#endif
#else
#define SOTWAISTATTITUDEFROMSENSOR_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistAttitudeFromSensor
:public dg::Entity
{
class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistAttitudeFromSensor : 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 --- */
WaistAttitudeFromSensor( const std::string& name );
virtual ~WaistAttitudeFromSensor( void );
WaistAttitudeFromSensor(const std::string& name);
virtual ~WaistAttitudeFromSensor(void);
public: /* --- SIGNAL --- */
VectorRollPitchYaw& computeAttitudeWaist(VectorRollPitchYaw& res, const int& time);
VectorRollPitchYaw & computeAttitudeWaist( VectorRollPitchYaw & res,
const int& time );
dg::SignalPtr<MatrixRotation,int> attitudeSensorSIN;
dg::SignalPtr<MatrixHomogeneous,int> positionSensorSIN;
dg::SignalTimeDependent<VectorRollPitchYaw,int> attitudeWaistSOUT;
dg::SignalPtr<MatrixRotation, int> attitudeSensorSIN;
dg::SignalPtr<MatrixHomogeneous, int> positionSensorSIN;
dg::SignalTimeDependent<VectorRollPitchYaw, int> attitudeWaistSOUT;
};
class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistPoseFromSensorAndContact
:public WaistAttitudeFromSensor
{
class SOTWAISTATTITUDEFROMSENSOR_EXPORT WaistPoseFromSensorAndContact : public WaistAttitudeFromSensor {
public:
static const std::string CLASS_NAME;
protected:
void fromSensor(const bool& inFromSensor) {
fromSensor_ = inFromSensor;
}
bool fromSensor() const {
return fromSensor_;
}
void fromSensor(const bool& inFromSensor) { fromSensor_ = inFromSensor; }
bool fromSensor() const { return fromSensor_; }
private:
bool fromSensor_;
public: /* --- CONSTRUCTION --- */
WaistPoseFromSensorAndContact( const std::string& name );
virtual ~WaistPoseFromSensorAndContact( void );
WaistPoseFromSensorAndContact(const std::string& name);
virtual ~WaistPoseFromSensorAndContact(void);
public: /* --- SIGNAL --- */
dynamicgraph::Vector& computePositionWaist(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computePositionWaist( dynamicgraph::Vector& res,
const int& time );
dg::SignalPtr<MatrixHomogeneous,int> positionContactSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> positionWaistSOUT;
dg::SignalPtr<MatrixHomogeneous, int> positionContactSIN;
dg::SignalTimeDependent<dynamicgraph::Vector, int> positionWaistSOUT;
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
#endif // #ifndef __SOT_WAISTATTITUDEFROMSENSOR_H__
......@@ -17,7 +17,6 @@
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
......@@ -31,54 +30,49 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (zmpreffromcom_EXPORTS)
# define SOTZMPREFFROMCOM_EXPORT __declspec(dllexport)
# else
# define SOTZMPREFFROMCOM_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(zmpreffromcom_EXPORTS)
#define SOTZMPREFFROMCOM_EXPORT __declspec(dllexport)
#else
#define SOTZMPREFFROMCOM_EXPORT __declspec(dllimport)
#endif
#else
# define SOTZMPREFFROMCOM_EXPORT
#define SOTZMPREFFROMCOM_EXPORT
#endif
namespace dynamicgraph { namespace sot {
namespace dynamicgraph {
namespace sot {
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom
:public dg::Entity
{
class SOTZMPREFFROMCOM_EXPORT ZmprefFromCom : 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:
double dt;
const static double DT_DEFAULT; // = 5e-3; // 5ms
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 --- */
ZmprefFromCom( const std::string& name );
virtual ~ZmprefFromCom( void );
ZmprefFromCom(const std::string& name);
virtual ~ZmprefFromCom(void);
public: /* --- SIGNAL --- */
dynamicgraph::Vector& computeZmpref(dynamicgraph::Vector& res, const int& time);
dynamicgraph::Vector& computeZmpref( dynamicgraph::Vector& res,
const int& time );
dg::SignalPtr<MatrixHomogeneous,int> waistPositionSIN;
dg::SignalPtr<dynamicgraph::Vector,int> comPositionSIN;
dg::SignalPtr<dynamicgraph::Vector,int> dcomSIN;
dg::SignalTimeDependent<dynamicgraph::Vector,int> zmprefSOUT;
dg::SignalPtr<MatrixHomogeneous, int> waistPositionSIN;
dg::SignalPtr<dynamicgraph::Vector, int> comPositionSIN;
dg::SignalPtr<dynamicgraph::Vector, int> dcomSIN;
dg::SignalTimeDependent<dynamicgraph::Vector, int> zmprefSOUT;
};
} /* namespace sot */
} /* namespace dynamicgraph */
} /* namespace sot */} /* namespace dynamicgraph */
#endif // #ifndef __SOT_ZMPREFFROMCOM_H__
#endif // #ifndef __SOT_ZMPREFFROMCOM_H__
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
# flake8: noqa
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
# Create a simple kinematic solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
from dynamic_graph.sot.core.matrix_util import matrixToRPY, matrixToTuple, rotate, vectorToTuple
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
# Create the robot romeo.
from dynamic_graph.sot.romeo.robot import *
from numpy import *
robot = Robot('romeo', device=RobotSimu('romeo'))
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a simple kinematic solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize ( robot )
solver = initialize(robot)
dt = 5e-3
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
dt=5e-3
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
runner = inc()
[go, stop, next, n] = loopShortcuts(runner)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
......@@ -36,9 +40,10 @@ runner=inc()
# ---- TASK GRIP ---
# Defines a task for the right hand.
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'right-wrist', 'right-wrist')
# Move the operational point.
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
......@@ -51,12 +56,12 @@ solver.sot.clear()
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
locals()['contact' + name] = contact
# --- RUN ----------------------------------------------------------------------
......@@ -65,16 +70,15 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
# 2st param: objective
# 3rd param: selected parameters
# 4th param: gain
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
target = (0.5, -0.2, 0.8)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
# Fill the stack with some tasks.
solver.push(contactRF.task)
solver.push(contactLF.task)
solver.push (robot.tasks ['com'])
solver.push(robot.tasks['com'])
solver.push(taskRH.task)
# type inc to run one iteration, or go to run indefinitely.
# go()
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
# flake8: noqa
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
# Create a simple kinematic solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
from dynamic_graph.sot.core.matrix_util import matrixToRPY, matrixToTuple, rotate, vectorToTuple
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
# Create the robot romeo.
from dynamic_graph.sot.romeo.robot import *
from numpy import *
robot = Robot('romeo_small', device=RobotSimu('romeo_small'))
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a simple kinematic solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize ( robot )
solver = initialize(robot)
dt = 5e-3
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
dt=5e-3
@loopInThread
def inc():
robot.device.increment(dt)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
runner = inc()
[go, stop, next, n] = loopShortcuts(runner)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
......@@ -36,9 +40,10 @@ runner=inc()
# ---- TASK GRIP ---
# Defines a task for the right hand.
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'right-wrist', 'right-wrist')
# Move the operational point.
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
......@@ -51,12 +56,12 @@ solver.sot.clear()
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]:
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
contact.feature.frame('desired')
contact.gain.setConstant(10)
contact.keep()
locals()['contact'+name] = contact
locals()['contact' + name] = contact
# --- RUN ----------------------------------------------------------------------
......@@ -65,16 +70,15 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
# 2st param: objective
# 3rd param: selected parameters
# 4th param: gain
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
target = (0.5, -0.2, 0.8)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
# Fill the stack with some tasks.
solver.push(contactRF.task)
solver.push(contactLF.task)
solver.push (robot.tasks ['com'])
solver.push(robot.tasks['com'])
solver.push(taskRH.task)
# type inc to run one iteration, or go to run indefinitely.
# go()
......@@ -7,53 +7,50 @@
*/
#ifndef ANGLE_ESTIMATOR_COMMAND_H
#define ANGLE_ESTIMATOR_COMMAND_H
#define ANGLE_ESTIMATOR_COMMAND_H
#include <boost/assign/list_of.hpp>
#include <boost/assign/list_of.hpp>
#include <dynamic-graph/command.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
namespace dynamicgraph { namespace sot {
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
namespace dynamicgraph {
namespace sot {
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
// Command FromSensor
class FromSensor : public Command
{
public:
virtual ~FromSensor() {
sotDEBUGIN(15);
sotDEBUGOUT(15);
}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
FromSensor(Dynamic& entity, const std::string& docstring) :
Command(entity, boost::assign::list_of(Value::STRING)
(Value::STRING)(Value::STRING)(Value::STRING), docstring)
{
}
virtual Value doExecute()
{
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string vrmlDirectory = values[0].value();
std::string vrmlMainFile = values[1].value();
std::string xmlSpecificityFiles = values[2].value();
std::string xmlRankFile = values[3].value();
robot.setVrmlDirectory(vrmlDirectory);
robot.setVrmlMainFile(vrmlMainFile);
robot.setXmlSpecificityFile(xmlSpecificityFiles);
robot.setXmlRankFile(xmlRankFile);
// return void
return Value();
}
}; // class FromSensor
} // namespace command
} // namespace sot
} // namespace dynamicgraph
// Command FromSensor
class FromSensor : public Command {
public:
virtual ~FromSensor() {
sotDEBUGIN(15);
sotDEBUGOUT(15);
}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
FromSensor(Dynamic& entity, const std::string& docstring)
: Command(entity, boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING)(Value::STRING),
docstring) {}
virtual Value doExecute() {
Dynamic& robot = static_cast<Dynamic&>(owner());
std::vector<Value> values = getParameterValues();
std::string vrmlDirectory = values[0].value();
std::string vrmlMainFile = values[1].value();
std::string xmlSpecificityFiles = values[2].value();
std::string xmlRankFile = values[3].value();
robot.setVrmlDirectory(vrmlDirectory);
robot.setVrmlMainFile(vrmlMainFile);
robot.setXmlSpecificityFile(xmlSpecificityFiles);
robot.setXmlRankFile(xmlRankFile);
// return void
return Value();
}
}; // class FromSensor
} // namespace command
} // namespace sot
} // namespace dynamicgraph
#endif //ANGLE_ESTIMATOR_COMMAND_H
#endif // ANGLE_ESTIMATOR_COMMAND_H
This diff is collapsed.
......@@ -7,66 +7,61 @@
*/
#ifndef DYNAMIC_COMMAND_H
#define DYNAMIC_COMMAND_H
#define DYNAMIC_COMMAND_H
#include <fstream>
#include <boost/assign/list_of.hpp>
#include <fstream>
#include <boost/assign/list_of.hpp>
#include <dynamic-graph/command.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
namespace dynamicgraph { namespace sot {
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
namespace dynamicgraph {
namespace sot {
namespace command {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
// Command DisplayModel
class DisplayModel : public Command
{
public:
virtual ~DisplayModel() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
DisplayModel(DynamicPinocchio& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(),
docstring)
{
}
virtual Value doExecute()
{
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner());
robot.displayModel();
return Value();
}
}; // class DisplayModel
// Command DisplayModel
class DisplayModel : public Command {
public:
virtual ~DisplayModel() {
sotDEBUGIN(15);
sotDEBUGOUT(15);
}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
DisplayModel(DynamicPinocchio& entity, const std::string& docstring)
: Command(entity, std::vector<Value::Type>(), docstring) {}
virtual Value doExecute() {
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner());
robot.displayModel();
return Value();
}
}; // class DisplayModel
// Command GetDimension
class GetDimension : public Command {
public:
virtual ~GetDimension() {
sotDEBUGIN(15);
sotDEBUGOUT(15);
}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
GetDimension(DynamicPinocchio& entity, const std::string& docstring)
: Command(entity, std::vector<Value::Type>(), docstring) {}
virtual Value doExecute() {
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner());
unsigned int dimension = robot.m_model->nv;
return Value(dimension);
}
}; // class GetDimension
// Command GetDimension
class GetDimension : public Command
{
public:
virtual ~GetDimension() { sotDEBUGIN(15);
sotDEBUGOUT(15);}
/// Create command and store it in Entity
/// \param entity instance of Entity owning this command
/// \param docstring documentation of the command
GetDimension(DynamicPinocchio& entity, const std::string& docstring) :
Command(entity, std::vector<Value::Type>(),
docstring)
{
}
virtual Value doExecute()
{
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner());
unsigned int dimension = robot.m_model->nv;
return Value(dimension);
}
}; // class GetDimension
} // namespace command
} /* namespace sot */
} /* namespace dynamicgraph */
} // namespace command
} /* namespace sot */} /* namespace dynamicgraph */
#endif //DYNAMIC_COMMAND_H
#endif // DYNAMIC_COMMAND_H
......@@ -14,13 +14,9 @@ using namespace dynamicgraph;
using namespace dynamicgraph::sot;
extern "C" {
::dynamicgraph::Entity*
EntityMaker_DynamicPinocchio(const std::string& objname)
{
return new DynamicPinocchio (objname);
}
::dynamicgraph::EntityRegisterer
reg_Dynamic ("DynamicPinocchio",
&EntityMaker_DynamicPinocchio);
::dynamicgraph::Entity* EntityMaker_DynamicPinocchio(const std::string& objname) {
return new DynamicPinocchio(objname);
}
//DYNAMICGRAPH_FACTORY_DYNAMIC_PLUGIN(Dynamic,"Dynamic");
::dynamicgraph::EntityRegisterer reg_Dynamic("DynamicPinocchio", &EntityMaker_DynamicPinocchio);
}
// DYNAMICGRAPH_FACTORY_DYNAMIC_PLUGIN(Dynamic,"Dynamic");
from dynamic import DynamicPinocchio as DynamicCpp
from angle_estimator import AngleEstimator
from zmp_from_forces import ZmpFromForces
import numpy as np
from numpy import arctan2, arcsin, sin, cos, sqrt
from numpy import cos, sin, sqrt
from dynamic import DynamicPinocchio as DynamicCpp
#DynamicOld = Dynamic
# DynamicOld = Dynamic
class DynamicPinocchio (DynamicCpp):
class DynamicPinocchio(DynamicCpp):
def __init__(self, name):
DynamicCpp.__init__(self, name)
self.model = None
self.data = None
def setData(self, pinocchio_data):
dynamic.wrap.set_pinocchio_data(self.obj,pinocchio_data)
dynamic.wrap.set_pinocchio_data(self.obj, pinocchio_data) # noqa TODO
self.data = pinocchio_data
return
def setModel(self, pinocchio_model):
dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model)
dynamic.wrap.set_pinocchio_model(self.obj, pinocchio_model) # noqa TODO
self.model = pinocchio_model
return
def fromSotToPinocchio(q_sot, freeflyer=True):
if freeflyer:
[r,p,y] = q_sot[3:6]
[r, p, y] = q_sot[3:6]
cr = cos(r)
cp = cos(p)
cy = cos(y)
......@@ -32,43 +33,42 @@ def fromSotToPinocchio(q_sot, freeflyer=True):
sp = sin(p)
sy = sin(y)
rotmat = np.matrix([[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
[sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
[-sp, cp*sr, cp*cr]])
rotmat = np.matrix([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], [-sp, cp * sr, cp * cr]])
d0 = rotmat[0,0]
d1 = rotmat[1,1]
d2 = rotmat[2,2]
rr = 1.0+d0+d1+d2
d0 = rotmat[0, 0]
d1 = rotmat[1, 1]
d2 = rotmat[2, 2]
rr = 1.0 + d0 + d1 + d2
if rr>0:
if rr > 0:
s = 0.5 / sqrt(rr)
_x = (rotmat[2,1] - rotmat[1,2]) * s
_y = (rotmat[0,2] - rotmat[2,0]) * s
_z = (rotmat[1,0] - rotmat[0,1]) * s
_x = (rotmat[2, 1] - rotmat[1, 2]) * s
_y = (rotmat[0, 2] - rotmat[2, 0]) * s
_z = (rotmat[1, 0] - rotmat[0, 1]) * s
_r = 0.25 / s
else:
#Trace is less than zero, so need to determine which
#major diagonal is largest
# Trace is less than zero, so need to determine which
# major diagonal is largest
if ((d0 > d1) and (d0 > d2)):
s = 0.5 / sqrt(1 + d0 - d1 - d2)
_x = 0.5 * s
_y = (rotmat[0,1] + rotmat[1,0]) * s
_z = (rotmat[0,2] + rotmat[2,0]) * s
_r = (rotmat[1,2] + rotmat[2,1]) * s
_y = (rotmat[0, 1] + rotmat[1, 0]) * s
_z = (rotmat[0, 2] + rotmat[2, 0]) * s
_r = (rotmat[1, 2] + rotmat[2, 1]) * s
elif (d1 > d2):
s = 0.5 / sqrt(1 + d0 - d1 - d2)
_x = (rotmat[0,1] + rotmat[1,0]) * s
_x = (rotmat[0, 1] + rotmat[1, 0]) * s
_y = 0.5 * s
_z = (rotmat[1,2] + rotmat[2,1]) * s
_r = (rotmat[0,2] + rotmat[2,0]) * s
_z = (rotmat[1, 2] + rotmat[2, 1]) * s
_r = (rotmat[0, 2] + rotmat[2, 0]) * s
else:
s = 0.5 / sqrt(1 + d0 - d1 - d2)
_x = (rotmat[0,2] + rotmat[2,0]) * s
_y = (rotmat[1,2] + rotmat[2,1]) * s
_x = (rotmat[0, 2] + rotmat[2, 0]) * s
_y = (rotmat[1, 2] + rotmat[2, 1]) * s
_z = 0.5 * s
_r = (rotmat[0,1] + rotmat[1,0]) * s
_r = (rotmat[0, 1] + rotmat[1, 0]) * s
return np.matrix([q_sot[0:3]+(_x,_y,_z,_r)+q_sot[6:]])
return np.matrix([q_sot[0:3] + (_x, _y, _z, _r) + q_sot[6:]])
else:
return np.matrix([q_sot[0:]])
......@@ -2,8 +2,10 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from __future__ import print_function
import sys
from functools import reduce
# Robotviewer is optional
hasRobotViewer = True
......@@ -12,33 +14,25 @@ try:
except ImportError:
hasRobotViewer = False
####################
# Helper functions #
####################
def toList(tupleOfTuple):
result = [[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],