From e8b526d6a2afd546ce3470e5cb788f26cc122d0d Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 19 Aug 2019 17:53:03 +0200 Subject: [PATCH 01/10] [State-Integrator] First draft of the integrator To be plugged before the device if integration of the control is needed: Only for control in Velocity and Acceleration for now. Check the size of the signal to separate two cases: with and without a control on the freeflyer. The integration of the freeflyer works as an odometry predictive system. --- .gitignore | 1 + CMakeLists.txt | 1 + include/CMakeLists.txt | 1 + .../sot-dynamic-pinocchio/state-integrator.h | 207 ++++++++++ src/state-integrator.cpp | 384 ++++++++++++++++++ 5 files changed, 594 insertions(+) create mode 100644 include/sot-dynamic-pinocchio/state-integrator.h create mode 100644 src/state-integrator.cpp 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 7f9218b..00cac0a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,6 +55,7 @@ SET(plugins angle-estimator waist-attitude-from-sensor zmp-from-forces + state-integrator ) SET(LIBRARY_NAME ${PROJECT_NAME}) diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 02d9461..312bbae 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -11,6 +11,7 @@ SET(${PROJECT_NAME}_HEADERS matrix-inertia.h integrator-force-rk4.h angle-estimator.h + state-integrator.h ) # Recreate correct path for the headers diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h new file mode 100644 index 0000000..97078a2 --- /dev/null +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -0,0 +1,207 @@ +/* + * Copyright 2010-2018, CNRS + * Florent Lamiraux + * Olivier Stasse + * + * CNRS + * + * See LICENSE.txt + */ + +#ifndef SOT_STATEINTEGRATOR_HH +#define SOT_STATEINTEGRATOR_HH + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +/* SOT */ +/// dg +#include +#include +/// sot-core +#include "sot/core/periodic-call.hh" +#include +#include "sot/core/api.hh" +#include +/// pinocchio +#include +#include "pinocchio/multibody/model.hpp" +#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 { + +/// Specifies the nature of one joint control +enum ControlType { + VELOCITY = 0, + ACCELERATION = 1 +}; + +const std::string ControlType_s[] = { + "VELOCITY", "ACCELERATION" +}; + + + +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity +{ + public: + + 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 timeStep(double ts) { timestep_ = ts;} + + 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_; + + /// Store Position of free flyer joint + Eigen::VectorXd ffPose_; + /// Store Velocity of free flyer joint + Eigen::VectorXd ffVel_; + + /// Torque vector of each actuator. + dg::Vector torque_; + ///@} + + 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); + + /// 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 integrate( const double & dt = 5e-2 ); + + /// Read directly the URDF model + void setURDFModel(const std::string &aURDFModel); + + /// \name Sanity check parameterization + /// \{ + void setSanityCheck(const bool & enableCheck); + /// \} + + /// \name Get the control type from an int (of the controlTypeSIN signal) as in the enum + /// Check the types: velocity = 0 or acceleration = 1 + /// \{ + int getControlType(const int &ctrlType, ControlType &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 type of the control vector + /// It can be velocity or acceleration. + /// It depends on each of the actuator + dg::SignalPtr controlTypeSIN; + + + /// \name StateIntegrator current state. + /// \{ + /// \brief Output integrated state from control. + dg::Signal stateSOUT_; + /// \brief Output integrated velocity from control. + dg::Signal velocitySOUT_; + /// \brief Output integrated freeFlyer position from control (odometry predictive system). + dg::Signal freeFlyerPositionOdomSOUT_; + /// \brief Output integrated freeFlyer velocity from control. + dg::Signal freeFlyerVelocitySOUT_; + /// \} + + /// \brief Provides the itegrated control information. + void getControl(std::map &anglesOut); + ///@} + + protected: + + /// Integrate the freeflyer state (to obtain position or velocity). + /// Compute roll pitch yaw angles + /// Publish the result on the dedicated signal. + void integrateRollPitchYaw(dg::Vector& state, dg::Signal& signal, const dg::Vector& control, double dt); + + /// Store Position of free flyer joint as MatrixHomogeneous + MatrixHomogeneous freeFlyerPose_; + + /// Get freeflyer pose + const MatrixHomogeneous& freeFlyerPose(); + + public: + + 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_; + + 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/state-integrator.cpp b/src/state-integrator.cpp new file mode 100644 index 0000000..3da4464 --- /dev/null +++ b/src/state-integrator.cpp @@ -0,0 +1,384 @@ +/* + * 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 + +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 + +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" ) + , controlTypeSIN(NULL, "StateIntegrator(" + n + ")::input(double)::controlType" ) + , stateSOUT_( "StateIntegrator(" + n + ")::output(vector)::state" ) + , velocitySOUT_("StateIntegrator(" + n + ")::output(vector)::velocity" ) + , freeFlyerPositionOdomSOUT_("StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionOdom") + , freeFlyerVelocitySOUT_("StateIntegrator(" + n + ")::output(vector)::freeFlyerVelocity") + , debug_mode_(5) +{ + signalRegistration( controlSIN + << controlTypeSIN + << stateSOUT_ + << velocitySOUT_ + << freeFlyerPositionOdomSOUT_ + << freeFlyerVelocitySOUT_); + position_.fill(.0); + stateSOUT_.setConstant( position_ ); + + velocity_.resize(position_.size()); + velocity_.setZero(); + velocitySOUT_.setConstant( velocity_ ); + + ffPose_.fill(.0); + freeFlyerPositionOdomSOUT_.setConstant( ffPose_ ); + + ffVel_.resize(ffPose_.size()); + ffVel_.setZero(); + freeFlyerVelocitySOUT_.setConstant( ffVel_ ); + + /* --- Commands --- */ + { + std::string docstring; + docstring = + "\n" + " Set state vector value\n" + "\n"; + addCommand("set", + new command::Setter(*this, &StateIntegrator::setState, docstring)); + + docstring = + "\n" + " Set velocity vector value\n" + "\n"; + addCommand("setVelocity", + new command::Setter(*this, &StateIntegrator::setVelocity, 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::integrateRollPitchYaw(Vector& state, Signal& signal, const Vector& control, double dt) +{ + using Eigen::AngleAxisd; + using Eigen::Vector3d; + 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); + + // Update freeflyer pose/vel and signal + state.head<3>() = qout.head<3>(); + state.segment<3>(3) = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix().eulerAngles(0, 1, 2); + signal.setConstant(state); +} + +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.normalized().toRotationMatrix(); + + return freeFlyerPose_; +} + +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 if (st.size() == model_.nq+6) + { + position_ = st.tail(model_.nq); + ffPose_ = st.head<6>(); + freeFlyerPositionOdomSOUT_.setConstant( ffPose_); + } + stateSOUT_ .setConstant( position_ ); +} + +void StateIntegrator::setVelocity( const Vector& vel ) +{ + if (vel.size() == model_.nv) + { + velocity_ = vel; + } + else if (vel.size() == model_.nv+6) + { + velocity_ = vel.tail(model_.nv); + ffVel_ = vel.head<6>(); + freeFlyerVelocitySOUT_.setConstant( ffVel_ ); + } + velocitySOUT_ .setConstant( velocity_ ); +} + +void StateIntegrator::setRoot( const Matrix & root ) +{ + Eigen::Matrix4d _matrix4d(root); + MatrixHomogeneous _root(_matrix4d); + setRoot( _root ); +} + +void StateIntegrator::setRoot( const MatrixHomogeneous & worldMwaist ) +{ + freeFlyerPose_ = worldMwaist; + ffPose_.head<3>() = worldMwaist.translation(); + ffPose_.segment<3>(3) = worldMwaist.linear().eulerAngles(0,1,2); + freeFlyerPositionOdomSOUT_.setConstant( ffPose_); +} + +void StateIntegrator::setSanityCheck(const bool & enableCheck) +{ + sanityCheck_ = enableCheck; +} + +int StateIntegrator::getControlType(const int &ctrlType, ControlType &aCtrlType) +{ + for (int j = 0; j < 2; j++) + { + if (ctrlType == j) + { + aCtrlType = (ControlType)j; + return 0; + } + } + if (debug_mode_ > 1) + { + std::cerr << "Control type not allowed/recognized: " + << ctrlType + << "\n Authorized control types: " + << "VELOCITY = 0 | ACCELERATION = 1" + << std::endl; + } + return 1; +} + +void StateIntegrator::integrate( const double & dt ) +{ + const Vector & controlIN = controlSIN.accessCopy(); + const Vector & controlTypeIN = controlTypeSIN.accessCopy(); + + if (controlIN.size() != controlTypeIN.size()) + { + if (debug_mode_ > 1) + { + std::cerr << "Signals controlSIN and controlTypeSIN must have the same size: " + << "\n controlIN.size(): " << controlIN.size() + << "\n controlTypeIN.size(): " << controlTypeIN.size() + << std::endl; + } + return; + } + + Vector controlFreeFlyer(6), controlFreeFlyerType(6); + Vector control(model_.nv), controlType(model_.nv); + bool hasFreeFlyer = false; + + if (sanityCheck_ && controlIN.hasNaN()) + { + dgRTLOG () << "StateIntegrator::integrate: Control has NaN values: " << '\n' + << controlIN.transpose() << '\n'; + return; + } + // Check size to separate freeFlyer + if (controlIN.size() == model_.nv+6) + { + controlFreeFlyer = controlIN.head<6>(); + control = controlIN.tail(model_.nv); + controlFreeFlyerType = controlTypeIN.head<6>(); + controlType = controlTypeIN.tail(model_.nv); + hasFreeFlyer = true; + } + else + { + control = controlIN; + controlType = controlTypeIN; + } + + // Integration of the joints + for (int i=0;i 0) + { + if (debug_mode_ > 1) + { + std::cerr << "No control type for joint at position " << i + << " in the controlSIN vector" + << std::endl; + } + break; + } + // Set acceleration from control and integrates to find velocity. + if (type == ACCELERATION) + { + acceleration_[i] = control[i]; + velocity_[i] = velocity_[i] + acceleration_[i] * (0.5)*dt; + } + // Set velocity from control. + else if (type == VELOCITY) + { + acceleration_[i] = 0; + velocity_[i] = control[i]; + } + // Velocity integration to get position + position_[i] = position_[i] + velocity_[i] * dt; + } + + stateSOUT_ .setConstant( position_ ); + velocitySOUT_ .setConstant( velocity_ ); + + // Freeflyer integration + if (hasFreeFlyer) + { + ControlType ffType; + if (getControlType(int(controlFreeFlyerType[0]), ffType) > 0) + { + if (debug_mode_ > 1) + { + std::cerr << "No valid control type for freeflyer in the controlTypeSIN signal" + << std::endl; + } + return; + } + if (ffType == ACCELERATION) + { + // Integrate once to obtain velocity -> update ffVel_ and signal freeFlyerVelocitySOUT_ + integrateRollPitchYaw(ffVel_, freeFlyerVelocitySOUT_, controlFreeFlyer, dt); + } + else if (ffType == VELOCITY) + { + // Set ffVel_ from control for the integration in position and update freeFlyerVelocitySOUT_ + ffVel_ = controlFreeFlyer; + freeFlyerVelocitySOUT_.setConstant(ffVel_); + } + // Integrate to obtain position -> update ffPose_ and signal freeFlyerPositionOdomSOUT_ + integrateRollPitchYaw(ffPose_, freeFlyerPositionOdomSOUT_, ffVel_, dt); + } +} + + +/* --- DISPLAY ------------------------------------------------------------ */ + +void StateIntegrator::display ( std::ostream& os ) const +{ + os << name << ": " << position_ << endl; +} + + +void StateIntegrator::getControl(map &controlOut) +{ + ODEBUG5FULL("start"); + sotDEBUGIN(25) ; + + // Integrate control + integrate(timestep_); + sotDEBUG (25) << "position_ = " << position_ << std::endl; + + ODEBUG5FULL("position_ = " << position_); + + vector lcontrolOut; + lcontrolOut.resize(position_.size()); + Eigen::VectorXd::Map(&lcontrolOut[0], position_.size()) = position_; + + controlOut["control"].setValues(lcontrolOut); + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + From 40053dc6c541389c498ebfd73dd6e56274e27f94 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 20 Aug 2019 14:42:58 +0200 Subject: [PATCH 02/10] [Device] Change name of the control type enum to use it with device --- .../sot-dynamic-pinocchio/state-integrator.h | 22 ++++++++----------- src/state-integrator.cpp | 16 +++++++------- 2 files changed, 17 insertions(+), 21 deletions(-) diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h index 97078a2..0294399 100644 --- a/include/sot-dynamic-pinocchio/state-integrator.h +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -51,13 +51,13 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -/// Specifies the nature of one joint control -enum ControlType { - VELOCITY = 0, - ACCELERATION = 1 +/// Specifies the nature of one joint control from SoT side +enum SoTControlType { + VEL = 0, + ACC = 1 }; -const std::string ControlType_s[] = { +const std::string SoTControlType_s[] = { "VELOCITY", "ACCELERATION" }; @@ -95,9 +95,6 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity Eigen::VectorXd ffPose_; /// Store Velocity of free flyer joint Eigen::VectorXd ffVel_; - - /// Torque vector of each actuator. - dg::Vector torque_; ///@} bool sanityCheck_; @@ -131,7 +128,7 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// \name Get the control type from an int (of the controlTypeSIN signal) as in the enum /// Check the types: velocity = 0 or acceleration = 1 /// \{ - int getControlType(const int &ctrlType, ControlType &aCtrlType); + int getControlType(const int &ctrlType, SoTControlType &aCtrlType); /// \} public: /* --- DISPLAY --- */ @@ -150,7 +147,6 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// It depends on each of the actuator dg::SignalPtr controlTypeSIN; - /// \name StateIntegrator current state. /// \{ /// \brief Output integrated state from control. @@ -177,11 +173,11 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// Store Position of free flyer joint as MatrixHomogeneous MatrixHomogeneous freeFlyerPose_; - /// Get freeflyer pose - const MatrixHomogeneous& freeFlyerPose(); - public: + public: + /// Get freeflyer pose + const MatrixHomogeneous& freeFlyerPose(); virtual void setRoot( const dg::Matrix & root ); virtual void setRoot( const MatrixHomogeneous & worldMwaist ); diff --git a/src/state-integrator.cpp b/src/state-integrator.cpp index 3da4464..cb57863 100644 --- a/src/state-integrator.cpp +++ b/src/state-integrator.cpp @@ -227,13 +227,13 @@ void StateIntegrator::setSanityCheck(const bool & enableCheck) sanityCheck_ = enableCheck; } -int StateIntegrator::getControlType(const int &ctrlType, ControlType &aCtrlType) +int StateIntegrator::getControlType(const int &ctrlType, SoTControlType &aCtrlType) { for (int j = 0; j < 2; j++) { if (ctrlType == j) { - aCtrlType = (ControlType)j; + aCtrlType = (SoTControlType)j; return 0; } } @@ -293,7 +293,7 @@ void StateIntegrator::integrate( const double & dt ) // Integration of the joints for (int i=0;i 0) { if (debug_mode_ > 1) @@ -305,13 +305,13 @@ void StateIntegrator::integrate( const double & dt ) break; } // Set acceleration from control and integrates to find velocity. - if (type == ACCELERATION) + if (type == ACC) { acceleration_[i] = control[i]; velocity_[i] = velocity_[i] + acceleration_[i] * (0.5)*dt; } // Set velocity from control. - else if (type == VELOCITY) + else if (type == VEL) { acceleration_[i] = 0; velocity_[i] = control[i]; @@ -326,7 +326,7 @@ void StateIntegrator::integrate( const double & dt ) // Freeflyer integration if (hasFreeFlyer) { - ControlType ffType; + SoTControlType ffType; if (getControlType(int(controlFreeFlyerType[0]), ffType) > 0) { if (debug_mode_ > 1) @@ -336,12 +336,12 @@ void StateIntegrator::integrate( const double & dt ) } return; } - if (ffType == ACCELERATION) + if (ffType == ACC) { // Integrate once to obtain velocity -> update ffVel_ and signal freeFlyerVelocitySOUT_ integrateRollPitchYaw(ffVel_, freeFlyerVelocitySOUT_, controlFreeFlyer, dt); } - else if (ffType == VELOCITY) + else if (ffType == VEL) { // Set ffVel_ from control for the integration in position and update freeFlyerVelocitySOUT_ ffVel_ = controlFreeFlyer; From 88f130ecf64a88e9a803aedc597f1955b1b5a4ee Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 20 Aug 2019 14:53:54 +0200 Subject: [PATCH 03/10] [Device] Add a unit test for state integrator Use device.cpp from sot-core. Add yaml files to create the device for the test. Set the input control of the integrator to -0.5 m/s (velocity) for every joints. Plug the output stateSOUT_ of the integrator to the input control of the device. The control type of the joints on the hardware side (in device) is in position. At the end of the loop the output of the device (motor control) correspond to the limits of the joints in position (as expected, the velocity control push the joints to their limits). In the test a freeflyer is given and integrated as an odometry system. --- unitTesting/CMakeLists.txt | 2 + unitTesting/sot_controller.yaml | 37 ++++++ unitTesting/sot_params.yaml | 9 ++ unitTesting/test_state_integrator.cpp | 160 ++++++++++++++++++++++++++ 4 files changed, 208 insertions(+) create mode 100644 unitTesting/sot_controller.yaml create mode 100644 unitTesting/sot_params.yaml create mode 100644 unitTesting/test_state_integrator.cpp diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt index 0ca9d4f..93e7283 100644 --- a/unitTesting/CMakeLists.txt +++ b/unitTesting/CMakeLists.txt @@ -4,6 +4,7 @@ ADD_DEFINITIONS(-DDEBUG=2) SET(tests test_constructor +test_state_integrator #test_config # dummy # test_djj @@ -49,6 +50,7 @@ FOREACH(test ${tests}) dp-integrator-force dp-angle-estimator dp-waist-attitude-from-sensor + dp-state-integrator ${LIBRARY_NAME} ) diff --git a/unitTesting/sot_controller.yaml b/unitTesting/sot_controller.yaml new file mode 100644 index 0000000..d5e68db --- /dev/null +++ b/unitTesting/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/unitTesting/sot_params.yaml b/unitTesting/sot_params.yaml new file mode 100644 index 0000000..91cd35b --- /dev/null +++ b/unitTesting/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/unitTesting/test_state_integrator.cpp b/unitTesting/test_state_integrator.cpp new file mode 100644 index 0000000..8c5de66 --- /dev/null +++ b/unitTesting/test_state_integrator.cpp @@ -0,0 +1,160 @@ +/* + * Copyright 2018, + * Olivier Stasse, + * + * CNRS + * See LICENSE.txt + * + */ + +#include +#include + +#ifndef WIN32 +#include +#endif + +using namespace std; + +#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::Device &aDevice) { + // Reflect how the data are splitted in two yaml files in the sot + std::ifstream yaml_file_controller("../../unitTesting/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("../../unitTesting/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; +} + + +int main(int, char **) { + + unsigned int debug_mode = 2; + + std::string robot_description; + ifstream urdfFile; + std::string filename = "/opt/openrobots/share/simple_humanoid_description/urdf/simple_humanoid.urdf"; + urdfFile.open(filename.c_str()); + if (!urdfFile.is_open()) { + std::cerr << "Unable to open " << filename << std::endl; + return -1; + } + stringstream strStream; + strStream << urdfFile.rdbuf(); + robot_description = strStream.str(); + + /// Test reading the URDF file. + dg::sot::Device aDevice(std::string("simple_humanoid")); + aDevice.setDebugMode(debug_mode); + aDevice.setURDFModel(robot_description); + + if (ReadYAMLFILE(aDevice) < 0) + return -1; + + dg::sot::StateIntegrator aIntegrator(std::string("integrator")); + aIntegrator.setURDFModel(robot_description); + + dg::Vector aState(29); // without freeFlyer + for(unsigned j=0;j controlOut; + aDevice.getControl(controlOut); + double diff = 0, diffCont = 0, ldiff, ldiffCont; + + 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); + ldiffCont = controlOut["control"].getValues()[lctl_index] - lowerLim; + diff += ldiff; + diffCont += ldiffCont; + std::cout << "Position lowerLim: " << lowerLim << "\n" + << "motorcontrolSOUT: " << aControl[lctl_index] << " -- " + << "diff: " << ldiff << "\n" + << "controlOut: " << controlOut["control"].getValues()[lctl_index] << " -- " + << "diff: " << ldiffCont << " \n" + << "Velocity limit: " << urdf_joints[u_index]->limits->velocity + << std::endl; + } + } + else + { + std::cout << "motorcontrolSOUT: " << aControl[lctl_index]<< "\n" + << "controlOut: " << controlOut["control"].getValues()[lctl_index] << std::endl; + } + } + std::cout << "\n ########### \n " << std::endl; + std::cout << "totalDiff: " << diff << std::endl; + std::cout << "totalDiffCont: " << diffCont << std::endl; + + return 0; +} From 7d120b4380a961336ebabfccff6825bcd96aae01 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 23 Aug 2019 17:45:11 +0200 Subject: [PATCH 04/10] [Device] Adding boost::bind to the signals SOUT_ - remove controlTypeSIN Replace signal controlTypeSIN by setter : with strings or ints (for addcommand) --- .../sot-dynamic-pinocchio/state-integrator.h | 74 +++-- src/state-integrator.cpp | 296 ++++++++++++------ unitTesting/test_state_integrator.cpp | 54 ++-- 3 files changed, 268 insertions(+), 156 deletions(-) diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h index 0294399..2db803e 100644 --- a/include/sot-dynamic-pinocchio/state-integrator.h +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -15,6 +15,7 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include /* SOT */ /// dg #include @@ -51,14 +52,18 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { +typedef Eigen::Matrix StringVector; + /// Specifies the nature of one joint control from SoT side enum SoTControlType { - VEL = 0, - ACC = 1 + qVEL = 0, + qACC = 1, + ffVEL = 2, + ffACC = 3 }; const std::string SoTControlType_s[] = { - "VELOCITY", "ACCELERATION" + "qVEL", "qACC", "ffVEL", "ffACC" }; @@ -76,7 +81,7 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity static const double TIMESTEP_DEFAULT; /// Set integration time. - void timeStep(double ts) { timestep_ = ts;} + void init(const double& step); protected: /// \brief Current integration step. @@ -91,6 +96,10 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// Acceleration vector of each actuator. dg::Vector acceleration_; + /// Type of the control vector + /// It can be velocity or acceleration for the actuators or the Freeflyer. + StringVector controlTypeVector_; + /// Store Position of free flyer joint Eigen::VectorXd ffPose_; /// Store Velocity of free flyer joint @@ -110,13 +119,6 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity void setVelocitySize(const unsigned int& size); virtual void setVelocity(const dg::Vector & vel); - /// 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 integrate( const double & dt = 5e-2 ); - /// Read directly the URDF model void setURDFModel(const std::string &aURDFModel); @@ -125,12 +127,20 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity void setSanityCheck(const bool & enableCheck); /// \} - /// \name Get the control type from an int (of the controlTypeSIN signal) as in the enum - /// Check the types: velocity = 0 or acceleration = 1 + /// \name Set the control types of the controlled joints/freeflyer + /// Allowed types (string): qVEL | qACC | ffVEL | ffACC + void setControlType(const StringVector& controlTypeVector); + + /// \name Set the control types of the controlled joints/freeflyer + /// Allowed types (int): qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 + 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 int &ctrlType, SoTControlType &aCtrlType); + 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) { @@ -142,38 +152,44 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// 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 type of the control vector - /// It can be velocity or acceleration. - /// It depends on each of the actuator - dg::SignalPtr controlTypeSIN; /// \name StateIntegrator current state. /// \{ /// \brief Output integrated state from control. - dg::Signal stateSOUT_; + dg::SignalTimeDependent stateSOUT_; /// \brief Output integrated velocity from control. - dg::Signal velocitySOUT_; + dg::SignalTimeDependent velocitySOUT_; /// \brief Output integrated freeFlyer position from control (odometry predictive system). - dg::Signal freeFlyerPositionOdomSOUT_; + dg::SignalTimeDependent freeFlyerPositionOdomSOUT_; /// \brief Output integrated freeFlyer velocity from control. - dg::Signal freeFlyerVelocitySOUT_; + dg::SignalTimeDependent freeFlyerVelocitySOUT_; /// \} - - /// \brief Provides the itegrated control information. - void getControl(std::map &anglesOut); ///@} protected: /// Integrate the freeflyer state (to obtain position or velocity). /// Compute roll pitch yaw angles - /// Publish the result on the dedicated signal. - void integrateRollPitchYaw(dg::Vector& state, dg::Signal& signal, const dg::Vector& control, double dt); + void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control, double dt); /// 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 integrate(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 (callback signal freeFlyerPositionOdomSOUT_). + dg::Vector& getFreeFlyerPosition(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 @@ -187,6 +203,8 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity pinocchio::Model model_; // Debug mode int debug_mode_; + // Last integration iteration + int last_integration_; public: diff --git a/src/state-integrator.cpp b/src/state-integrator.cpp index cb57863..14b32e0 100644 --- a/src/state-integrator.cpp +++ b/src/state-integrator.cpp @@ -53,7 +53,8 @@ using namespace dynamicgraph; #define ODEBUG5(x) #endif -const std::string StateIntegrator::CLASS_NAME = "StateIntegrator"; +DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StateIntegrator, "StateIntegrator"); +//const std::string StateIntegrator::CLASS_NAME = "StateIntegrator"; const double StateIntegrator::TIMESTEP_DEFAULT = 0.001; /* --------------------------------------------------------------------- */ @@ -67,36 +68,47 @@ StateIntegrator::StateIntegrator( const std::string& n ) , ffPose_(6) , sanityCheck_(true) , controlSIN( NULL, "StateIntegrator(" + n + ")::input(double)::control" ) - , controlTypeSIN(NULL, "StateIntegrator(" + n + ")::input(double)::controlType" ) - , stateSOUT_( "StateIntegrator(" + n + ")::output(vector)::state" ) - , velocitySOUT_("StateIntegrator(" + n + ")::output(vector)::velocity" ) - , freeFlyerPositionOdomSOUT_("StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionOdom") - , freeFlyerVelocitySOUT_("StateIntegrator(" + n + ")::output(vector)::freeFlyerVelocity") + , 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" ) + , freeFlyerPositionOdomSOUT_(boost::bind(&StateIntegrator::getFreeFlyerPosition,this,_1,_2), + controlSIN, + "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionOdom") + , freeFlyerVelocitySOUT_(boost::bind(&StateIntegrator::getFreeFlyerVelocity,this,_1,_2), + controlSIN, + "StateIntegrator(" + n + ")::output(vector)::freeFlyerVelocity") , debug_mode_(5) + , last_integration_(-1) { signalRegistration( controlSIN - << controlTypeSIN << stateSOUT_ << velocitySOUT_ << freeFlyerPositionOdomSOUT_ << freeFlyerVelocitySOUT_); position_.fill(.0); - stateSOUT_.setConstant( position_ ); velocity_.resize(position_.size()); velocity_.setZero(); - velocitySOUT_.setConstant( velocity_ ); ffPose_.fill(.0); - freeFlyerPositionOdomSOUT_.setConstant( ffPose_ ); ffVel_.resize(ffPose_.size()); ffVel_.setZero(); - freeFlyerVelocitySOUT_.setConstant( ffVel_ ); /* --- 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" @@ -110,6 +122,14 @@ StateIntegrator::StateIntegrator( const std::string& n ) "\n"; addCommand("setVelocity", new command::Setter(*this, &StateIntegrator::setVelocity, docstring)); + + docstring = + "\n" + " Set control type vector value (for each joint).\n" + " Vector of types (int): qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3\n" + "\n"; + addCommand("setControlType", + new command::Setter(*this, &StateIntegrator::setControlTypeInt, docstring)); void(StateIntegrator::*setRootPtr)(const Matrix&) = &StateIntegrator::setRoot; docstring = command::docCommandVoid1("Set the root position.", @@ -130,7 +150,11 @@ StateIntegrator::~StateIntegrator( ) { return; } -void StateIntegrator::integrateRollPitchYaw(Vector& state, Signal& signal, const Vector& control, double dt) +void StateIntegrator::init(const double& step){ + timestep_ = step; +} + +void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control, double dt) { using Eigen::AngleAxisd; using Eigen::Vector3d; @@ -147,10 +171,9 @@ void StateIntegrator::integrateRollPitchYaw(Vector& state, Signal& SE3().integrate (qin, control*dt, qout); - // Update freeflyer pose/vel and signal + // Update freeflyer state (pose|vel) state.head<3>() = qout.head<3>(); state.segment<3>(3) = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix().eulerAngles(0, 1, 2); - signal.setConstant(state); } const MatrixHomogeneous& StateIntegrator::freeFlyerPose() @@ -169,6 +192,32 @@ const MatrixHomogeneous& StateIntegrator::freeFlyerPose() return freeFlyerPose_; } +void StateIntegrator::setControlType(const StringVector& controlTypeVector) +{ + controlTypeVector_ = controlTypeVector; +} + +void StateIntegrator::setControlTypeInt(const Vector& controlTypeVector) +{ + std::string type; + controlTypeVector_.resize(controlTypeVector.size()); + + for (unsigned int i=0; i(); - freeFlyerPositionOdomSOUT_.setConstant( ffPose_); + int size = int(st.size() - model_.nq); + ffPose_ = st.head(size); } - stateSOUT_ .setConstant( position_ ); } void StateIntegrator::setVelocity( const Vector& vel ) @@ -198,13 +246,12 @@ void StateIntegrator::setVelocity( const Vector& vel ) { velocity_ = vel; } - else if (vel.size() == model_.nv+6) + else { velocity_ = vel.tail(model_.nv); - ffVel_ = vel.head<6>(); - freeFlyerVelocitySOUT_.setConstant( ffVel_ ); + int size = int(vel.size() - model_.nq); + ffVel_ = vel.head(size); } - velocitySOUT_ .setConstant( velocity_ ); } void StateIntegrator::setRoot( const Matrix & root ) @@ -219,7 +266,6 @@ void StateIntegrator::setRoot( const MatrixHomogeneous & worldMwaist ) freeFlyerPose_ = worldMwaist; ffPose_.head<3>() = worldMwaist.translation(); ffPose_.segment<3>(3) = worldMwaist.linear().eulerAngles(0,1,2); - freeFlyerPositionOdomSOUT_.setConstant( ffPose_); } void StateIntegrator::setSanityCheck(const bool & enableCheck) @@ -227,11 +273,11 @@ void StateIntegrator::setSanityCheck(const bool & enableCheck) sanityCheck_ = enableCheck; } -int StateIntegrator::getControlType(const int &ctrlType, SoTControlType &aCtrlType) +int StateIntegrator::getControlType(const std::string &strCtrlType, SoTControlType &aCtrlType) { - for (int j = 0; j < 2; j++) + for (int j = 0; j < 4; j++) { - if (ctrlType == j) + if (strCtrlType==SoTControlType_s[j]) { aCtrlType = (SoTControlType)j; return 0; @@ -240,116 +286,92 @@ int StateIntegrator::getControlType(const int &ctrlType, SoTControlType &aCtrlTy if (debug_mode_ > 1) { std::cerr << "Control type not allowed/recognized: " - << ctrlType + << strCtrlType << "\n Authorized control types: " - << "VELOCITY = 0 | ACCELERATION = 1" + << "qVEL | qACC | ffVEL | ffACC" << std::endl; } return 1; } -void StateIntegrator::integrate( const double & dt ) +void StateIntegrator::integrate(int t, const double & dt) { + controlSIN(t); const Vector & controlIN = controlSIN.accessCopy(); - const Vector & controlTypeIN = controlTypeSIN.accessCopy(); - if (controlIN.size() != controlTypeIN.size()) - { - if (debug_mode_ > 1) - { - std::cerr << "Signals controlSIN and controlTypeSIN must have the same size: " - << "\n controlIN.size(): " << controlIN.size() - << "\n controlTypeIN.size(): " << controlTypeIN.size() - << std::endl; - } + if (controlTypeVector_.size() == 0){ + dgRTLOG () << "StateIntegrator::integrate: The controlType vector cannot be empty" << '\n'; return; } - Vector controlFreeFlyer(6), controlFreeFlyerType(6); - Vector control(model_.nv), controlType(model_.nv); - bool hasFreeFlyer = false; - if (sanityCheck_ && controlIN.hasNaN()) { dgRTLOG () << "StateIntegrator::integrate: Control has NaN values: " << '\n' << controlIN.transpose() << '\n'; return; } - // Check size to separate freeFlyer - if (controlIN.size() == model_.nv+6) - { - controlFreeFlyer = controlIN.head<6>(); - control = controlIN.tail(model_.nv); - controlFreeFlyerType = controlTypeIN.head<6>(); - controlType = controlTypeIN.tail(model_.nv); - hasFreeFlyer = true; - } - else - { - control = controlIN; - controlType = controlTypeIN; - } - // Integration of the joints - for (int i=0;i 0) + if (getControlType(controlTypeVector_[indexTypeVector], type) > 0) { if (debug_mode_ > 1) { - std::cerr << "No control type for joint at position " << i - << " in the controlSIN vector" + std::cerr << "No control type for joint at position " << indexTypeVector + << " in the controlType vector" << std::endl; } break; } - // Set acceleration from control and integrates to find velocity. - if (type == ACC) + + // Control of the freeflyer in acceleration + if (type == ffACC) { - acceleration_[i] = control[i]; - velocity_[i] = velocity_[i] + acceleration_[i] * (0.5)*dt; + // Set controlFreeFlyer from control (size 6 because derivative of twist) + Vector controlFreeFlyer = controlIN.segment<6>(indexControlVector); + // Integrate once to obtain velocity -> update ffVel_ + integrateRollPitchYaw(ffVel_, controlFreeFlyer, dt); + indexControlVector = indexControlVector + 6; } - // Set velocity from control. - else if (type == VEL) + // Control of the freeflyer in velocity + else if (type == ffVEL) { - acceleration_[i] = 0; - velocity_[i] = control[i]; + // Set ffVel_ (twist) from control for the integration in position + ffVel_ = controlIN.segment<6>(indexControlVector); + indexControlVector = indexControlVector + 6; } - // Velocity integration to get position - position_[i] = position_[i] + velocity_[i] * dt; - } - - stateSOUT_ .setConstant( position_ ); - velocitySOUT_ .setConstant( velocity_ ); - // Freeflyer integration - if (hasFreeFlyer) - { - SoTControlType ffType; - if (getControlType(int(controlFreeFlyerType[0]), ffType) > 0) + // Control of a joint in acceleration + else if (type == qACC) { - if (debug_mode_ > 1) - { - std::cerr << "No valid control type for freeflyer in the controlTypeSIN signal" - << std::endl; - } - return; + // Set acceleration from control and integrate to find velocity. + acceleration_[indexComputedVector] = controlIN[indexControlVector]; + velocity_[indexComputedVector] = velocity_[indexComputedVector] + acceleration_[indexComputedVector] * (0.5)*dt; + indexControlVector++; } - if (ffType == ACC) + // Control of a joint in velocity + else if (type == qVEL) { - // Integrate once to obtain velocity -> update ffVel_ and signal freeFlyerVelocitySOUT_ - integrateRollPitchYaw(ffVel_, freeFlyerVelocitySOUT_, controlFreeFlyer, dt); + // Set velocity from control. + acceleration_[indexComputedVector] = 0; + velocity_[indexComputedVector] = controlIN[indexControlVector]; + indexControlVector++; } - else if (ffType == VEL) - { - // Set ffVel_ from control for the integration in position and update freeFlyerVelocitySOUT_ - ffVel_ = controlFreeFlyer; - freeFlyerVelocitySOUT_.setConstant(ffVel_); + + // Velocity integration + if (type == ffVEL || type == ffACC){ + // Integrate freeflyer velocity to obtain position -> update ffPose_ from ffVel_ + integrateRollPitchYaw(ffPose_, ffVel_, dt); } - // Integrate to obtain position -> update ffPose_ and signal freeFlyerPositionOdomSOUT_ - integrateRollPitchYaw(ffPose_, freeFlyerPositionOdomSOUT_, ffVel_, dt); - } + else if (type == qVEL || type == qACC){ + // Velocity integration of the joint to get position + position_[indexComputedVector] = position_[indexComputedVector] + velocity_[indexComputedVector] * dt; + indexComputedVector++; + } + } } @@ -361,24 +383,90 @@ void StateIntegrator::display ( std::ostream& os ) const } -void StateIntegrator::getControl(map &controlOut) +Vector& StateIntegrator::getPosition(Vector &controlOut, const int& t) { ODEBUG5FULL("start"); sotDEBUGIN(25) ; // Integrate control - integrate(timestep_); + if (last_integration_ != t){ + integrate(t, timestep_); + last_integration_ = t; + } sotDEBUG (25) << "position_ = " << position_ << std::endl; ODEBUG5FULL("position_ = " << position_); - vector lcontrolOut; - lcontrolOut.resize(position_.size()); - Eigen::VectorXd::Map(&lcontrolOut[0], position_.size()) = position_; + controlOut = position_; - controlOut["control"].setValues(lcontrolOut); + return controlOut; ODEBUG5FULL("end"); sotDEBUGOUT(25) ; } +Vector& StateIntegrator::getVelocity(Vector &controlOut, const int& t) +{ + ODEBUG5FULL("start"); + sotDEBUGIN(25) ; + + // Integrate control + if (last_integration_ != t){ + integrate(t, timestep_); + last_integration_ = t; + } + sotDEBUG (25) << "velocity_ = " << velocity_ << std::endl; + + ODEBUG5FULL("velocity_ = " << velocity_); + + controlOut = velocity_; + + return controlOut; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + +Vector& StateIntegrator::getFreeFlyerPosition(Vector &ffPose, const int& t) +{ + ODEBUG5FULL("start"); + sotDEBUGIN(25); + + // Integrate control + if (last_integration_ != t){ + integrate(t, timestep_); + last_integration_ = t; + } + sotDEBUG (25) << "ffPose_ = " << ffPose_ << std::endl; + + ODEBUG5FULL("ffPose_ = " << ffPose_); + + 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_ != t){ + integrate(t, timestep_); + last_integration_ = 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/unitTesting/test_state_integrator.cpp b/unitTesting/test_state_integrator.cpp index 8c5de66..6cfbc66 100644 --- a/unitTesting/test_state_integrator.cpp +++ b/unitTesting/test_state_integrator.cpp @@ -71,6 +71,7 @@ int main(int, char **) { return -1; dg::sot::StateIntegrator aIntegrator(std::string("integrator")); + aIntegrator.init(0.005); aIntegrator.setURDFModel(robot_description); dg::Vector aState(29); // without freeFlyer @@ -78,24 +79,40 @@ int main(int, char **) { aState(j) = 0.0; aIntegrator.setState(aState); - /// Fix constant vector for the control entry and type - dg::Vector aControlVector(35); // with freeFlyer - dg::Vector aControlTypeVector(35); // with freeFlyer - double dt = 0.005; + /// Fix constant vector for the control entry + dg::Vector aControlVector(35); // with freeFlyer for (unsigned int i = 0; i < 35; i++) { - aControlVector[i] = -0.5; - aControlTypeVector[i] = 0; //velocity + aControlVector[i] = -0.5; // in velocity } + + // Set the type vector defining the type of control for each joint and the freeflyer + // With strings + // Eigen::Matrix aControlTypeVector; + // aControlTypeVector[0] = "ffVEL"; // with freeFlyer in velocity + // for (unsigned int i = 1; i < 30; i++) + // { + // aControlTypeVector[i] = "qVEL"; //velocity + // } + // With int -> for addCommand + dg::Vector aControlTypeVector(30); + // Types in int qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 + aControlTypeVector[0] = 2.0; // with freeFlyer in velocity + for (unsigned int i = 1; i < 30; i++) + { + aControlTypeVector[i] = 0.0; //velocity + } + aIntegrator.controlSIN.setConstant(aControlVector); - aIntegrator.controlTypeSIN.setConstant(aControlTypeVector); + // aIntegrator.setControlType(aControlTypeVector); + aIntegrator.setControlTypeInt(aControlTypeVector); - std::cout << "Size of the integrator stateSOUT_ control vector: " << aIntegrator.stateSOUT_(0).size() << std::endl; aDevice.controlSIN.plug(&aIntegrator.stateSOUT_); for (unsigned int i = 0; i < 2000; i++) - { - aIntegrator.integrate(dt); - aDevice.increment(); + { + aDevice.motorcontrolSOUT_.recompute(i); + aDevice.motorcontrolSOUT_.setReady(); + aIntegrator.stateSOUT_.setReady(); } const dg::Vector & poseFF = aIntegrator.freeFlyerPositionOdomSOUT_(2001); std::cout << "\n ########### \n " << std::endl; @@ -108,13 +125,8 @@ int main(int, char **) { std::cout << "Final integrator stateSOUT_ : " << aIntegrator.stateSOUT_(2001) << std::endl; - - const urdf::ModelInterfaceSharedPtr aModel = aDevice.getModel(); - const dg::Vector & aControl = aDevice.motorcontrolSOUT_(2001); - map controlOut; - aDevice.getControl(controlOut); - double diff = 0, diffCont = 0, ldiff, ldiffCont; + double diff = 0, ldiff; vector< ::urdf::JointSharedPtr > urdf_joints = aDevice.getURDFJoints(); @@ -134,27 +146,21 @@ int main(int, char **) { { double lowerLim = urdf_joints[u_index]->limits->lower; ldiff = (aControl[lctl_index] - lowerLim); - ldiffCont = controlOut["control"].getValues()[lctl_index] - lowerLim; diff += ldiff; - diffCont += ldiffCont; std::cout << "Position lowerLim: " << lowerLim << "\n" << "motorcontrolSOUT: " << aControl[lctl_index] << " -- " << "diff: " << ldiff << "\n" - << "controlOut: " << controlOut["control"].getValues()[lctl_index] << " -- " - << "diff: " << ldiffCont << " \n" << "Velocity limit: " << urdf_joints[u_index]->limits->velocity << std::endl; } } else { - std::cout << "motorcontrolSOUT: " << aControl[lctl_index]<< "\n" - << "controlOut: " << controlOut["control"].getValues()[lctl_index] << std::endl; + std::cout << "motorcontrolSOUT: " << aControl[lctl_index] << std::endl; } } std::cout << "\n ########### \n " << std::endl; std::cout << "totalDiff: " << diff << std::endl; - std::cout << "totalDiffCont: " << diffCont << std::endl; return 0; } From 76f1f651b5e7156ab23fc9b25853539867c95b9b Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 3 Sep 2019 10:50:47 +0200 Subject: [PATCH 05/10] [State-integrator] Separate integrate function in 2: for joints or freeflyer Add entry signal for freeflyer (freeFlyerSIN), add a variable to define controlType of the Freeflyer (controlTypeFF_) with setter. Add two output signals for freeflyer one for Euler angles (freeFlyerPositionEulerSOUT_) and the other for quaternions (freeFlyerPositionQuatSOUT_). Change the computation of the Euler angles to assure their range to be [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]. --- .../sot-dynamic-pinocchio/state-integrator.h | 43 ++- src/state-integrator.cpp | 364 ++++++++++-------- unitTesting/test_state_integrator.cpp | 69 ++-- 3 files changed, 289 insertions(+), 187 deletions(-) diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h index 2db803e..0b5dc17 100644 --- a/include/sot-dynamic-pinocchio/state-integrator.h +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -16,6 +16,7 @@ /* --------------------------------------------------------------------- */ #include +#include /* SOT */ /// dg #include @@ -97,8 +98,10 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity dg::Vector acceleration_; /// Type of the control vector - /// It can be velocity or acceleration for the actuators or the Freeflyer. + /// It can be velocity or acceleration for the actuators. StringVector controlTypeVector_; + /// Type of the control for the Freeflyer (velocity/acceleration). + std::string controlTypeFF_; /// Store Position of free flyer joint Eigen::VectorXd ffPose_; @@ -119,6 +122,9 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity 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); @@ -127,10 +133,13 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity void setSanityCheck(const bool & enableCheck); /// \} - /// \name Set the control types of the controlled joints/freeflyer - /// Allowed types (string): qVEL | qACC | ffVEL | ffACC + /// \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 types of the controlled joints/freeflyer /// Allowed types (int): qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 void setControlTypeInt(const Vector& controlTypeVector); @@ -152,6 +161,8 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// 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. /// \{ @@ -159,8 +170,10 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity dg::SignalTimeDependent stateSOUT_; /// \brief Output integrated velocity from control. dg::SignalTimeDependent velocitySOUT_; - /// \brief Output integrated freeFlyer position from control (odometry predictive system). - dg::SignalTimeDependent freeFlyerPositionOdomSOUT_; + /// \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_; /// \} @@ -180,14 +193,24 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// 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 integrate(int t, const double & dt = 5e-2); + 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 (callback signal freeFlyerPositionOdomSOUT_). - dg::Vector& getFreeFlyerPosition(dg::Vector &ffPose, 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); @@ -205,6 +228,8 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity int debug_mode_; // Last integration iteration int last_integration_; + // Last integration iteration for freeflyer + int last_integration_FF_; public: diff --git a/src/state-integrator.cpp b/src/state-integrator.cpp index 14b32e0..bcd57f7 100644 --- a/src/state-integrator.cpp +++ b/src/state-integrator.cpp @@ -65,28 +65,35 @@ StateIntegrator::StateIntegrator( const std::string& n ) : Entity(n) , timestep_(TIMESTEP_DEFAULT) , position_(6) + , controlTypeFF_("") , ffPose_(6) , sanityCheck_(true) , controlSIN( NULL, "StateIntegrator(" + n + ")::input(double)::control" ) - , 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" ) - , freeFlyerPositionOdomSOUT_(boost::bind(&StateIntegrator::getFreeFlyerPosition,this,_1,_2), - controlSIN, - "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionOdom") - , freeFlyerVelocitySOUT_(boost::bind(&StateIntegrator::getFreeFlyerVelocity,this,_1,_2), - controlSIN, - "StateIntegrator(" + n + ")::output(vector)::freeFlyerVelocity") + , 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_ - << freeFlyerPositionOdomSOUT_ + << freeFlyerPositionEulerSOUT_ + << freeFlyerPositionQuatSOUT_ << freeFlyerVelocitySOUT_); position_.fill(.0); @@ -113,7 +120,7 @@ StateIntegrator::StateIntegrator( const std::string& n ) "\n" " Set state vector value\n" "\n"; - addCommand("set", + addCommand("setState", new command::Setter(*this, &StateIntegrator::setState, docstring)); docstring = @@ -123,14 +130,35 @@ StateIntegrator::StateIntegrator( const std::string& 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 | ffVEL:2 | ffACC:3\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 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"); @@ -150,14 +178,14 @@ StateIntegrator::~StateIntegrator( ) { return; } -void StateIntegrator::init(const double& step){ +void StateIntegrator::init(const double& step) { timestep_ = step; } -void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control, double dt) -{ +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; @@ -169,15 +197,26 @@ void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control * AngleAxisd(state(4), Vector3d::UnitY()) * AngleAxisd(state(3), Vector3d::UnitX()); - SE3().integrate (qin, control*dt, qout); - + SE3().integrate (qin, control * dt, qout); + + // Create the Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] + Matrix3d rotationMatrix = QuaternionMapd(qout.tail<4>().data()).normalized().toRotationMatrix(); + double m = sqrt(pow(rotationMatrix(2, 1), 2.0) + pow(rotationMatrix(2, 2), 2.0)); + 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 + } // Update freeflyer state (pose|vel) state.head<3>() = qout.head<3>(); - state.segment<3>(3) = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix().eulerAngles(0, 1, 2); + state.segment<3>(3) << r, p, y; } -const MatrixHomogeneous& StateIntegrator::freeFlyerPose() -{ +const MatrixHomogeneous& StateIntegrator::freeFlyerPose() { using Eigen::AngleAxisd; using Eigen::Vector3d; using Eigen::Quaterniond; @@ -192,23 +231,22 @@ const MatrixHomogeneous& StateIntegrator::freeFlyerPose() return freeFlyerPose_; } -void StateIntegrator::setControlType(const StringVector& controlTypeVector) -{ +void StateIntegrator::setControlType(const StringVector& controlTypeVector) { controlTypeVector_ = controlTypeVector; } -void StateIntegrator::setControlTypeInt(const Vector& controlTypeVector) -{ +void StateIntegrator::setControlTypeFreeFlyer(const std::string& controlTypeFF) { + controlTypeFF_ = controlTypeFF; +} + +void StateIntegrator::setControlTypeInt(const Vector& controlTypeVector) { std::string type; controlTypeVector_.resize(controlTypeVector.size()); - for (unsigned int i=0; i() = worldMwaist.translation(); - ffPose_.segment<3>(3) = worldMwaist.linear().eulerAngles(0,1,2); + ffPose_.segment<3>(3) = worldMwaist.linear().eulerAngles(0, 1, 2); } -void StateIntegrator::setSanityCheck(const bool & enableCheck) -{ +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]) - { +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) - { + if (debug_mode_ > 1) { std::cerr << "Control type not allowed/recognized: " - << strCtrlType + << strCtrlType << "\n Authorized control types: " << "qVEL | qACC | ffVEL | ffACC" << std::endl; @@ -294,103 +332,99 @@ int StateIntegrator::getControlType(const std::string &strCtrlType, SoTControlTy return 1; } -void StateIntegrator::integrate(int t, const double & dt) -{ +void StateIntegrator::integrateControl(int t, const double & dt) { controlSIN(t); const Vector & controlIN = controlSIN.accessCopy(); - if (controlTypeVector_.size() == 0){ + if (controlTypeVector_.size() == 0) { dgRTLOG () << "StateIntegrator::integrate: The controlType vector cannot be empty" << '\n'; return; } - if (sanityCheck_ && controlIN.hasNaN()) - { + if (sanityCheck_ && controlIN.hasNaN()) { dgRTLOG () << "StateIntegrator::integrate: Control has NaN values: " << '\n' << controlIN.transpose() << '\n'; return; } - - // Integration - unsigned int indexControlVector = 0, indexComputedVector = 0; - for (unsigned int indexTypeVector = 0; indexTypeVector 0) - { - if (debug_mode_ > 1) - { - std::cerr << "No control type for joint at position " << indexTypeVector + // Integration joints + for (unsigned int j = 0; j < controlTypeVector_.size(); j++) { + SoTControlType type; + 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; - } - - // Control of the freeflyer in acceleration - if (type == ffACC) - { - // Set controlFreeFlyer from control (size 6 because derivative of twist) - Vector controlFreeFlyer = controlIN.segment<6>(indexControlVector); - // Integrate once to obtain velocity -> update ffVel_ - integrateRollPitchYaw(ffVel_, controlFreeFlyer, dt); - indexControlVector = indexControlVector + 6; - } - // Control of the freeflyer in velocity - else if (type == ffVEL) - { - // Set ffVel_ (twist) from control for the integration in position - ffVel_ = controlIN.segment<6>(indexControlVector); - indexControlVector = indexControlVector + 6; - } - - // Control of a joint in acceleration - else if (type == qACC) - { + } + // Control of a joint in acceleration + if (type == qACC) { // Set acceleration from control and integrate to find velocity. - acceleration_[indexComputedVector] = controlIN[indexControlVector]; - velocity_[indexComputedVector] = velocity_[indexComputedVector] + acceleration_[indexComputedVector] * (0.5)*dt; - indexControlVector++; + acceleration_[j] = controlIN[j]; + velocity_[j] = velocity_[j] + acceleration_[j] * (0.5) * dt; } - // Control of a joint in velocity - else if (type == qVEL) - { + // Control of a joint in velocity + else if (type == qVEL) { // Set velocity from control. - acceleration_[indexComputedVector] = 0; - velocity_[indexComputedVector] = controlIN[indexControlVector]; - indexControlVector++; + acceleration_[j] = 0; + velocity_[j] = controlIN[j]; } - // Velocity integration - if (type == ffVEL || type == ffACC){ - // Integrate freeflyer velocity to obtain position -> update ffPose_ from ffVel_ - integrateRollPitchYaw(ffPose_, ffVel_, dt); - } - else if (type == qVEL || type == qACC){ - // Velocity integration of the joint to get position - position_[indexComputedVector] = position_[indexComputedVector] + velocity_[indexComputedVector] * dt; - indexComputedVector++; - } - } + // 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_ == "") { + dgRTLOG () << "StateIntegrator::integrate: The controlType of the freeflyer cannot be empty" << '\n'; + return; + } + + if (sanityCheck_ && freeFlyerIN.hasNaN()) { + dgRTLOG () << "StateIntegrator::integrate: Control of the FreeFlyer has NaN values: " << '\n' + << freeFlyerIN.transpose() << '\n'; + return; + } + + SoTControlType typeFF; + if (getControlType(controlTypeFF_, typeFF) > 0) { + if (debug_mode_ > 1) { + std::cerr << "No control type for the Freeflyer " << std::endl; + } + return; + } + // Control of the freeflyer in acceleration + if (typeFF == ffACC) { + // Integrate once to obtain velocity -> update ffVel_ + integrateRollPitchYaw(ffVel_, freeFlyerIN, dt); + } + // Control of the freeflyer in velocity + else if (typeFF == 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; +void StateIntegrator::display ( std::ostream& os ) const { + os << name << ": " << position_ << endl; } -Vector& StateIntegrator::getPosition(Vector &controlOut, const int& t) -{ +Vector& StateIntegrator::getPosition(Vector &controlOut, const int& t) { ODEBUG5FULL("start"); sotDEBUGIN(25) ; // Integrate control - if (last_integration_ != t){ - integrate(t, timestep_); + if (last_integration_ != t) { + integrateControl(t, timestep_); last_integration_ = t; } sotDEBUG (25) << "position_ = " << position_ << std::endl; @@ -405,14 +439,13 @@ Vector& StateIntegrator::getPosition(Vector &controlOut, const int& t) sotDEBUGOUT(25) ; } -Vector& StateIntegrator::getVelocity(Vector &controlOut, const int& t) -{ +Vector& StateIntegrator::getVelocity(Vector &controlOut, const int& t) { ODEBUG5FULL("start"); sotDEBUGIN(25) ; // Integrate control - if (last_integration_ != t){ - integrate(t, timestep_); + if (last_integration_ != t) { + integrateControl(t, timestep_); last_integration_ = t; } sotDEBUG (25) << "velocity_ = " << velocity_ << std::endl; @@ -427,15 +460,14 @@ Vector& StateIntegrator::getVelocity(Vector &controlOut, const int& t) sotDEBUGOUT(25) ; } -Vector& StateIntegrator::getFreeFlyerPosition(Vector &ffPose, const int& t) -{ +Vector& StateIntegrator::getFreeFlyerPositionEuler(Vector &ffPose, const int& t) { ODEBUG5FULL("start"); sotDEBUGIN(25); // Integrate control - if (last_integration_ != t){ - integrate(t, timestep_); - last_integration_ = t; + if (last_integration_FF_ != t) { + integrateFreeFlyer(t, timestep_); + last_integration_FF_ = t; } sotDEBUG (25) << "ffPose_ = " << ffPose_ << std::endl; @@ -449,15 +481,45 @@ Vector& StateIntegrator::getFreeFlyerPosition(Vector &ffPose, const int& t) sotDEBUGOUT(25) ; } -Vector& StateIntegrator::getFreeFlyerVelocity(Vector &ffVel, const int& t) -{ +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.normalized().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_ != t){ - integrate(t, timestep_); - last_integration_ = t; + if (last_integration_FF_ != t) { + integrateFreeFlyer(t, timestep_); + last_integration_FF_ = t; } sotDEBUG (25) << "ffVel_ = " << ffVel_ << std::endl; diff --git a/unitTesting/test_state_integrator.cpp b/unitTesting/test_state_integrator.cpp index 6cfbc66..64b5e4a 100644 --- a/unitTesting/test_state_integrator.cpp +++ b/unitTesting/test_state_integrator.cpp @@ -45,7 +45,6 @@ int ReadYAMLFILE(dg::sot::Device &aDevice) { return 0; } - int main(int, char **) { unsigned int debug_mode = 2; @@ -79,50 +78,66 @@ int main(int, char **) { aState(j) = 0.0; aIntegrator.setState(aState); - /// Fix constant vector for the control entry - dg::Vector aControlVector(35); // with freeFlyer - for (unsigned int i = 0; i < 35; i++) + /// 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); - // Set the type vector defining the type of control for each joint and the freeflyer - // With strings - // Eigen::Matrix aControlTypeVector; - // aControlTypeVector[0] = "ffVEL"; // with freeFlyer in velocity - // for (unsigned int i = 1; i < 30; i++) - // { - // aControlTypeVector[i] = "qVEL"; //velocity - // } - // With int -> for addCommand - dg::Vector aControlTypeVector(30); - // Types in int qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 - aControlTypeVector[0] = 2.0; // with freeFlyer in velocity - for (unsigned int i = 1; i < 30; i++) + // Fix FreeFlyer control entry of the integrator + dg::Vector aFFControlVector(6); //TWIST + for (unsigned int i = 0; i < 6; i++) { - aControlTypeVector[i] = 0.0; //velocity + aFFControlVector[i] = -0.5; // in velocity } + aIntegrator.freeFlyerSIN.setConstant(aFFControlVector); - aIntegrator.controlSIN.setConstant(aControlVector); - // aIntegrator.setControlType(aControlTypeVector); - aIntegrator.setControlTypeInt(aControlTypeVector); + // 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); - aDevice.controlSIN.plug(&aIntegrator.stateSOUT_); + // 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.freeFlyerPositionOdomSOUT_(2001); + const dg::Vector & poseFF = aIntegrator.freeFlyerPositionEulerSOUT_(2001); std::cout << "\n ########### \n " << std::endl; - std::cout << "Final freeFlyerPositionOdomSOUT_: " << poseFF << std::endl; + std::cout << "Final freeFlyerPositionEulerSOUT_: " << poseFF << std::endl; - const dg::sot::MatrixHomogeneous & ffpose = aIntegrator.freeFlyerPose(); + std::cout << "\n ########### \n " << std::endl; + const dg::Vector & poseFFQ = aIntegrator.freeFlyerPositionQuatSOUT_(2001); + std::cout << "Final freeFlyerPositionQuatSOUT_: " << poseFFQ << std::endl; + + std::cout << "\n ########### \n " << std::endl; + const dg::sot::MatrixHomogeneous & ffposeMat = aIntegrator.freeFlyerPose(); std::cout << "Final freeFlyerPosition MatrixHomogeneous: " - << ffpose.translation() << "\n" - << ffpose.linear() << std::endl; + << ffposeMat.translation() << "\n" + << ffposeMat.linear() << std::endl; + std::cout << "\n ########### \n " << std::endl; std::cout << "Final integrator stateSOUT_ : " << aIntegrator.stateSOUT_(2001) << std::endl; const dg::Vector & aControl = aDevice.motorcontrolSOUT_(2001); From 8f80cdf03dd3550fbcc13689be7b351f747b88fd Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 3 Sep 2019 10:52:36 +0200 Subject: [PATCH 06/10] [State-integration] Format files with google style --- .../sot-dynamic-pinocchio/state-integrator.h | 30 +++++----- src/state-integrator.cpp | 16 +++--- unitTesting/test_state_integrator.cpp | 55 ++++++++----------- 3 files changed, 45 insertions(+), 56 deletions(-) diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h index 0b5dc17..0eec36b 100644 --- a/include/sot-dynamic-pinocchio/state-integrator.h +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -16,7 +16,7 @@ /* --------------------------------------------------------------------- */ #include -#include +#include /* SOT */ /// dg #include @@ -53,7 +53,7 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -typedef Eigen::Matrix StringVector; +typedef Eigen::Matrix StringVector; /// Specifies the nature of one joint control from SoT side enum SoTControlType { @@ -73,8 +73,7 @@ const std::string SoTControlType_s[] = { /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity -{ +class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity { public: static const std::string CLASS_NAME; @@ -97,7 +96,7 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// Acceleration vector of each actuator. dg::Vector acceleration_; - /// Type of the control vector + /// Type of the control vector /// It can be velocity or acceleration for the actuators. StringVector controlTypeVector_; /// Type of the control for the Freeflyer (velocity/acceleration). @@ -134,22 +133,22 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// \} /// \name Set the control types of the controlled joints - /// Allowed types (string): qVEL | qACC + /// 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 types of the controlled joints/freeflyer /// Allowed types (int): qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 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) { @@ -158,7 +157,7 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity public: /* --- SIGNALS --- */ - /// Input signal handling the control vector + /// 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 @@ -191,14 +190,14 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity /// 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 + /// - 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 + /// - 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_). @@ -233,8 +232,7 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity public: - const pinocchio::Model & getModel() - { + const pinocchio::Model & getModel() { return model_; } diff --git a/src/state-integrator.cpp b/src/state-integrator.cpp index bcd57f7..682767c 100644 --- a/src/state-integrator.cpp +++ b/src/state-integrator.cpp @@ -77,8 +77,8 @@ StateIntegrator::StateIntegrator( const std::string& n ) controlSIN, "StateIntegrator(" + n + ")::output(vector)::velocity" ) , freeFlyerPositionEulerSOUT_(boost::bind(&StateIntegrator::getFreeFlyerPositionEuler, this, _1, _2), - freeFlyerSIN, - "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionEuler") + freeFlyerSIN, + "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionEuler") , freeFlyerPositionQuatSOUT_(boost::bind(&StateIntegrator::getFreeFlyerPositionQuat, this, _1, _2), freeFlyerSIN, "StateIntegrator(" + n + ")::output(vector)::freeFlyerPositionQuat") @@ -202,12 +202,12 @@ void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control // Create the Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi] Matrix3d rotationMatrix = QuaternionMapd(qout.tail<4>().data()).normalized().toRotationMatrix(); double m = sqrt(pow(rotationMatrix(2, 1), 2.0) + pow(rotationMatrix(2, 2), 2.0)); - double p = atan2(-rotationMatrix(2,0), m); + double p = atan2(-rotationMatrix(2, 0), m); double r, y; - if (abs(abs(p) - M_PI / 2) < 0.001){ + if (abs(abs(p) - M_PI / 2) < 0.001) { r = 0; y = -atan2(rotationMatrix(0, 1), rotationMatrix(1, 1)); - } else { + } else { y = atan2(rotationMatrix(1, 0), rotationMatrix(0, 0)); // alpha r = atan2(rotationMatrix(2, 1), rotationMatrix(2, 2)); // gamma } @@ -356,7 +356,7 @@ void StateIntegrator::integrateControl(int t, const double & dt) { << std::endl; } break; - } + } // Control of a joint in acceleration if (type == qACC) { // Set acceleration from control and integrate to find velocity. @@ -375,7 +375,7 @@ void StateIntegrator::integrateControl(int t, const double & dt) { } } -void StateIntegrator::integrateFreeFlyer(int t, const double & dt){ +void StateIntegrator::integrateFreeFlyer(int t, const double & dt) { freeFlyerSIN(t); const Vector & freeFlyerIN = freeFlyerSIN.accessCopy(); @@ -487,7 +487,7 @@ Vector& StateIntegrator::getFreeFlyerPositionQuat(Vector &ffPose, const int& t) using Eigen::Vector3d; ODEBUG5FULL("start"); sotDEBUGIN(25); - + ffPose.resize(7); // Integrate control if (last_integration_FF_ != t) { diff --git a/unitTesting/test_state_integrator.cpp b/unitTesting/test_state_integrator.cpp index 64b5e4a..5076ac9 100644 --- a/unitTesting/test_state_integrator.cpp +++ b/unitTesting/test_state_integrator.cpp @@ -31,13 +31,13 @@ namespace dg = dynamicgraph; int ReadYAMLFILE(dg::sot::Device &aDevice) { // Reflect how the data are splitted in two yaml files in the sot - std::ifstream yaml_file_controller("../../unitTesting/sot_controller.yaml"); + std::ifstream yaml_file_controller("../../unitTesting/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("../../unitTesting/sot_params.yaml"); + std::ifstream yaml_file_params("../../unitTesting/sot_params.yaml"); std::string yaml_string_params; yaml_string_params.assign((std::istreambuf_iterator(yaml_file_params) ), (std::istreambuf_iterator() ) ); @@ -74,37 +74,34 @@ int main(int, char **) { aIntegrator.setURDFModel(robot_description); dg::Vector aState(29); // without freeFlyer - for(unsigned j=0;j aControlTypeVector; - for (unsigned int i = 0; i < 29; i++) - { + Eigen::Matrix aControlTypeVector; + for (unsigned int i = 0; i < 29; i++) { aControlTypeVector[i] = "qVEL"; //velocity } - aIntegrator.setControlType(aControlTypeVector); + aIntegrator.setControlType(aControlTypeVector); // Set type of control for the FreeFlyer - aIntegrator.setControlTypeFreeFlyer("ffVEL"); //in velocity - + aIntegrator.setControlTypeFreeFlyer("ffVEL"); //in velocity + // With int -> for addCommand // dg::Vector aControlTypeVector(29); // Types in int qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 @@ -115,10 +112,9 @@ int main(int, char **) { // } // aIntegrator.setControlTypeInt(aControlTypeVector); - // PLUG the output signal of the integrator to the entry of the device + // 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++) - { + for (unsigned int i = 0; i < 2000; i++) { aDevice.motorcontrolSOUT_.recompute(i); aDevice.motorcontrolSOUT_.setReady(); aIntegrator.stateSOUT_.setReady(); @@ -133,9 +129,9 @@ int main(int, char **) { std::cout << "\n ########### \n " << std::endl; const dg::sot::MatrixHomogeneous & ffposeMat = aIntegrator.freeFlyerPose(); - std::cout << "Final freeFlyerPosition MatrixHomogeneous: " - << ffposeMat.translation() << "\n" - << ffposeMat.linear() << std::endl; + std::cout << "Final freeFlyerPosition MatrixHomogeneous: " + << ffposeMat.translation() << "\n" + << ffposeMat.linear() << std::endl; std::cout << "\n ########### \n " << std::endl; std::cout << "Final integrator stateSOUT_ : " << aIntegrator.stateSOUT_(2001) << std::endl; @@ -148,30 +144,25 @@ int main(int, char **) { dgsot::JointSHWControlType_iterator it_control_type; for (it_control_type = aDevice.jointDevices_.begin(); it_control_type != aDevice.jointDevices_.end(); - it_control_type++) - { + 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)) - { + 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" + << "diff: " << ldiff << "\n" << "Velocity limit: " << urdf_joints[u_index]->limits->velocity << std::endl; } - } - else - { - std::cout << "motorcontrolSOUT: " << aControl[lctl_index] << std::endl; + } else { + std::cout << "motorcontrolSOUT: " << aControl[lctl_index] << std::endl; } } std::cout << "\n ########### \n " << std::endl; From 0c55661bb564a14e55c2c03131547285a86007c0 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 3 Sep 2019 14:40:18 +0200 Subject: [PATCH 07/10] [State-integrator] Change controlTypeVector values to SoTControlType Store the values as SoTControlType instead of using strings. First integration (acceleration) of freeflyer changed to normal integration not rollPitchYaw (mistake). Add command to define the type of one joint. --- .../sot-dynamic-pinocchio/state-integrator.h | 15 ++- src/state-integrator.cpp | 119 +++++++++++------- unitTesting/test_state_integrator.cpp | 9 +- 3 files changed, 88 insertions(+), 55 deletions(-) diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h index 0eec36b..d58147f 100644 --- a/include/sot-dynamic-pinocchio/state-integrator.h +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -98,9 +98,9 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity { /// Type of the control vector /// It can be velocity or acceleration for the actuators. - StringVector controlTypeVector_; + std::vector controlTypeVector_; /// Type of the control for the Freeflyer (velocity/acceleration). - std::string controlTypeFF_; + SoTControlType controlTypeFF_; /// Store Position of free flyer joint Eigen::VectorXd ffPose_; @@ -139,8 +139,11 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity { /// Allowed types (string): ffVEL | ffACC void setControlTypeFreeFlyer(const std::string& controlTypeFF); - /// \name Set the control types of the controlled joints/freeflyer - /// Allowed types (int): qVEL:0 | qACC:1 | ffVEL:2 | ffACC:3 + /// \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 @@ -180,9 +183,11 @@ class SOTSTATEINTEGRATOR_EXPORT StateIntegrator: public Entity { protected: - /// Integrate the freeflyer state (to obtain position or velocity). + /// 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_; diff --git a/src/state-integrator.cpp b/src/state-integrator.cpp index 682767c..65cf4a5 100644 --- a/src/state-integrator.cpp +++ b/src/state-integrator.cpp @@ -65,7 +65,6 @@ StateIntegrator::StateIntegrator( const std::string& n ) : Entity(n) , timestep_(TIMESTEP_DEFAULT) , position_(6) - , controlTypeFF_("") , ffPose_(6) , sanityCheck_(true) , controlSIN( NULL, "StateIntegrator(" + n + ")::input(double)::control" ) @@ -151,6 +150,16 @@ StateIntegrator::StateIntegrator( const std::string& 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" @@ -199,9 +208,18 @@ void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control 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] - Matrix3d rotationMatrix = QuaternionMapd(qout.tail<4>().data()).normalized().toRotationMatrix(); - double m = sqrt(pow(rotationMatrix(2, 1), 2.0) + pow(rotationMatrix(2, 2), 2.0)); + 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) { @@ -211,9 +229,7 @@ void StateIntegrator::integrateRollPitchYaw(Vector& state, const Vector& control y = atan2(rotationMatrix(1, 0), rotationMatrix(0, 0)); // alpha r = atan2(rotationMatrix(2, 1), rotationMatrix(2, 2)); // gamma } - // Update freeflyer state (pose|vel) - state.head<3>() = qout.head<3>(); - state.segment<3>(3) << r, p, y; + rollPitchYaw << r, p, y; } const MatrixHomogeneous& StateIntegrator::freeFlyerPose() { @@ -226,33 +242,57 @@ const MatrixHomogeneous& StateIntegrator::freeFlyerPose() { quat = AngleAxisd(ffPose_(5), Vector3d::UnitZ()) * AngleAxisd(ffPose_(4), Vector3d::UnitY()) * AngleAxisd(ffPose_(3), Vector3d::UnitX()); - freeFlyerPose_.linear() = quat.normalized().toRotationMatrix(); + freeFlyerPose_.linear() = quat.toRotationMatrix(); return freeFlyerPose_; } void StateIntegrator::setControlType(const StringVector& controlTypeVector) { - controlTypeVector_ = 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) { - controlTypeFF_ = 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) { - std::string type; controlTypeVector_.resize(controlTypeVector.size()); - for (unsigned int i = 0; i < controlTypeVector.size(); i++) { - try { - type = SoTControlType_s[int(controlTypeVector[i])]; - } catch (...) { - dgRTLOG () << "StateIntegrator::setControlTypeInt: The controlType at position " - << i << " is not valid: " << controlTypeVector[i] << "\n" - << "Expected values are (int): qVEL = 0 | qACC = 1 | ffVEL = 2 | ffACC = 3" - << '\n'; - } - controlTypeVector_[i] = type; + setControlTypeJointInt(i, int(controlTypeVector[i])); } } @@ -300,7 +340,7 @@ void StateIntegrator::setVelocityFreeflyer( const Vector& vel ) { } void StateIntegrator::setRoot( const Matrix & root ) { - Eigen::Matrix4d _matrix4d(root); + Eigen::Matrix4d _matrix4d(root); // needed to define type of root for transformation to MatrixHomogeneous MatrixHomogeneous _root(_matrix4d); setRoot( _root ); } @@ -308,7 +348,10 @@ void StateIntegrator::setRoot( const Matrix & root ) { void StateIntegrator::setRoot( const MatrixHomogeneous & worldMwaist ) { freeFlyerPose_ = worldMwaist; ffPose_.head<3>() = worldMwaist.translation(); - ffPose_.segment<3>(3) = worldMwaist.linear().eulerAngles(0, 1, 2); + Eigen::Vector3d rollPitchYaw; + Eigen::Matrix3d rot(worldMwaist.linear()); + rotationMatrixToEuler(rot, rollPitchYaw); + ffPose_.segment<3>(3) << rollPitchYaw; } void StateIntegrator::setSanityCheck(const bool & enableCheck) { @@ -348,23 +391,14 @@ void StateIntegrator::integrateControl(int t, const double & dt) { } // Integration joints for (unsigned int j = 0; j < controlTypeVector_.size(); j++) { - SoTControlType type; - 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; - } // Control of a joint in acceleration - if (type == qACC) { + 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 (type == qVEL) { + else if (controlTypeVector_[j] == qVEL) { // Set velocity from control. acceleration_[j] = 0; velocity_[j] = controlIN[j]; @@ -379,8 +413,8 @@ void StateIntegrator::integrateFreeFlyer(int t, const double & dt) { freeFlyerSIN(t); const Vector & freeFlyerIN = freeFlyerSIN.accessCopy(); - if (controlTypeFF_ == "") { - dgRTLOG () << "StateIntegrator::integrate: The controlType of the freeflyer cannot be empty" << '\n'; + if (controlTypeFF_ != ffACC && controlTypeFF_ != ffVEL) { + dgRTLOG () << "StateIntegrator::integrate: The controlType of the freeflyer is not properly set (empty?)" << '\n'; return; } @@ -390,20 +424,13 @@ void StateIntegrator::integrateFreeFlyer(int t, const double & dt) { return; } - SoTControlType typeFF; - if (getControlType(controlTypeFF_, typeFF) > 0) { - if (debug_mode_ > 1) { - std::cerr << "No control type for the Freeflyer " << std::endl; - } - return; - } // Control of the freeflyer in acceleration - if (typeFF == ffACC) { + if (controlTypeFF_ == ffACC) { // Integrate once to obtain velocity -> update ffVel_ - integrateRollPitchYaw(ffVel_, freeFlyerIN, dt); + ffVel_ = ffVel_ + freeFlyerIN * (0.5) * dt; } // Control of the freeflyer in velocity - else if (typeFF == ffVEL) { + else if (controlTypeFF_ == ffVEL) { // Set ffVel_ (twist) from control for the integration in position ffVel_ = freeFlyerIN; } @@ -500,7 +527,7 @@ Vector& StateIntegrator::getFreeFlyerPositionQuat(Vector &ffPose, const int& t) quat = AngleAxisd(ffPose_(5), Vector3d::UnitZ()) * AngleAxisd(ffPose_(4), Vector3d::UnitY()) * AngleAxisd(ffPose_(3), Vector3d::UnitX()); - ffPose.segment<4>(3) = quat.normalized().coeffs(); + ffPose.segment<4>(3) = quat.coeffs(); sotDEBUG (25) << "ffPose = " << ffPose << std::endl; diff --git a/unitTesting/test_state_integrator.cpp b/unitTesting/test_state_integrator.cpp index 5076ac9..c6b59f3 100644 --- a/unitTesting/test_state_integrator.cpp +++ b/unitTesting/test_state_integrator.cpp @@ -118,23 +118,24 @@ int main(int, char **) { 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_: " << poseFF << 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_: " << poseFFQ << std::endl; + 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: " + std::cout << "Final freeFlyerPosition MatrixHomogeneous: \n" << ffposeMat.translation() << "\n" << ffposeMat.linear() << std::endl; std::cout << "\n ########### \n " << std::endl; - std::cout << "Final integrator stateSOUT_ : " << aIntegrator.stateSOUT_(2001) << std::endl; + std::cout << "Final integrator stateSOUT_ : \n" << aIntegrator.stateSOUT_(2001) << std::endl; const dg::Vector & aControl = aDevice.motorcontrolSOUT_(2001); double diff = 0, ldiff; From 10574c42cb91372e69dff514cef4f260cb330704 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 5 Nov 2019 15:56:38 +0100 Subject: [PATCH 08/10] [State-integrator] Change name device to generic-device --- include/sot-dynamic-pinocchio/state-integrator.h | 3 ++- unitTesting/test_state_integrator.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/sot-dynamic-pinocchio/state-integrator.h b/include/sot-dynamic-pinocchio/state-integrator.h index d58147f..f2bec2e 100644 --- a/include/sot-dynamic-pinocchio/state-integrator.h +++ b/include/sot-dynamic-pinocchio/state-integrator.h @@ -75,7 +75,8 @@ const std::string SoTControlType_s[] = { 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; diff --git a/unitTesting/test_state_integrator.cpp b/unitTesting/test_state_integrator.cpp index c6b59f3..bba6ec6 100644 --- a/unitTesting/test_state_integrator.cpp +++ b/unitTesting/test_state_integrator.cpp @@ -19,7 +19,7 @@ using namespace std; #include #include #include -#include +#include #include "sot-dynamic-pinocchio/state-integrator.h" #include @@ -29,7 +29,7 @@ namespace dg = dynamicgraph; #define BOOST_TEST_MODULE test-state-integrator -int ReadYAMLFILE(dg::sot::Device &aDevice) { +int ReadYAMLFILE(dg::sot::GenericDevice &aDevice) { // Reflect how the data are splitted in two yaml files in the sot std::ifstream yaml_file_controller("../../unitTesting/sot_controller.yaml"); std::string yaml_string_controller; @@ -62,7 +62,7 @@ int main(int, char **) { robot_description = strStream.str(); /// Test reading the URDF file. - dg::sot::Device aDevice(std::string("simple_humanoid")); + dg::sot::GenericDevice aDevice(std::string("simple_humanoid")); aDevice.setDebugMode(debug_mode); aDevice.setURDFModel(robot_description); From 63b76ee10218beac0ee30a8b4507441c6cda485b Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 31 Jul 2020 11:08:16 +0200 Subject: [PATCH 09/10] [State-integrator] Remove absolute urdf path in test Use CMAKE path --- tests/test_state_integrator.cpp | 38 +++++++++++++++++++++++++-------- 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/tests/test_state_integrator.cpp b/tests/test_state_integrator.cpp index bace7bc..01d3e5b 100644 --- a/tests/test_state_integrator.cpp +++ b/tests/test_state_integrator.cpp @@ -16,6 +16,8 @@ using namespace std; #include +#include +#include #include #include #include @@ -49,17 +51,35 @@ BOOST_AUTO_TEST_CASE(test_state_integrator) { unsigned int debug_mode = 2; - std::string robot_description; - ifstream urdfFile; - std::string filename = "/opt/openrobots/share/simple_humanoid_description/urdf/simple_humanoid.urdf"; - urdfFile.open(filename.c_str()); - if (!urdfFile.is_open()) { - std::cerr << "Unable to open " << filename << std::endl; - BOOST_CHECK(false); - return; + // 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; } - stringstream strStream; + + // If not found fails + if (filename.size()==0) + { + cerr << "Unable to find simple_humanoid.urdf" << 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. From ec6d944a3c164da546df7613e4589cee19b44ab0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20RAMUZAT?= Date: Tue, 4 Aug 2020 12:32:32 +0200 Subject: [PATCH 10/10] Update tests/test_state_integrator.cpp Co-authored-by: Joseph Mirabel --- tests/test_state_integrator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_state_integrator.cpp b/tests/test_state_integrator.cpp index 01d3e5b..657ea66 100644 --- a/tests/test_state_integrator.cpp +++ b/tests/test_state_integrator.cpp @@ -71,7 +71,7 @@ BOOST_AUTO_TEST_CASE(test_state_integrator) { // If not found fails if (filename.size()==0) { - cerr << "Unable to find simple_humanoid.urdf" << endl; + cerr << "Unable to find simple_humanoid_description/urdf/simple_humanoid.urdf in CMAKE_PREFIX_PATH" << endl; exit(-1); }