diff --git a/.gitignore b/.gitignore index 6799242..5d1eb8b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ *.pyc *~ build +_build* .gitignore diff --git a/CMakeLists.txt b/CMakeLists.txt index 3ff475e..f4e985e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,12 +9,17 @@ SET(PROJECT_DESCRIPTION "pinocchio bindings for dynamic-graph.") SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}") SET(PROJECT_SUFFIX "-v3") +SET(URDFDOM_COMPILE_FLAGS " -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_URDFDOM_USE_STD_SHARED_PTR -DPINOCCHIO_WITH_URDFDOM") + +add_definitions(${URDFDOM_COMPILE_FLAGS}) + # Project options OPTION(BUILD_PYTHON_INTERFACE "Build the python bindings" ON) OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF) OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON) # Project configuration +SET(CMAKE_CXX_STANDARD 11) IF(NOT INSTALL_PYTHON_INTERFACE_ONLY) SET(PROJECT_USE_CMAKE_EXPORT TRUE) ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) @@ -61,6 +66,7 @@ SET(${PROJECT_NAME}_HEADERS include/${CUSTOM_HEADER_DIR}/matrix-inertia.h include/${CUSTOM_HEADER_DIR}/integrator-force-rk4.h include/${CUSTOM_HEADER_DIR}/angle-estimator.h + include/${CUSTOM_HEADER_DIR}/state-integrator.h ) SET(${PROJECT_NAME}_SOURCES diff --git a/include/sot/dynamic-pinocchio/state-integrator.h b/include/sot/dynamic-pinocchio/state-integrator.h new file mode 100644 index 0000000..75ce013 --- /dev/null +++ b/include/sot/dynamic-pinocchio/state-integrator.h @@ -0,0 +1,251 @@ +/* + * Copyright 2010-2018, CNRS + * Florent Lamiraux + * Olivier Stasse + * + * CNRS + * + * See LICENSE.txt + */ + +#ifndef SOT_STATEINTEGRATOR_HH +#define SOT_STATEINTEGRATOR_HH + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#include +#include +#include +/* SOT */ +/// dg +#include +#include +/// sot-core +#include "sot/core/periodic-call.hh" +#include +#include "sot/core/api.hh" +#include +/// pinocchio +#include "pinocchio/multibody/model.hpp" +#include +#include "pinocchio/algorithm/joint-configuration.hpp" +#include "pinocchio/math/quaternion.hpp" + +/* --------------------------------------------------------------------- */ +/* --- API ------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#if defined (WIN32) +# if defined (state_integrator_EXPORTS) +# define SOTSTATEINTEGRATOR_EXPORT __declspec(dllexport) +# else +# define SOTSTATEINTEGRATOR_EXPORT __declspec(dllimport) +# endif +#else +# define SOTSTATEINTEGRATOR_EXPORT +#endif + + +namespace dgsot = dynamicgraph::sot; +namespace dg = dynamicgraph; + +namespace dynamicgraph { +namespace sot { + +typedef Eigen::Matrix StringVector; + +/// Specifies the nature of one joint control from SoT side +enum SoTControlType { + qVEL = 0, + qACC = 1, + ffVEL = 2, + ffACC = 3 +}; + +const std::string SoTControlType_s[] = { + "qVEL", "qACC", "ffVEL", "ffACC" +}; + + + +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + static const std::string CLASS_NAME; + virtual const std::string& getClassName(void) const {return CLASS_NAME;} + static const double TIMESTEP_DEFAULT; + + /// Set integration time. + void init(const double& step); + + protected: + /// \brief Current integration step. + double timestep_; + + /// \name Vectors related to the state. + ///@{ + /// Position of the robot wrt pinocchio. + Eigen::VectorXd position_; + /// Velocity of the robot wrt pinocchio. + Eigen::VectorXd velocity_; + /// Acceleration vector of each actuator. + dg::Vector acceleration_; + + /// Type of the control vector + /// It can be velocity or acceleration for the actuators. + std::vector controlTypeVector_; + /// Type of the control for the Freeflyer (velocity/acceleration). + SoTControlType controlTypeFF_; + + /// Store Position of free flyer joint + Eigen::VectorXd ffPose_; + /// Store Velocity of free flyer joint + Eigen::VectorXd ffVel_; + ///@} + + bool sanityCheck_; + + public: + + /* --- CONSTRUCTION --- */ + StateIntegrator(const std::string& name); + /* --- DESTRUCTION --- */ + virtual ~StateIntegrator(); + + virtual void setState(const dg::Vector& st); + void setVelocitySize(const unsigned int& size); + virtual void setVelocity(const dg::Vector & vel); + + virtual void setStateFreeflyer( const dg::Vector& st); + virtual void setVelocityFreeflyer(const dg::Vector & vel); + + /// Read directly the URDF model + void setURDFModel(const std::string &aURDFModel); + + /// \name Sanity check parameterization + /// \{ + void setSanityCheck(const bool & enableCheck); + /// \} + + /// \name Set the control types of the controlled joints + /// Allowed types (string): qVEL | qACC + void setControlType(const StringVector& controlTypeVector); + /// \name Set the control types of the controlled freeflyer + /// Allowed types (string): ffVEL | ffACC + void setControlTypeFreeFlyer(const std::string& controlTypeFF); + + /// \name Set the control type of a specific joint + /// Allowed types (int): qVEL:0 | qACC:1 + void setControlTypeJointInt(const int& jointNumber, const int& intType); + /// \name Set the control types of the controlled joints + /// Allowed types (int): qVEL:0 | qACC:1 + void setControlTypeInt(const Vector& controlTypeVector); + + /// \name Get the control type from a string (of the controlTypeVector) as in the enum + /// Check the types: qVEL | qACC | ffVEL | ffACC + /// \{ + int getControlType(const std::string &strCtrlType, SoTControlType &aCtrlType); + /// \} + + public: /* --- DISPLAY --- */ + virtual void display(std::ostream& os) const; + SOT_CORE_EXPORT friend std::ostream& operator<<(std::ostream& os, const StateIntegrator& r) { + r.display(os); return os; + } + + public: /* --- SIGNALS --- */ + + /// Input signal handling the control vector + /// This entity needs a control vector to be send to the device. + dg::SignalPtr controlSIN; + /// Input signal handling the control vector of the freeflyer + dg::SignalPtr freeFlyerSIN; + + /// \name StateIntegrator current state. + /// \{ + /// \brief Output integrated state from control. + dg::SignalTimeDependent stateSOUT_; + /// \brief Output integrated velocity from control. + dg::SignalTimeDependent velocitySOUT_; + /// \brief Output integrated freeFlyer position from control (odometry predictive system) with euler angles. + dg::SignalTimeDependent freeFlyerPositionEulerSOUT_; + /// \brief Output integrated freeFlyer position from control (odometry predictive system) with quaternions. + dg::SignalTimeDependent freeFlyerPositionQuatSOUT_; + /// \brief Output integrated freeFlyer velocity from control. + dg::SignalTimeDependent freeFlyerVelocitySOUT_; + /// \} + ///@} + + protected: + + /// Integrate the freeflyer state (to obtain position). + /// Compute roll pitch yaw angles + void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control, double dt); + // Computes Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] + void rotationMatrixToEuler(Eigen::Matrix3d& rotationMatrix, Eigen::Vector3d& rollPitchYaw); + + /// Store Position of free flyer joint as MatrixHomogeneous + MatrixHomogeneous freeFlyerPose_; + + /// Compute the new position, from the current control. + /// When sanity checks are enabled, this checks that the control has no NAN value. + /// There are two cases, depending on what the control is: + /// - velocity: integrate once to obtain the future position + /// - acceleration: integrate two times to obtain the future position + virtual void integrateControl(int t, const double & dt = 5e-2); + /// Compute the new freeflyer position, from the current control. + /// When sanity checks are enabled, this checks that the control has no NAN value. + /// There are two cases, depending on what the control is: + /// - velocity: integrate once to obtain the future position + /// - acceleration: integrate two times to obtain the future position + virtual void integrateFreeFlyer(int t, const double & dt = 5e-2); + + /// \brief Provides the itegrated control information in position (callback signal stateSOUT_). + dg::Vector& getPosition(dg::Vector &controlOut, const int& t); + /// \brief Provides the itegrated control information in velocity (callback signal velocitySOUT_). + dg::Vector& getVelocity(dg::Vector &controlOut, const int& t); + /// \brief Provides the itegrated control information of the freeflyer in position with euler angles + /// (callback signal freeFlyerPositionEulerSOUT_). + dg::Vector& getFreeFlyerPositionEuler(dg::Vector &ffPose, const int& t); + /// \brief Provides the itegrated control information of the freeflyer in position with quaternions + /// (callback signal freeFlyerPositionQuatSOUT_). + dg::Vector& getFreeFlyerPositionQuat(dg::Vector &ffPose, const int& t); + /// \brief Provides the itegrated control information of the freeflyer in velocity (callback signal freeFlyerVelocitySOUT_). + dg::Vector& getFreeFlyerVelocity(dg::Vector &ffVel, const int& t); + + public: + /// Get freeflyer pose + const MatrixHomogeneous& freeFlyerPose(); + virtual void setRoot( const dg::Matrix & root ); + virtual void setRoot( const MatrixHomogeneous & worldMwaist ); + + private: + + // Pinocchio Model of the robot + pinocchio::Model model_; + // Debug mode + int debug_mode_; + // Last integration iteration + int last_integration_; + // Last integration iteration for freeflyer + int last_integration_FF_; + + public: + + const pinocchio::Model & getModel() { + return model_; + } + +}; +} // namespace sot +} // namespace dynamicgraph + + +#endif /* #ifndef SOT_STATEINTEGRATOR_HH */ \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 913568f..8be86d9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -15,6 +15,7 @@ SET(plugins waist-attitude-from-sensor zmp-from-forces dynamic + state-integrator ) SET(integrator-force-rk4_plugins_deps integrator-force) diff --git a/src/state-integrator.cpp b/src/state-integrator.cpp new file mode 100644 index 0000000..b1a0f6b --- /dev/null +++ b/src/state-integrator.cpp @@ -0,0 +1,562 @@ +/* + * Copyright 2019, CNRS + * Author: Olivier Stasse + * + * Please check LICENSE.txt for licensing + * + */ + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +/* SOT */ +#define ENABLE_RT_LOG + +#include "sot/dynamic-pinocchio/state-integrator.h" +#include +using namespace std; + +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace dynamicgraph::sot; +using namespace dynamicgraph; + +#define DBGFILE "/tmp/state_integrator.txt" + +#if 0 +#define RESETDEBUG5() { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::out); \ + DebugFile.close();} +#define ODEBUG5FULL(x) { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::app); \ + DebugFile << __FILE__ << ":" \ + << __FUNCTION__ << "(#" \ + << __LINE__ << "):" << x << std::endl; \ + DebugFile.close();} +#define ODEBUG5(x) { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::app); \ + DebugFile << x << std::endl; \ + DebugFile.close();} + +#else +// Void the macro +#define RESETDEBUG5() +#define ODEBUG5FULL(x) +#define ODEBUG5(x) +#endif + +DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StateIntegrator, "StateIntegrator"); +//const std::string StateIntegrator::CLASS_NAME = "StateIntegrator"; +const double StateIntegrator::TIMESTEP_DEFAULT = 0.001; + +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +StateIntegrator::StateIntegrator( const std::string& n ) + : Entity(n) + , timestep_(TIMESTEP_DEFAULT) + , position_(6) + , ffPose_(6) + , sanityCheck_(true) + , controlSIN( NULL, "StateIntegrator(" + n + ")::input(double)::control" ) + , freeFlyerSIN( NULL, "StateIntegrator(" + n + ")::input(double)::freeFlyer" ) + , stateSOUT_(boost::bind(&StateIntegrator::getPosition, this, _1, _2), + controlSIN, + "StateIntegrator(" + n + ")::output(vector)::state" ) + , velocitySOUT_(boost::bind(&StateIntegrator::getVelocity, this, _1, _2), + controlSIN, + "StateIntegrator(" + n + ")::output(vector)::velocity" ) + , freeFlyerPositionEulerSOUT_(boost::bind(&StateIntegrator::getFreeFlyerPositionEuler, this, _1, _2), + freeFlyerSIN, + "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionEuler") + , freeFlyerPositionQuatSOUT_(boost::bind(&StateIntegrator::getFreeFlyerPositionQuat, this, _1, _2), + freeFlyerSIN, + "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionQuat") + , freeFlyerVelocitySOUT_(boost::bind(&StateIntegrator::getFreeFlyerVelocity, this, _1, _2), + freeFlyerSIN, + "StateIntegrator(" + n + ")::output(vector)::freeFlyerVelocity") + , debug_mode_(5) + , last_integration_(-1) + , last_integration_FF_(-1) { + signalRegistration( controlSIN + << freeFlyerSIN + << stateSOUT_ + << velocitySOUT_ + << freeFlyerPositionEulerSOUT_ + << freeFlyerPositionQuatSOUT_ + << freeFlyerVelocitySOUT_); + position_.fill(.0); + + velocity_.resize(position_.size()); + velocity_.setZero(); + + ffPose_.fill(.0); + + ffVel_.resize(ffPose_.size()); + ffVel_.setZero(); + + /* --- Commands --- */ + { + std::string docstring; + + docstring = + "\n" + " Set integration timestep value\n" + "\n"; + addCommand("init", + new command::Setter(*this, &StateIntegrator::init, docstring)); + + docstring = + "\n" + " Set state vector value\n" + "\n"; + addCommand("setState", + new command::Setter(*this, &StateIntegrator::setState, docstring)); + + docstring = + "\n" + " Set velocity vector value\n" + "\n"; + addCommand("setVelocity", + new command::Setter(*this, &StateIntegrator::setVelocity, docstring)); + + docstring = + "\n" + " Set Freeflyer state vector value\n" + "\n"; + addCommand("setStateFreeflyer", + new command::Setter(*this, &StateIntegrator::setStateFreeflyer, docstring)); + + docstring = + "\n" + " Set Freeflyer velocity vector value\n" + "\n"; + addCommand("setVelocityFreeflyer", + new command::Setter(*this, &StateIntegrator::setVelocityFreeflyer, docstring)); + + docstring = + "\n" + " Set control type vector value (for each joint).\n" + " Vector of types (int): qVEL:0 | qACC:1 \n" + "\n"; + addCommand("setControlType", + new command::Setter(*this, &StateIntegrator::setControlTypeInt, docstring)); + + docstring = + "\n" + " Set control type value for a joint.\n" + " Joint position (int) \n" + " Type (int): qVEL:0 | qACC:1 \n" + "\n"; + addCommand("setControlTypeJoint", + command::makeCommandVoid2(*this, &StateIntegrator::setControlTypeJointInt, docstring)); + + docstring = + "\n" + " Set control type value (for freeflyer).\n" + " type (string): ffVEL | ffACC\n" + "\n"; + addCommand("setControlTypeFreeFlyer", + new command::Setter(*this, &StateIntegrator::setControlTypeFreeFlyer, docstring)); + + void(StateIntegrator::*setRootPtr)(const Matrix&) = &StateIntegrator::setRoot; + docstring = command::docCommandVoid1("Set the root position.", + "matrix homogeneous"); + addCommand("setRoot", command::makeCommandVoid1(*this, setRootPtr, docstring)); + + docstring = + "\n" + " Enable/Disable sanity checks\n" + "\n"; + addCommand("setSanityCheck", + new command::Setter + (*this, &StateIntegrator::setSanityCheck, docstring)); + } +} + +StateIntegrator::~StateIntegrator( ) { + return; +} + +void StateIntegrator::init(const double& step) { + timestep_ = step; +} + +void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control, double dt) { + using Eigen::AngleAxisd; + using Eigen::Vector3d; + using Eigen::Matrix3d; + using Eigen::QuaternionMapd; + + typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3; + Eigen::Matrix qin, qout; + qin.head<3>() = state.head<3>(); + + QuaternionMapd quat (qin.tail<4>().data()); + quat = AngleAxisd(state(5), Vector3d::UnitZ()) + * AngleAxisd(state(4), Vector3d::UnitY()) + * AngleAxisd(state(3), Vector3d::UnitX()); + + SE3().integrate (qin, control * dt, qout); + + Matrix3d rotationMatrix = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix(); + // Create the Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] + Vector3d rollPitchYaw; + rotationMatrixToEuler(rotationMatrix, rollPitchYaw); + // Update freeflyer state (pose) + state.head<3>() = qout.head<3>(); + state.segment<3>(3) << rollPitchYaw; + +} + +void StateIntegrator::rotationMatrixToEuler(Eigen::Matrix3d& rotationMatrix, Eigen::Vector3d& rollPitchYaw){ + double m = sqrt(rotationMatrix(2, 1) * rotationMatrix(2, 1) + rotationMatrix(2, 2) * rotationMatrix(2, 2)); + double p = atan2(-rotationMatrix(2, 0), m); + double r, y; + if (abs(abs(p) - M_PI / 2) < 0.001) { + r = 0; + y = -atan2(rotationMatrix(0, 1), rotationMatrix(1, 1)); + } else { + y = atan2(rotationMatrix(1, 0), rotationMatrix(0, 0)); // alpha + r = atan2(rotationMatrix(2, 1), rotationMatrix(2, 2)); // gamma + } + rollPitchYaw << r, p, y; +} + +const MatrixHomogeneous& StateIntegrator::freeFlyerPose() { + using Eigen::AngleAxisd; + using Eigen::Vector3d; + using Eigen::Quaterniond; + + freeFlyerPose_.translation() = ffPose_.head<3>(); + Quaterniond quat; + quat = AngleAxisd(ffPose_(5), Vector3d::UnitZ()) + * AngleAxisd(ffPose_(4), Vector3d::UnitY()) + * AngleAxisd(ffPose_(3), Vector3d::UnitX()); + freeFlyerPose_.linear() = quat.toRotationMatrix(); + + return freeFlyerPose_; +} + +void StateIntegrator::setControlType(const StringVector& controlTypeVector) { + SoTControlType type; + controlTypeVector_.resize(controlTypeVector.size()); + + for (unsigned int j = 0; j < controlTypeVector.size(); j++) { + if (getControlType(controlTypeVector[j], type) > 0) { + if (debug_mode_ > 1) { + std::cerr << "No control type for joint at position " << j + << " in the controlType vector" + << std::endl; + } + break; + } + controlTypeVector_[j] = type; + } +} + +void StateIntegrator::setControlTypeFreeFlyer(const std::string& controlTypeFF) { + SoTControlType typeFF; + if (getControlType(controlTypeFF, typeFF) > 0) { + if (debug_mode_ > 1) { + std::cerr << "No control type for Freeflyer." << std::endl; + } + return; + } + controlTypeFF_ = typeFF; +} + +void StateIntegrator::setControlTypeJointInt(const int& jointNumber, const int& intType){ + SoTControlType type; + try { + type = (SoTControlType)intType; + } catch (...) { + dgRTLOG () << "StateIntegrator::setControlTypeJointInt: The controlType at position " + << jointNumber << " is not valid: " << intType << "\n" + << "Expected values are (int): qVEL = 0 | qACC = 1" + << '\n'; + return; + } + controlTypeVector_[jointNumber] = type; +} + +void StateIntegrator::setControlTypeInt(const Vector& controlTypeVector) { + controlTypeVector_.resize(controlTypeVector.size()); + for (unsigned int i = 0; i < controlTypeVector.size(); i++) { + setControlTypeJointInt(i, int(controlTypeVector[i])); + } +} + +void StateIntegrator::setURDFModel(const std::string &aURDFModel) { + pinocchio::urdf::buildModelFromXML(aURDFModel, model_, false); + position_.resize(model_.nq); + velocity_.resize(model_.nv); + acceleration_.resize(model_.nv); +} + +void StateIntegrator::setState( const Vector& st ) { + if (st.size() == model_.nq) { + position_ = st; + } else { + std::cerr << "Asked size of State vector: " << st.size() + << " not the expected one: " << model_.nq << std::endl; + } +} + +void StateIntegrator::setVelocity( const Vector& vel ) { + if (vel.size() == model_.nv) { + velocity_ = vel; + } else { + std::cerr << "Asked size of Velocity vector: " << vel.size() + << " not the expected one: " << model_.nv << std::endl; + } +} + +void StateIntegrator::setStateFreeflyer( const Vector& st ) { + if (st.size() == 6 || st.size() == 7) { + ffPose_ = st; + } else { + std::cerr << "Asked size of State Freeflyer vector: " << st.size() + << " not the expected one: 6 (euler angle) or 7 (quaternion)" << std::endl; + } +} + +void StateIntegrator::setVelocityFreeflyer( const Vector& vel ) { + if (vel.size() == 6) { + ffVel_ = vel; + } else { + std::cerr << "Asked size of velocity Freeflyer vector: " << vel.size() + << " not the expected one: 6 (twist)" << std::endl; + } +} + +void StateIntegrator::setRoot( const Matrix & root ) { + Eigen::Matrix4d _matrix4d(root); // needed to define type of root for transformation to MatrixHomogeneous + MatrixHomogeneous _root(_matrix4d); + setRoot( _root ); +} + +void StateIntegrator::setRoot( const MatrixHomogeneous & worldMwaist ) { + freeFlyerPose_ = worldMwaist; + ffPose_.head<3>() = worldMwaist.translation(); + Eigen::Vector3d rollPitchYaw; + Eigen::Matrix3d rot(worldMwaist.linear()); + rotationMatrixToEuler(rot, rollPitchYaw); + ffPose_.segment<3>(3) << rollPitchYaw; +} + +void StateIntegrator::setSanityCheck(const bool & enableCheck) { + sanityCheck_ = enableCheck; +} + +int StateIntegrator::getControlType(const std::string &strCtrlType, SoTControlType &aCtrlType) { + for (int j = 0; j < 4; j++) { + if (strCtrlType == SoTControlType_s[j]) { + aCtrlType = (SoTControlType)j; + return 0; + } + } + if (debug_mode_ > 1) { + std::cerr << "Control type not allowed/recognized: " + << strCtrlType + << "\n Authorized control types: " + << "qVEL | qACC | ffVEL | ffACC" + << std::endl; + } + return 1; +} + +void StateIntegrator::integrateControl(int t, const double & dt) { + controlSIN(t); + const Vector & controlIN = controlSIN.accessCopy(); + + if (controlTypeVector_.size() == 0) { + dgRTLOG () << "StateIntegrator::integrate: The controlType vector cannot be empty" << '\n'; + return; + } + + if (sanityCheck_ && controlIN.hasNaN()) { + dgRTLOG () << "StateIntegrator::integrate: Control has NaN values: " << '\n' + << controlIN.transpose() << '\n'; + return; + } + // Integration joints + for (unsigned int j = 0; j < controlTypeVector_.size(); j++) { + // Control of a joint in acceleration + if (controlTypeVector_[j] == qACC) { + // Set acceleration from control and integrate to find velocity. + acceleration_[j] = controlIN[j]; + velocity_[j] = velocity_[j] + acceleration_[j] * (0.5) * dt; + } + // Control of a joint in velocity + else if (controlTypeVector_[j] == qVEL) { + // Set velocity from control. + acceleration_[j] = 0; + velocity_[j] = controlIN[j]; + } + + // Velocity integration of the joint to get position + position_[j] = position_[j] + velocity_[j] * dt; + } +} + +void StateIntegrator::integrateFreeFlyer(int t, const double & dt) { + freeFlyerSIN(t); + const Vector & freeFlyerIN = freeFlyerSIN.accessCopy(); + + if (controlTypeFF_ != ffACC && controlTypeFF_ != ffVEL) { + dgRTLOG () << "StateIntegrator::integrate: The controlType of the freeflyer is not properly set (empty?)" << '\n'; + return; + } + + if (sanityCheck_ && freeFlyerIN.hasNaN()) { + dgRTLOG () << "StateIntegrator::integrate: Control of the FreeFlyer has NaN values: " << '\n' + << freeFlyerIN.transpose() << '\n'; + return; + } + + // Control of the freeflyer in acceleration + if (controlTypeFF_ == ffACC) { + // Integrate once to obtain velocity -> update ffVel_ + ffVel_ = ffVel_ + freeFlyerIN * (0.5) * dt; + } + // Control of the freeflyer in velocity + else if (controlTypeFF_ == ffVEL) { + // Set ffVel_ (twist) from control for the integration in position + ffVel_ = freeFlyerIN; + } + // Integrate freeflyer velocity to obtain position -> update ffPose_ from ffVel_ + integrateRollPitchYaw(ffPose_, ffVel_, dt); +} + +/* --- DISPLAY ------------------------------------------------------------ */ + +void StateIntegrator::display ( std::ostream& os ) const { + os << name << ": " << position_ << endl; +} + + +Vector& StateIntegrator::getPosition(Vector &controlOut, const int& t) { + ODEBUG5FULL("start"); + sotDEBUGIN(25) ; + + // Integrate control + if (last_integration_ != t) { + integrateControl(t, timestep_); + last_integration_ = t; + } + sotDEBUG (25) << "position_ = " << position_ << std::endl; + + ODEBUG5FULL("position_ = " << position_); + + controlOut = position_; + + return controlOut; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + +Vector& StateIntegrator::getVelocity(Vector &controlOut, const int& t) { + ODEBUG5FULL("start"); + sotDEBUGIN(25) ; + + // Integrate control + if (last_integration_ != t) { + integrateControl(t, timestep_); + last_integration_ = t; + } + sotDEBUG (25) << "velocity_ = " << velocity_ << std::endl; + + ODEBUG5FULL("velocity_ = " << velocity_); + + controlOut = velocity_; + + return controlOut; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + +Vector& StateIntegrator::getFreeFlyerPositionEuler(Vector &ffPose, const int& t) { + ODEBUG5FULL("start"); + sotDEBUGIN(25); + + // Integrate control + if (last_integration_FF_ != t) { + integrateFreeFlyer(t, timestep_); + last_integration_FF_ = t; + } + sotDEBUG (25) << "ffPose_ = " << ffPose_ << std::endl; + + ODEBUG5FULL("ffPose_ = " << ffPose_); + + ffPose = ffPose_; + + return ffPose; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + +Vector& StateIntegrator::getFreeFlyerPositionQuat(Vector &ffPose, const int& t) { + using Eigen::Quaterniond; + using Eigen::AngleAxisd; + using Eigen::Vector3d; + ODEBUG5FULL("start"); + sotDEBUGIN(25); + + ffPose.resize(7); + // Integrate control + if (last_integration_FF_ != t) { + integrateFreeFlyer(t, timestep_); + last_integration_FF_ = t; + } + + ffPose.head<3>() = ffPose_.head<3>(); + Quaterniond quat; + quat = AngleAxisd(ffPose_(5), Vector3d::UnitZ()) + * AngleAxisd(ffPose_(4), Vector3d::UnitY()) + * AngleAxisd(ffPose_(3), Vector3d::UnitX()); + ffPose.segment<4>(3) = quat.coeffs(); + + sotDEBUG (25) << "ffPose = " << ffPose << std::endl; + + ODEBUG5FULL("ffPose = " << ffPose); + + return ffPose; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + +Vector& StateIntegrator::getFreeFlyerVelocity(Vector &ffVel, const int& t) { + ODEBUG5FULL("start"); + sotDEBUGIN(25); + + // Integrate control + if (last_integration_FF_ != t) { + integrateFreeFlyer(t, timestep_); + last_integration_FF_ = t; + } + sotDEBUG (25) << "ffVel_ = " << ffVel_ << std::endl; + + ODEBUG5FULL("ffVel_ = " << ffVel_); + + ffVel = ffVel_; + + return ffVel; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25); +} \ No newline at end of file diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 19b558d..cbd8907 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,13 +1,14 @@ # Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST SET(tests - test_constructor - #test_config - # dummy - # test_djj - # test_dyn - # test_results - ) +test_constructor +test_state_integrator +#test_config +# dummy +# test_djj +# test_dyn +# test_results +) SET(test_dyn_plugins_dependencies dynamic) @@ -40,9 +41,13 @@ FOREACH(test ${tests}) integrator-force angle-estimator waist-attitude-from-sensor + state-integrator ) ENDFOREACH(test) +configure_file(sot_controller.yaml ${CMAKE_CURRENT_BINARY_DIR} COPYONLY) +configure_file(sot_params.yaml ${CMAKE_CURRENT_BINARY_DIR} COPYONLY) + IF(BUILD_PYTHON_INTERFACE) ADD_SUBDIRECTORY(python) ENDIF(BUILD_PYTHON_INTERFACE) diff --git a/tests/sot_controller.yaml b/tests/sot_controller.yaml new file mode 100644 index 0000000..d5e68db --- /dev/null +++ b/tests/sot_controller.yaml @@ -0,0 +1,37 @@ +sot_controller: + type: sot_controller/RCSotController + joints: + - LLEG_HIP_P + - LLEG_HIP_R + - LLEG_HIP_Y + - LLEG_KNEE + - LLEG_ANKLE_P + - LLEG_ANKLE_R + - RLEG_HIP_P + - RLEG_HIP_R + - RLEG_HIP_Y + - RLEG_KNEE + - RLEG_ANKLE_P + - RLEG_ANKLE_R + - WAIST_P + - WAIST_R + - CHEST + - RARM_SHOULDER_P + - RARM_SHOULDER_R + - RARM_SHOULDER_Y + - RARM_ELBOW + - RARM_WRIST_Y + - RARM_WRIST_P + - RARM_WRIST_R + - LARM_SHOULDER_P + - LARM_SHOULDER_R + - LARM_SHOULDER_Y + - LARM_ELBOW + - LARM_WRIST_Y + - LARM_WRIST_P + - LARM_WRIST_R + + left_ft_sensor: left_ankle_ft + right_ft_sensor: right_ankle_ft + base_imu_sensor: base_imu + check_mode: False diff --git a/tests/sot_params.yaml b/tests/sot_params.yaml new file mode 100644 index 0000000..91cd35b --- /dev/null +++ b/tests/sot_params.yaml @@ -0,0 +1,9 @@ +sot_controller: + libname: libsot-controller.so + joint_names: [ LLEG_HIP_P, LLEG_HIP_R, LLEG_HIP_Y, LLEG_KNEE, LLEG_ANKLE_P, LLEG_ANKLE_R, RLEG_HIP_P, RLEG_HIP_R, RLEG_HIP_Y, RLEG_KNEE, RLEG_ANKLE_P, RLEG_ANKLE_R, WAIST_P, WAIST_R, CHEST, RARM_SHOULDER_P, RARM_SHOULDER_R, RARM_SHOULDER_Y, RARM_ELBOW, RARM_WRIST_Y, RARM_WRIST_P, RARM_WRIST_R, LARM_SHOULDER_P, LARM_SHOULDER_R, LARM_SHOULDER_Y, LARM_ELBOW, LARM_WRIST_Y, LARM_WRIST_P, LARM_WRIST_R ] + map_rc_to_sot_device: { motor-angles: motor-angles , + joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents, + torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 } + control_mode: POSITION + dt: 0.001 + jitter: 0.0004 diff --git a/tests/test_state_integrator.cpp b/tests/test_state_integrator.cpp new file mode 100644 index 0000000..657ea66 --- /dev/null +++ b/tests/test_state_integrator.cpp @@ -0,0 +1,196 @@ +/* + * Copyright 2018, + * Olivier Stasse, + * + * CNRS + * See LICENSE.txt + * + */ + +#include +#include + +#ifndef WIN32 +#include +#endif + +using namespace std; +#include +#include +#include +#include +#include +#include +#include +#include "sot/dynamic-pinocchio/state-integrator.h" + +#include +#include + +namespace dg = dynamicgraph; + +#define BOOST_TEST_MODULE test-state-integrator + +int ReadYAMLFILE(dg::sot::GenericDevice &aDevice) { + // Reflect how the data are splitted in two yaml files in the sot + std::ifstream yaml_file_controller("sot_controller.yaml"); + std::string yaml_string_controller; + yaml_string_controller.assign((std::istreambuf_iterator(yaml_file_controller) ), + (std::istreambuf_iterator() ) ); + aDevice.ParseYAMLString(yaml_string_controller); + + std::ifstream yaml_file_params("sot_params.yaml"); + std::string yaml_string_params; + yaml_string_params.assign((std::istreambuf_iterator(yaml_file_params) ), + (std::istreambuf_iterator() ) ); + aDevice.ParseYAMLString(yaml_string_params); + return 0; +} + +BOOST_AUTO_TEST_CASE(test_state_integrator) { + + unsigned int debug_mode = 2; + + // Get environment variable CMAKE_PREFIX_PATH + const string s_cmake_prefix_path = getenv( "CMAKE_PREFIX_PATH" ); + + // Read the various paths + vector paths; + boost::split(paths, s_cmake_prefix_path, boost::is_any_of(":;")); + + // Search simple_humanoid.urdf + string filename=""; + for (auto test_path : paths) + { + filename = test_path + + string("/share/simple_humanoid_description/urdf/simple_humanoid.urdf"); + if ( boost::filesystem::exists(filename)) + break; + } + + // If not found fails + if (filename.size()==0) + { + cerr << "Unable to find simple_humanoid_description/urdf/simple_humanoid.urdf in CMAKE_PREFIX_PATH" << endl; + exit(-1); + } + + // Otherwise read the file + ifstream urdfFile(filename); + ostringstream strStream; + strStream << urdfFile.rdbuf(); + std::string robot_description; + robot_description = strStream.str(); + + /// Test reading the URDF file. + dg::sot::GenericDevice aDevice(std::string("simple_humanoid")); + aDevice.setDebugMode(debug_mode); + aDevice.setURDFModel(robot_description); + + if (ReadYAMLFILE(aDevice) < 0){ + BOOST_CHECK(false); + return; + } + + dg::sot::StateIntegrator aIntegrator(std::string("integrator")); + aIntegrator.init(0.005); + aIntegrator.setURDFModel(robot_description); + + dg::Vector aState(29); // without freeFlyer + for (unsigned j = 0; j < aState.size(); j++) + aState(j) = 0.0; + aIntegrator.setState(aState); + + /// Fix constant vector for the control entry of the integrator + dg::Vector aControlVector(29); + for (unsigned int i = 0; i < 29; i++) { + aControlVector[i] = -0.5; // in velocity + } + aIntegrator.controlSIN.setConstant(aControlVector); + + // Fix FreeFlyer control entry of the integrator + dg::Vector aFFControlVector(6); //TWIST + for (unsigned int i = 0; i < 6; i++) { + aFFControlVector[i] = -0.5; // in velocity + } + aIntegrator.freeFlyerSIN.setConstant(aFFControlVector); + + // Set the type vector defining the type of control for each joint + // With strings + Eigen::Matrix aControlTypeVector; + for (unsigned int i = 0; i < 29; i++) { + aControlTypeVector[i] = "qVEL"; //velocity + } + aIntegrator.setControlType(aControlTypeVector); + // Set type of control for the FreeFlyer + aIntegrator.setControlTypeFreeFlyer("ffVEL"); //in velocity + + // With int -> for addCommand + // dg::Vector aControlTypeVector(29); + // Types in int qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 + // aIntegrator.setControlTypeFreeFlyer("ffVEL"); // with freeFlyer in velocity + // for (unsigned int i = 0; i < 29; i++) + // { + // aControlTypeVector[i] = 0.0; //velocity + // } + // aIntegrator.setControlTypeInt(aControlTypeVector); + + // PLUG the output signal of the integrator to the entry of the device + aDevice.stateSIN.plug(&aIntegrator.stateSOUT_); + for (unsigned int i = 0; i < 2000; i++) { + aDevice.motorcontrolSOUT_.recompute(i); + aDevice.motorcontrolSOUT_.setReady(); + aIntegrator.stateSOUT_.setReady(); + + } + const dg::Vector & poseFF = aIntegrator.freeFlyerPositionEulerSOUT_(2001); + std::cout << "\n ########### \n " << std::endl; + std::cout << "Final freeFlyerPositionEulerSOUT_: \n" << poseFF << std::endl; + + std::cout << "\n ########### \n " << std::endl; + const dg::Vector & poseFFQ = aIntegrator.freeFlyerPositionQuatSOUT_(2001); + std::cout << "Final freeFlyerPositionQuatSOUT_: \n" << poseFFQ << std::endl; + + std::cout << "\n ########### \n " << std::endl; + const dg::sot::MatrixHomogeneous & ffposeMat = aIntegrator.freeFlyerPose(); + std::cout << "Final freeFlyerPosition MatrixHomogeneous: \n" + << ffposeMat.translation() << "\n" + << ffposeMat.linear() << std::endl; + + std::cout << "\n ########### \n " << std::endl; + std::cout << "Final integrator stateSOUT_ : \n" << aIntegrator.stateSOUT_(2001) << std::endl; + + const dg::Vector & aControl = aDevice.motorcontrolSOUT_(2001); + double diff = 0, ldiff; + + vector< ::urdf::JointSharedPtr > urdf_joints = aDevice.getURDFJoints(); + + dgsot::JointSHWControlType_iterator it_control_type; + for (it_control_type = aDevice.jointDevices_.begin(); + it_control_type != aDevice.jointDevices_.end(); + it_control_type++) { + int lctl_index = it_control_type->second.control_index; + int u_index = it_control_type->second.urdf_index; + std::cout << "\n ########### \n " << std::endl; + std::cout << "urdf_joints: " << urdf_joints[u_index]->name << std::endl; + + if (it_control_type->second.SoTcontrol == dgsot::POSITION) { + if (u_index != -1 && (urdf_joints[u_index]->limits)) { + double lowerLim = urdf_joints[u_index]->limits->lower; + ldiff = (aControl[lctl_index] - lowerLim); + diff += ldiff; + std::cout << "Position lowerLim: " << lowerLim << "\n" + << "motorcontrolSOUT: " << aControl[lctl_index] << " -- " + << "diff: " << ldiff << "\n" + << "Velocity limit: " << urdf_joints[u_index]->limits->velocity + << std::endl; + } + } else { + std::cout << "motorcontrolSOUT: " << aControl[lctl_index] << std::endl; + } + } + std::cout << "\n ########### \n " << std::endl; + std::cout << "totalDiff: " << diff << std::endl; + + BOOST_CHECK_CLOSE(diff, 0.0, 1e-3); +}