From 82bae54739093798597d50c74032bbb40a11bd40 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 31 Jul 2019 12:19:56 +0200 Subject: [PATCH 01/43] [Device] First draft removing integration and using urdf/yaml files --- cmake | 2 +- include/sot/core/device.hh | 467 ++++++++---- src/tools/device.cpp | 1175 +++++++++++++++++++---------- src/tools/robot-simu.cpp | 52 +- unitTesting/CMakeLists.txt | 1 + unitTesting/tools/test_device.cpp | 392 ++++++++++ 6 files changed, 1495 insertions(+), 594 deletions(-) create mode 100644 unitTesting/tools/test_device.cpp diff --git a/cmake b/cmake index 0fb087eb8..5c8c19f49 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 0fb087eb8e78db616329de0e4ab55abbe015bbe8 +Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index f81de5ed7..5cc5fa7b4 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -1,9 +1,11 @@ /* - * Copyright 2010, + * Copyright 2010-2018, CNRS * Florent Lamiraux + * Olivier Stasse * * CNRS * + * See LICENSE.txt */ #ifndef SOT_DEVICE_HH @@ -13,173 +15,326 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +#include +#include + +/// URDF DOM +#include +#include +#include + +/// YAML CPP +#include + /* -- MaaL --- */ #include namespace dg = dynamicgraph; /* SOT */ +/// dg #include #include +/// sot-core #include "sot/core/periodic-call.hh" #include #include "sot/core/api.hh" +#include + +namespace dgsot = dynamicgraph::sot; +namespace dg = dynamicgraph; namespace dynamicgraph { - namespace sot { - - /// Define the type of input expected by the robot - enum ControlInput - { - CONTROL_INPUT_NO_INTEGRATION=0, - CONTROL_INPUT_ONE_INTEGRATION=1, - CONTROL_INPUT_TWO_INTEGRATION=2, - CONTROL_INPUT_SIZE=3 - }; - - const std::string ControlInput_s[] = - { - "noInteg", "oneInteg", "twoInteg" - }; - - /* --------------------------------------------------------------------- */ - /* --- CLASS ----------------------------------------------------------- */ - /* --------------------------------------------------------------------- */ - - class SOT_CORE_EXPORT Device - :public Entity - { - public: - static const std::string CLASS_NAME; - virtual const std::string& getClassName(void) const { - return CLASS_NAME; - } - - enum ForceSignalSource - { - FORCE_SIGNAL_RLEG, - FORCE_SIGNAL_LLEG, - FORCE_SIGNAL_RARM, - FORCE_SIGNAL_LARM - }; - - protected: - dg::Vector state_; - dg::Vector velocity_; - bool sanityCheck_; - dg::Vector vel_control_; - ControlInput controlInputType_; - bool withForceSignals[4]; - PeriodicCall periodicCallBefore_; - PeriodicCall periodicCallAfter_; - double timestep_; - - /// \name Robot bounds used for sanity checks - /// \{ - Vector upperPosition_; - Vector upperVelocity_; - Vector upperTorque_; - Vector lowerPosition_; - Vector lowerVelocity_; - Vector lowerTorque_; - /// \} - public: - - /* --- CONSTRUCTION --- */ - Device(const std::string& name); - /* --- DESTRUCTION --- */ - virtual ~Device(); - - virtual void setStateSize(const unsigned int& size); - virtual void setState(const dg::Vector& st); - void setVelocitySize(const unsigned int& size); - virtual void setVelocity(const dg::Vector & vel); - virtual void setSecondOrderIntegration(); - virtual void setNoIntegration(); - virtual void setControlInputType(const std::string& cit); - virtual void increment(const double & dt = 5e-2); - - /// \name Sanity check parameterization - /// \{ - void setSanityCheck (const bool & enableCheck); - void setPositionBounds(const Vector& lower, const Vector& upper); - void setVelocityBounds(const Vector& lower, const Vector& upper); - void setTorqueBounds (const Vector& lower, const Vector& upper); - /// \} - - public: /* --- DISPLAY --- */ - virtual void display(std::ostream& os) const; - virtual void cmdDisplay(); - SOT_CORE_EXPORT friend std::ostream& - operator<<(std::ostream& os,const Device& r) { - r.display(os); return os; - } - - public: /* --- SIGNALS --- */ - - dynamicgraph::SignalPtr controlSIN; - dynamicgraph::SignalPtr attitudeSIN; - dynamicgraph::SignalPtr zmpSIN; - - /// \name Device current state. - /// \{ - dynamicgraph::Signal stateSOUT; - dynamicgraph::Signal velocitySOUT; - dynamicgraph::Signal attitudeSOUT; - /*! \brief The current state of the robot from the command viewpoint. */ - dynamicgraph::Signal motorcontrolSOUT; - dynamicgraph::Signal previousControlSOUT; - /*! \brief The ZMP reference send by the previous controller. */ - dynamicgraph::Signal ZMPPreviousControllerSOUT; - /// \} - - /// \name Real robot current state - /// This corresponds to the real encoders values and take into - /// account the stabilization step. Therefore, this usually - /// does *not* match the state control input signal. - /// \{ - /// Motor positions - dynamicgraph::Signal robotState_; - /// Motor velocities - dynamicgraph::Signal robotVelocity_; - /// The force torque sensors - dynamicgraph::Signal* forcesSOUT[4]; - /// Motor torques - /// \todo why pseudo ? - dynamicgraph::Signal pseudoTorqueSOUT; - /// \} - - protected: - /// Compute roll pitch yaw angles of freeflyer joint. - void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control, - double dt); - /// Store Position of free flyer joint - MatrixHomogeneous ffPose_; - /// Compute the new position, from the current control. - /// - /// When sanity checks are enabled, this checks that the control is within - /// bounds. There are three cases, depending on what the control is: - /// - position: checks that the position is within bounds, - /// - velocity: checks that the velocity and the future position are - /// within bounds, - /// - acceleration: checks that the acceleration, the future velocity and - /// position are within bounds. - /// \todo in order to check the acceleration, we need - /// pinocchio and the contact forces in order to estimate - /// the joint torques for the given acceleration. - virtual void integrate( const double & dt ); - protected: - /// Get freeflyer pose - const MatrixHomogeneous& freeFlyerPose() const; - public: - virtual void setRoot( const dg::Matrix & root ); - - - virtual void setRoot( const MatrixHomogeneous & worldMwaist ); - private: - // Intermediate variable to avoid dynamic allocation - dg::Vector forceZero6; - }; - } // namespace sot +namespace sot { + +/// Specifies the nature of one joint control +/// It is used for both the SoT side and the hardware side. +enum ControlType { + POSITION = 0, + VELOCITY = 1, + ACCELERATION = 2, + TORQUE = 3 +}; + +const std::string ControlType_s[] = { + "POSITION", "VELOCITY", "ACCELERATION", "TORQUE" +}; + +//@} + +/// \brief Store information on each joint. +struct JointSoTHWControlType { + /// Defines the control from the Stack-of-Tasks side (for instance, position) + ControlType SoTcontrol; + /// Defines the control from the hardware side. + ControlType HWcontrol; + /// Position of the joint in the control vector. + int control_index; + /// Position of the joint in the URDF index. + int urdf_index; + + /// Various indexes for the sensor signals. + /// This may vary if some joints does not support this feature. + ///@{ + /// Position of the joint in the temperature vector + int temperature_index; + + /// Position of the joint in the velocity vector + int velocity_index; + + /// Position of the joint in the current vector + int current_index; + + /// Position of the joint in the torque vector + int torque_index; + + /// Position of the joint in the joint-angles vector + int joint_angle_index; + + /// Position of the joint in the motor-angles vector + int motor_angle_index; + + ///@} + JointSoTHWControlType(); +}; + +struct IMUSOUT { + std::string imu_sensor_name; + dg::Signal attitudeSOUT; + dg::Signal accelerometerSOUT; + dg::Signal gyrometerSOUT; + IMUSOUT(const std::string &limu_sensor_name, + const std::string &device_name): + imu_sensor_name(limu_sensor_name) + , attitudeSOUT("Device(" + device_name + + ")::output(vector6)::" + imu_sensor_name + "_attitudeSOUT") + , accelerometerSOUT("Device(" + device_name + + ")::output(vector3)::" + imu_sensor_name + "_accelerometerSOUT") + , gyrometerSOUT("Device(" + device_name + + ")::output(vector3)::" + imu_sensor_name + "_gyrometerSOUT") + {} +}; + +typedef std::map::iterator +JointSHWControlType_iterator; +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +class SOT_CORE_EXPORT Device: public Entity +{ + + public: + static const std::string CLASS_NAME; + virtual const std::string& getClassName(void) const { + return CLASS_NAME; + } + static const double TIMESTEP_DEFAULT; + + /// Maps of joint devices. + std::map jointDevices_; + + /// Set integration time. + void timeStep(double ts) { timestep_ = ts;} + + protected: + /// \brief Current integration step. + double timestep_; + + /// \name Vectors related to the state. + ///@{ + + /// Control vector of the robot wrt urdf file. + Eigen::VectorXd control_; + + ///@} + + + bool sanityCheck_; + + /// Specifies the control input by each element of the state vector. + std::map sotControlType_; + std::map hwControlType_; + + /// + PeriodicCall periodicCallBefore_; + PeriodicCall periodicCallAfter_; + + public: + + /* --- CONSTRUCTION --- */ + Device(const std::string& name); + /* --- DESTRUCTION --- */ + virtual ~Device(); + + virtual void setState(const dg::Vector& st); + + /// Set control input type. + virtual void setSoTControlType(const std::string &jointNames, + const std::string &sotCtrlType); + virtual void setHWControlType(const std::string &jointNames, + const std::string &hwCtrlType); + virtual void increment(); + /// Read directly the URDF model + void setURDFModel(const std::string &aURDFModel); + + /// \name Sanity check parameterization + /// \{ + void setSanityCheck (const bool & enableCheck); + /// \} + + /// \name Set index in vector (position, velocity, acceleration, control) + /// \{ + void setControlPos(const std::string &jointName, + const unsigned & index); + /// \} + public: /* --- DISPLAY --- */ + virtual void display(std::ostream& os) const; + SOT_CORE_EXPORT friend std::ostream& + operator<<(std::ostream& os, const Device& 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 hardware. + /// The control vector can be position and effort. + /// It depends on each of the actuator + dg::SignalPtr controlSIN; + + + /// \name Device current state. + /// \{ + /// \brief Output integrated state from control. + dg::Signal stateSOUT_; + /// \brief Output attitude provided by the hardware + /*! \brief The current state of the robot from the command viewpoint. */ + dg::Signal motorcontrolSOUT_; + dg::Signal previousControlSOUT_; + /// \} + + /// \name Real robot current state + /// This corresponds to the real encoders values and take into + /// account the stabilization step. Therefore, this usually + /// does *not* match the state control input signal. + /// \{ + /// Motor positions + dg::Signal robotState_; + /// Motor velocities + dg::Signal robotVelocity_; + /// The force torque sensors + std::vector*> forcesSOUT_; + /// The imu sensors + std::vector imuSOUT_; + /// Motor or pseudo torques (estimated or build from PID) + dg::Signal * pseudoTorqueSOUT_; + /// Temperature signal + dg::Signal * temperatureSOUT_; + /// Current signal + dg::Signal * currentsSOUT_; + /// Motor angles signal + dg::Signal * motor_anglesSOUT_; + /// Joint angles signal + dg::Signal * joint_anglesSOUT_; + /// P gains signal + dg::Signal * p_gainsSOUT_; + /// D gains signal + dg::Signal * d_gainsSOUT_; + /// \} + + /// Parse a YAML string for configuration. + int ParseYAMLString(const std::string &aYamlString); + + /// \name Robot Side + ///@{ + + /// \brief Allows the robot to fill in the internal variables of the device + /// to publish data on the signals. + void setSensors(std::map &sensorsIn); + void setupSetSensors(std::map &sensorsIn); + void nominalSetSensors(std::map &sensorsIn); + void cleanupSetSensors(std::map &sensorsIn); + + /// \brief Provides to the robot the control information. + void getControl(std::map &anglesOut); + ///@} + protected: + + + void setControlType(const std::string &strCtrlType, + ControlType &aCtrlType); + + /// Compute the new control, from the given one. + /// When the control is in position, checks that the position is within bounds. + virtual void updateControl(const Vector & controlIN); + + /// \name YAML related methods + /// @{ + /// Parse YAML for mapping between hardware and sot + /// starting from a YAML-cpp node. + int ParseYAMLMapHardware(YAML::Node &map_hs_control); + + /// Parse YAML for sensors from a YAML-cpp node. + int ParseYAMLSensors(YAML::Node &map_sensors); + + /// Parse YAML for joint sensors from YAML-cpp node. + int ParseYAMLJointSensor(std::string & joint_name, + YAML::Node &aJointSensors); + /// @} + + /// \name Signals related methods + ///@{ + /// \brief Creates a signal called Device(DeviceName)::output(vector6)::force_sensor_name + void CreateAForceSignal(const std::string & force_sensor_name); + /// \brief Creates a signal called Device(DeviceName)::output(vector6)::imu_sensor_name + void CreateAnImuSignal(const std::string & imu_sensor_name); + + /// \brief Creates signals based on the joints information parsed by the YAML string. + int UpdateSignals(); + + ///@} + /// Get freeflyer pose + const MatrixHomogeneous& freeFlyerPose() const; + + /// Protected methods for internal variables filling + void setSensorsForce(std::map &SensorsIn, int t); + void setSensorsIMU(std::map &SensorsIn, int t); + void setSensorsEncoders(std::map &SensorsIn, int t); + void setSensorsVelocities(std::map &SensorsIn, int t); + void setSensorsTorquesCurrents(std::map &SensorsIn, int t); + + void setSensorsGains(std::map &SensorsIn, int t); + + public: + virtual void setRoot( const dg::Matrix & root ); + virtual void setRoot( const MatrixHomogeneous & worldMwaist ); + + private: + + // Intermediate variable to avoid dynamic allocation + dg::Vector forceZero6; + + // URDF Model of the robot + ::urdf::ModelInterfaceSharedPtr model_; + std::vector< ::urdf::JointSharedPtr > urdf_joints_; + + // Debug mode + int debug_mode_; + + // Intermediate index when parsing YAML file. + int temperature_index_, velocity_index_, + current_index_, torque_index_, + joint_angle_index_, + motor_angle_index_ + ; + + public: + const ::urdf::ModelInterfaceSharedPtr & getModel() { + return model_; + } +}; +} // namespace sot } // namespace dynamicgraph #endif /* #ifndef SOT_DEVICE_HH */ + diff --git a/src/tools/device.cpp b/src/tools/device.cpp index 0818c07d6..bc09ffa40 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -1,9 +1,9 @@ /* - * Copyright 2010, - * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux - * - * CNRS + * Copyright 2019, CNRS + * Author: Olivier Stasse * + * Please check LICENSE.txt for licensing + * */ /* --------------------------------------------------------------------- */ @@ -13,7 +13,6 @@ /* SOT */ #define ENABLE_RT_LOG -#include #include "sot/core/device.hh" #include using namespace std; @@ -21,123 +20,110 @@ using namespace std; #include #include #include -#include #include #include -#include using namespace dynamicgraph::sot; using namespace dynamicgraph; -const std::string Device::CLASS_NAME = "Device"; +#define DBGFILE "/tmp/device.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 Device::CLASS_NAME = "Device"; +const double Device::TIMESTEP_DEFAULT = 0.001; + + +JointSoTHWControlType::JointSoTHWControlType(): + control_index(-1) + ,urdf_index(-1) + ,temperature_index(-1) + ,velocity_index(-1) + ,current_index(-1) + ,torque_index(-1) + ,joint_angle_index(-1) + ,motor_angle_index(-1) +{ +} /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -void Device:: -integrateRollPitchYaw -(Vector& state, - const Vector& control, - double dt) +Device::~Device( ) { - 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.head<6>()*dt, qout); - - // Update freeflyer pose - ffPose_.translation() = qout.head<3>(); - state.head<3>() = qout.head<3>(); + for( unsigned int i=0; i().data()).toRotationMatrix(); - state.segment<3>(3) = ffPose_.linear().eulerAngles(2,1,0).reverse(); -} + for( unsigned int i=0; i("Device("+n+")::output(vector6)::forceRLEG"); - forcesSOUT[1] = - new Signal("Device("+n+")::output(vector6)::forceLLEG"); - forcesSOUT[2] = - new Signal("Device("+n+")::output(vector6)::forceRARM"); - forcesSOUT[3] = - new Signal("Device("+n+")::output(vector6)::forceLARM"); - - signalRegistration( controlSIN< - (*this, &Device::setStateSize, docstring)); docstring = "\n" " Set state vector value\n" @@ -146,56 +132,31 @@ Device( const std::string& n ) new command::Setter (*this, &Device::setState, docstring)); - docstring = - "\n" - " Set velocity vector value\n" - "\n"; - addCommand("setVelocity", - new command::Setter - (*this, &Device::setVelocity, docstring)); - void(Device::*setRootPtr)(const Matrix&) = &Device::setRoot; - docstring - = command::docCommandVoid1("Set the root position.", - "matrix homogeneous"); + docstring = command::docCommandVoid1("Set the root position.", + "matrix homogeneous"); addCommand("setRoot", command::makeCommandVoid1(*this,setRootPtr, docstring)); - /* Second Order Integration set. */ - docstring = - "\n" - " Set the position calculous starting from \n" - " acceleration measure instead of velocity \n" - "\n"; - - addCommand - ("setSecondOrderIntegration", - command::makeCommandVoid0 - (*this,&Device::setSecondOrderIntegration, - docstring)); - - /* Display information */ - docstring = - "\n" - " Display information on device \n" - "\n"; - addCommand - ("display", - command::makeCommandVoid0 - (*this,&Device::cmdDisplay,docstring)); - /* SET of control input type. */ - docstring = - "\n" - " Set the type of control input which can be \n" - " acceleration, velocity, or position\n" - "\n"; + /* SET of SoT control input type per joint */ + addCommand("setSoTControlType", + command::makeCommandVoid2(*this,&Device::setSoTControlType, + command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", + "Joint name", + "Control type: [TORQUE|ACCELERATION|VELOCITY|POSITION]") + )); - addCommand("setControlInputType", - new command::Setter - (*this, &Device::setControlInputType, docstring)); + /* SET of HW control input type per joint */ + addCommand("setHWControlType", + command::makeCommandVoid2(*this,&Device::setHWControlType, + command::docCommandVoid2 ("Set HW control input type per joint", + "Joint name", + "Control type: [TORQUE|ACCELERATION|VELOCITY|POSITION]") + )); + docstring = "\n" " Enable/Disable sanity checks\n" @@ -204,35 +165,6 @@ Device( const std::string& n ) new command::Setter (*this, &Device::setSanityCheck, docstring)); - addCommand("setPositionBounds", - command::makeCommandVoid2 - (*this,&Device::setPositionBounds, - command::docCommandVoid2 - ("Set robot position bounds", - "vector: lower bounds", - "vector: upper bounds"))); - - addCommand("setVelocityBounds", - command::makeCommandVoid2 - (*this,&Device::setVelocityBounds, - command::docCommandVoid2 - ("Set robot velocity bounds", - "vector: lower bounds", - "vector: upper bounds"))); - - addCommand("setTorqueBounds", - command::makeCommandVoid2 - (*this,&Device::setTorqueBounds, - command::docCommandVoid2 - ("Set robot torque bounds", - "vector: lower bounds", - "vector: upper bounds"))); - - addCommand("getTimeStep", - command::makeDirectGetter - (*this, &this->timestep_, - command::docDirectGetter - ("Time step", "double"))); // Handle commands and signals called in a synchronous way. periodicCallBefore_.addSpecificCommands(*this, commandMap, "before."); @@ -241,205 +173,104 @@ Device( const std::string& n ) } } -void Device:: -setStateSize( const unsigned int& size ) +void Device::setState( const Vector& st ) { - state_.resize(size); state_.fill( .0 ); - stateSOUT .setConstant( state_ ); - previousControlSOUT.setConstant( state_ ); - pseudoTorqueSOUT.setConstant( state_ ); - motorcontrolSOUT .setConstant( state_ ); - - Device::setVelocitySize(size); - - Vector zmp(3); zmp.fill( .0 ); - ZMPPreviousControllerSOUT .setConstant( zmp ); + updateControl(st); + stateSOUT_ .setConstant( control_ ); + motorcontrolSOUT_ .setConstant( control_ ); } -void Device:: -setVelocitySize( const unsigned int& size ) -{ - velocity_.resize(size); - velocity_.fill(.0); - velocitySOUT.setConstant( velocity_ ); -} - -void Device:: -setState( const Vector& st ) -{ - if (sanityCheck_) { - const Vector::Index& s = st.size(); - switch (controlInputType_) { - case CONTROL_INPUT_TWO_INTEGRATION: - dgRTLOG() - << "Sanity check for this control is not well supported. " - "In order to make it work, use pinocchio and the contact forces " - "to estimate the joint torques for the given acceleration.\n"; - if ( s != lowerTorque_.size() - || s != upperTorque_.size() ) - throw std::invalid_argument ("Upper and/or lower torque bounds " - "do not match state size. Set them first with setTorqueBounds"); - case CONTROL_INPUT_ONE_INTEGRATION: - if ( s != lowerVelocity_.size() - || s != upperVelocity_.size() ) - throw std::invalid_argument ("Upper and/or lower velocity bounds " - "do not match state size." - " Set them first with setVelocityBounds"); - case CONTROL_INPUT_NO_INTEGRATION: - break; - default: - throw std::invalid_argument ("Invalid control mode type."); - } - } - state_ = st; - stateSOUT .setConstant( state_ ); - motorcontrolSOUT .setConstant( state_ ); -} -void Device:: -setVelocity( const Vector& vel ) -{ - velocity_ = vel; - velocitySOUT .setConstant( velocity_ ); -} - -void Device:: -setRoot( const Matrix & root ) +void Device::setRoot( const Matrix & root ) { Eigen::Matrix4d _matrix4d(root); MatrixHomogeneous _root(_matrix4d); setRoot( _root ); } -void Device:: -setRoot( const MatrixHomogeneous & worldMwaist ) +void Device::setRoot( const MatrixHomogeneous & worldMwaist ) { VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse(); - Vector q = state_; - q = worldMwaist.translation(); // abusive ... but working. - for( unsigned int i=0;i<3;++i ) q(i+3) = r(i); + for( unsigned int i=0;i<3;++i ) + { + control_(i) = worldMwaist.translation()(i); + control_(i+3) = r(i); + } } -void Device:: -setSecondOrderIntegration() + +void Device::setSanityCheck(const bool & enableCheck) { - controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION; - velocity_.resize(state_.size()); - velocity_.setZero(); - velocitySOUT.setConstant( velocity_ ); + sanityCheck_ = enableCheck; } -void Device:: -setNoIntegration() +void Device::setControlType(const std::string &strCtrlType, ControlType &aCtrlType) { - controlInputType_ = CONTROL_INPUT_NO_INTEGRATION; - velocity_.resize(state_.size()); - velocity_.setZero(); - velocitySOUT.setConstant( velocity_ ); + for(int j=0;j<4;j++) + { + if (strCtrlType==ControlType_s[j]){ + aCtrlType = (ControlType)j; + } + } } -void Device:: -setControlInputType(const std::string& cit) +void Device::setSoTControlType(const std::string &jointNames, const std::string &strCtrlType) { - for(int i=0; igetName(); + // model_.initTree(urdfTree); + // // details::parseRootTree(urdfTree->getRoot(),model_,false); + // } + if (!model_) + { + dgRTLOG() + << "The XML stream does not contain a valid URDF model." << std::endl; + return; } - if (upper.size() != state_.size()) { - oss << "Lower bound size should be " << state_.size(); - throw std::invalid_argument (oss.str()); + + /// Build the map between urdf file and the alphabetical order. + std::vector< ::urdf::LinkSharedPtr > urdf_links; + model_->getLinks(urdf_links); + for (unsigned j=0; j child_joints = urdf_links[j]->child_joints; + urdf_joints_.insert(urdf_joints_.end(), boost::make_move_iterator(child_joints.begin()), + boost::make_move_iterator(child_joints.end())); } - lowerTorque_ = lower; - upperTorque_ = upper; + + std::cout << "urdf_joints_.size(): " << urdf_joints_.size() << std::endl; + for(unsigned int i=0;iname].urdf_index=i; + std::cout << "jointDevices_ index: " << i + << " model_.names[i]: " << urdf_joints_[i]->name + << std::endl; + } + // Initialize urdf file vector. + control_.resize(urdf_joints_.size()); } -void Device:: -increment( const double & dt ) +void Device::increment() { - int time = stateSOUT.getTime(); + int time = stateSOUT_.getTime(); sotDEBUG(25) << "Time : " << time << std::endl; // Run Synchronous commands and evaluate signals outside the main @@ -467,33 +298,17 @@ increment( const double & dt ) << " running periodical commands (before)" << std::endl; } - /* Force the recomputation of the control. */ controlSIN( time ); - sotDEBUG(25) << "u" <second.control_index; + int u_index = it_control_type->second.urdf_index; + + if (lctl_index==-1) + { + if (debug_mode_>1) + { + std::cerr << "No control index for joint " + << urdf_joints_[u_index]->name << std::endl; + } + break; + } + + if (u_index!=-1) + { + control_[u_index] = controlIN[lctl_index]; + + // Position from control is directly provided. + if (it_control_type->second.SoTcontrol==POSITION) + { + double lowerLim = urdf_joints_[u_index]->limits->lower; + double upperLim = urdf_joints_[u_index]->limits->upper; + if (control_[u_index] < lowerLim) + { + control_[u_index] = lowerLim; + } + else if (control_[u_index] > upperLim) + { + control_[u_index] = upperLim; + } + } + } + } } -#define CHECK_BOUNDS(val, lower, upper, what) \ - for (int i = 0; i < val.size(); ++i) { \ - double old = val(i); \ - if (saturateBounds (val(i), lower(i), upper(i))) \ - dgRTLOG () << "Robot " what " bound violation at DoF " << i << \ - ": requested " << old << " but set " << val(i) << '\n'; \ + +int Device::ParseYAMLString(const std::string & aYamlString) +{ + YAML::Node map_global = YAML::Load(aYamlString); + + YAML::Node map_sot_controller = map_global["sot_controller"]; + + for (YAML::const_iterator it=map_sot_controller.begin(); + it!=map_sot_controller.end(); + it++) + { + if (debug_mode_>1) + { + std::cout << "key:" << it->first.as() << std::endl; + } + std::string name_elt_of_sot_controller = it->first.as(); + + YAML::Node elt_of_sot_controller = it->second; + + if (name_elt_of_sot_controller=="map_hardware_sot_control") + { + int r=ParseYAMLMapHardware(elt_of_sot_controller); + if (r<0) return r; + } + else if (name_elt_of_sot_controller=="sensors") + { + int r=ParseYAMLSensors(elt_of_sot_controller); + if (r<0) return r; + } } + UpdateSignals(); + return 0; +} -void Device::integrate( const double & dt ) +int Device::ParseYAMLJointSensor(std::string & joint_name, YAML::Node &aJointSensor) { - const Vector & controlIN = controlSIN.accessCopy(); + JointSoTHWControlType & aJointSoTHWControlType = jointDevices_[joint_name]; - if (sanityCheck_ && controlIN.hasNaN()) + if (debug_mode_>1) { - dgRTLOG () << "Device::integrate: Control has NaN values: " << '\n' - << controlIN.transpose() << '\n'; - return; + std::cout << "JointSensor for " << joint_name << std::endl; } + + for (std::size_t i=0;i(); + if (debug_mode_>1) + { + std::cout << "Found " << aSensor<< std::endl; + } + if (aSensor=="temperature") + { + aJointSoTHWControlType.temperature_index = temperature_index_; + temperature_index_++; + } + else if (aSensor=="velocity") + { + aJointSoTHWControlType.velocity_index = velocity_index_; + velocity_index_++; + } + else if (aSensor=="current") + { + aJointSoTHWControlType.current_index = current_index_; + current_index_++; + } + else if (aSensor=="torque") + { + aJointSoTHWControlType.torque_index = torque_index_; + torque_index_++; + } + else if (aSensor=="joint_angles") + { + aJointSoTHWControlType.joint_angle_index = joint_angle_index_; + joint_angle_index_++; + } + else if (aSensor=="motor_angles") + { + aJointSoTHWControlType.motor_angle_index = motor_angle_index_; + motor_angle_index_++; + } + } + return 0; +} - if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION) + +int Device::ParseYAMLMapHardware(YAML::Node & map_hs_control) +{ + if (debug_mode_>1) { - assert(state_.size()==controlIN.size()+6); - state_.tail(controlIN.size()) = controlIN; - return; + std::cout << "map_hs_control.size(): " + << map_hs_control.size() << std::endl; + std::cout << map_hs_control << std::endl; } + unsigned int i=0; + for (YAML::const_iterator it=map_hs_control.begin(); + it!=map_hs_control.end(); + it++) + { + if (debug_mode_>1) + { + std::cout << i << " " << std::endl; + std::cout << "key:" << it->first.as() << std::endl; + } - if( vel_control_.size() == 0 ) - vel_control_ = Vector::Zero(controlIN.size()); + std::string jointName = it->first.as(); + YAML::Node aNode = it->second; - // If control size is state size - 6, integrate joint angles, - // if control and state are of same size, integrate 6 first degrees of - // freedom as a translation and roll pitch yaw. + if (debug_mode_>1) + { + std::cout << "Type of value: " << aNode.Type() << std::endl; + } - if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION) + for (YAML::const_iterator it2=aNode.begin(); + it2!=aNode.end(); + it2++) + { + std::string aKey = it2->first.as(); + if (debug_mode_>1) + { + std::cout << "-- key:" << aKey << std::endl; + } + if (aKey=="hw") + { + std::string value = it2->second.as(); + if (debug_mode_>1) + { + std::cout << "-- Value: " << value << std::endl; + } + setHWControlType(jointName,value); + } + else if (aKey=="sot") + { + std::string value = it2->second.as(); + if (debug_mode_>1) + { + std::cout << "-- Value: " << value << std::endl; + } + setSoTControlType(jointName,value); + } + else if (aKey=="controlPos") + { + unsigned int index= it2->second.as(); + if (debug_mode_>1) + { + std::cout << "-- index: " << index << std::endl; + } + setControlPos(jointName,index); + } + else if (aKey=="sensors") + { + YAML::Node aNode = it2->second; + if (debug_mode_>1) + { + std::cout << "-- type: " << aNode.Type() << std::endl; + } + ParseYAMLJointSensor(jointName,aNode); + } + } + i++; + } + return 0; +} + +/* Sensor signals */ +int Device::ParseYAMLSensors(YAML::Node &map_sensors) +{ + if (map_sensors.IsNull()) { - // TODO check acceleration - // Position increment - vel_control_ = velocity_.tail(controlIN.size()) + (0.5*dt)*controlIN; - // Velocity integration. - velocity_.tail(controlIN.size()) += controlIN*dt; + std::cerr << "Device::ParseYAMLString: No sensor detected in YamlString " << std::endl; + return -1; } - else + + for (YAML::const_iterator it=map_sensors.begin(); + it!=map_sensors.end(); + it++) { - vel_control_ = controlIN; + if (debug_mode_>1) + { + std::cout << "sensor_type:" << it->first.as() << std::endl; + } + std::string sensor_type = it->first.as(); + + YAML::Node aNode = it->second; + if (debug_mode_>1) + { + std::cout << "Type of value: " << aNode.Type() << std::endl; + } + + // Iterates over types of node. + for (YAML::const_iterator it2=aNode.begin(); + it2!=aNode.end(); + it2++) + { + std::string sensor_name = it2->first.as(); + if (debug_mode_>1) + { + std::cout << "-- sensor_name:" << sensor_name << std::endl; + } + + if (sensor_type=="force_torque") + { + std::string force_sensor_name = it2->second.as(); + if (debug_mode_>1) + { + std::cout << "-- force_sensor_name: " << force_sensor_name << std::endl; + } + CreateAForceSignal(force_sensor_name); + } + else if (sensor_type=="imu") + { + std::string imu_sensor_name = it2->second.as(); + if (debug_mode_>1) + { + std::cout << "-- Value: " << imu_sensor_name << std::endl; + } + CreateAnImuSignal(imu_sensor_name); + } + else + { + std::cerr << "The sensor type " << sensor_type + << " is not recognized" << std::endl; + } + } } + return 0; +} + + +void Device::CreateAForceSignal(const std::string & force_sensor_name) +{ + dynamicgraph::Signal * aForceSOUT_; + /* --- SIGNALS --- */ + aForceSOUT_ = new Signal("Device("+getName()+")::output(vector6)::" + + force_sensor_name); + forcesSOUT_.push_back(aForceSOUT_); + signalRegistration(*aForceSOUT_); +} - // Velocity bounds check - if (sanityCheck_) { - CHECK_BOUNDS(velocity_, lowerVelocity_, upperVelocity_, "velocity"); +void Device::CreateAnImuSignal(const std::string &imu_sensor_name) +{ + IMUSOUT * anImuSOUT_; + /* --- SIGNALS --- */ + anImuSOUT_ = new IMUSOUT(imu_sensor_name,getName()); + imuSOUT_.push_back(anImuSOUT_); + signalRegistration(anImuSOUT_->attitudeSOUT + << anImuSOUT_->accelerometerSOUT + << anImuSOUT_->gyrometerSOUT); +} + + +int Device::UpdateSignals() +{ + if ((torque_index_!=0) && (pseudoTorqueSOUT_!=0)) + { + pseudoTorqueSOUT_ = new Signal("Device("+getName()+")::output(vector)::ptorque"); + } + + if ((current_index_!=0) && (currentsSOUT_!=0)) + { + currentsSOUT_ = new Signal("Device("+getName()+")::output(vector)::currents"); } - // Freeflyer integration - if (vel_control_.size() == state_.size()) { - integrateRollPitchYaw(state_, vel_control_, dt); + if ((temperature_index_!=0) && (temperatureSOUT_!=0)) + { + temperatureSOUT_ = new Signal("Device("+getName()+")::output(vector)::temperatures"); } - // Position integration - state_.tail(controlIN.size()) += vel_control_ * dt; + if ((motor_angle_index_!=0) && (motor_anglesSOUT_!=0)) + { + motor_anglesSOUT_ = new Signal("Device("+getName()+")::output(vector)::motor_angles"); + } - // Position bounds check - if (sanityCheck_) { - CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position"); + if ((joint_angle_index_!=0) && (joint_anglesSOUT_!=0)) + { + joint_anglesSOUT_ = new Signal("Device("+getName()+")::output(vector)::joint_angles"); } + + return 0; } + + /* --- DISPLAY ------------------------------------------------------------ */ void Device::display ( std::ostream& os ) const { - os << name <<": "< &SensorsIn, int t) { - std::cout << name <<": "< dgforces; + + sotDEBUGIN(15); + map::iterator it; + it = SensorsIn.find("forces"); + if (it!=SensorsIn.end()) + { + // Implements force recollection. + const vector& forcesIn = it->second.getValues(); + for(std::size_t i=0;isetConstant(dgforces); + forcesSOUT_[i]->setTime (t); + } + } + sotDEBUGIN(15); } + +void Device::setSensorsIMU(map &SensorsIn, int t) +{ + Eigen::Matrix aVector3d; + + map::iterator it; + + //TODO: Confirm if this can be made quaternion + for(std::size_t k=0;k& attitude = it->second.getValues (); + Eigen::Matrix pose; + + for (unsigned int i = 0; i < 3; ++i) + { + for (unsigned int j = 0; j < 3; ++j) + { + pose (i, j) = attitude[i * 3 + j]; + } + } + imuSOUT_[k]->attitudeSOUT.setConstant (pose); + imuSOUT_[k]->attitudeSOUT.setTime (t); + } + + it = SensorsIn.find("accelerometer_0"); + if (it!=SensorsIn.end()) + { + const vector& accelerometer = + SensorsIn ["accelerometer_0"].getValues (); + for (std::size_t i=0; i<3; ++i) + { + aVector3d(i) = accelerometer [i]; + } + imuSOUT_[k]->accelerometerSOUT.setConstant (aVector3d); + imuSOUT_[k]->accelerometerSOUT.setTime (t); + } + + it = SensorsIn.find("gyrometer_0"); + if (it!=SensorsIn.end()) + { + const vector& gyrometer = SensorsIn ["gyrometer_0"].getValues (); + for (std::size_t i=0; i<3; ++i) + { + aVector3d(i) = gyrometer [i]; + } + imuSOUT_[k]->gyrometerSOUT.setConstant (aVector3d); + imuSOUT_[k]->gyrometerSOUT.setTime (t); + } + } +} + +void Device::setSensorsEncoders(map &SensorsIn, int t) +{ + dg::Vector dgRobotState, motor_angles, joint_angles; + map::iterator it; + + if (motor_anglesSOUT_!=0) + { + it = SensorsIn.find("motor-angles"); + if (it!=SensorsIn.end()) + { + const vector& anglesIn = it->second.getValues(); + dgRobotState.resize (anglesIn.size () + 6); + motor_angles.resize(anglesIn.size ()); + for (unsigned i = 0; i < 6; ++i) + { + dgRobotState (i) = 0.; + } + for (unsigned i = 0; i < anglesIn.size(); ++i) + { + dgRobotState (i + 6) = anglesIn[i]; + motor_angles(i)= anglesIn[i]; + } + robotState_.setConstant(dgRobotState); + robotState_.setTime(t); + motor_anglesSOUT_->setConstant(motor_angles); + motor_anglesSOUT_->setTime(t); + } + } + + if (joint_anglesSOUT_!=0) + { + it = SensorsIn.find("joint-angles"); + if (it!=SensorsIn.end()) + { + const vector& joint_anglesIn = it->second.getValues(); + joint_angles.resize (joint_anglesIn.size () ); + for (unsigned i = 0; i < joint_anglesIn.size(); ++i) + { + joint_angles (i) = joint_anglesIn[i]; + } + joint_anglesSOUT_->setConstant(joint_angles); + joint_anglesSOUT_->setTime(t); + } + } +} + +void Device::setSensorsVelocities(map &SensorsIn, int t) +{ + dg::Vector dgRobotVelocity; + + map::iterator it; + + it = SensorsIn.find("velocities"); + if (it!=SensorsIn.end()) + { + const vector& velocitiesIn = it->second.getValues(); + dgRobotVelocity.resize (velocitiesIn.size () + 6); + for (unsigned i = 0; i < 6; ++i) + { + dgRobotVelocity (i) = 0.; + } + for (unsigned i = 0; i < velocitiesIn.size(); ++i) + { + dgRobotVelocity (i + 6) = velocitiesIn[i]; + } + robotVelocity_.setConstant(dgRobotVelocity); + robotVelocity_.setTime(t); + } +} + +void Device::setSensorsTorquesCurrents(map &SensorsIn, int t) +{ + dg::Vector torques,currents; + + map::iterator it; + + if (pseudoTorqueSOUT_!=0) + { + it = SensorsIn.find("torques"); + if (it!=SensorsIn.end()) + { + const std::vector& vtorques = SensorsIn["torques"].getValues(); + torques.resize(vtorques.size()); + for(std::size_t i = 0; i < vtorques.size(); ++i) + { + torques (i) = vtorques [i]; + } + pseudoTorqueSOUT_->setConstant(torques); + pseudoTorqueSOUT_->setTime(t); + } + } + + if (currentsSOUT_!=0) + { + it = SensorsIn.find("currents"); + if (it!=SensorsIn.end()) + { + const std::vector& vcurrents = SensorsIn["currents"].getValues(); + currents.resize(vcurrents.size()); + for(std::size_t i = 0; i < vcurrents.size(); ++i) + { + currents (i) = vcurrents[i]; + } + currentsSOUT_->setConstant(currents); + currentsSOUT_->setTime(t); + } + } +} + +void Device::setSensorsGains(map &SensorsIn, int t) +{ + dg::Vector p_gains, d_gains; + + map::iterator it; + if (p_gainsSOUT_!=0) + { + it = SensorsIn.find("p_gains"); + if (it!=SensorsIn.end()) + { + const std::vector& vp_gains = SensorsIn["p_gains"].getValues(); + p_gains.resize(vp_gains.size()); + for(std::size_t i = 0; i < vp_gains.size(); ++i) + { + p_gains (i) = vp_gains[i]; + } + p_gainsSOUT_->setConstant(p_gains); + p_gainsSOUT_->setTime(t); + } + } + + if (d_gainsSOUT_!=0) + { + it = SensorsIn.find("d_gains"); + if (it!=SensorsIn.end()) + { + const std::vector& vd_gains = SensorsIn["d_gains"].getValues(); + d_gains.resize(vd_gains.size()); + for(std::size_t i = 0; i < vd_gains.size(); ++i) + { + d_gains (i) = vd_gains[i]; + } + d_gainsSOUT_->setConstant(d_gains); + d_gainsSOUT_->setTime(t); + } + } +} + +void Device::setSensors(map &SensorsIn) +{ + sotDEBUGIN(25) ; + int t = stateSOUT_.getTime () + 1; + + setSensorsForce(SensorsIn,t); + setSensorsIMU(SensorsIn,t); + setSensorsEncoders(SensorsIn,t); + setSensorsVelocities(SensorsIn,t); + setSensorsTorquesCurrents(SensorsIn,t); + setSensorsGains(SensorsIn,t); + + sotDEBUGOUT(25); +} + +void Device::setupSetSensors(map &SensorsIn) +{ + setSensors (SensorsIn); +} + +void Device::nominalSetSensors(map &SensorsIn) +{ + setSensors (SensorsIn); +} + + +void Device::cleanupSetSensors(map &SensorsIn) +{ + setSensors (SensorsIn); +} + +void Device::getControl(map &controlOut) +{ + ODEBUG5FULL("start"); + sotDEBUGIN(25) ; + const Vector & controlIN = controlSIN.accessCopy(); + vector lcontrolOut; + lcontrolOut.resize(controlIN.size()); + + // Integrate control + increment(); + sotDEBUG (25) << "control = " << control_ << std::endl; + + ODEBUG5FULL("control = "<< control_); + + // Specify the joint values for the controller. + JointSHWControlType_iterator it_control_type; + for(it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) + { + int lctl_index = it_control_type->second.control_index; + if (it_control_type->second.HWcontrol==TORQUE) + { + lcontrolOut[lctl_index] = controlIN[lctl_index]; + } + else if (it_control_type->second.HWcontrol==POSITION) + { + int u_index = it_control_type->second.urdf_index; + lcontrolOut[lctl_index] = control_[u_index]; + } + } + + controlOut["control"].setValues(lcontrolOut); + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} + diff --git a/src/tools/robot-simu.cpp b/src/tools/robot-simu.cpp index 64672ea01..008754995 100644 --- a/src/tools/robot-simu.cpp +++ b/src/tools/robot-simu.cpp @@ -11,37 +11,31 @@ #include "sot/core/robot-simu.hh" namespace dynamicgraph { - namespace sot { - DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu,"RobotSimu"); +namespace sot { +DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(RobotSimu, "RobotSimu"); - RobotSimu::RobotSimu(const std::string& inName) : - Device(inName) - { - using namespace dynamicgraph::command; - std::string docstring; - /* Command increment. */ - docstring = - "\n" - " Integrate dynamics for time step provided as input\n" - "\n" - " take one floating point number as input\n" - "\n"; - addCommand("increment", - command::makeCommandVoid1((Device&)*this, - &Device::increment, docstring)); +RobotSimu::RobotSimu(const std::string& inName) : + Device(inName) { + using namespace dynamicgraph::command; + std::string docstring; + /* Command increment. */ + addCommand("increment", + command::makeCommandVoid0((Device&)*this, + &Device::increment, + command::docCommandVoid0("Increment the dynamic: check the control and run synchronous commands"))); - /* Set Time step. */ - docstring = - "\n" - " Set the time step provided\n" - "\n" - " take one floating point number as input\n" - "\n"; - addCommand("setTimeStep", - makeDirectSetter (*this, &this->timestep_, - docstring)); + /* Set Time step. */ + docstring = + "\n" + " Set the time step provided\n" + "\n" + " take one floating point number as input\n" + "\n"; + addCommand("setTimeStep", + makeDirectSetter (*this, &this->timestep_, + docstring)); - } - } // namespace sot +} +} // namespace sot } // namespace dynamicgraph diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt index 739a8778e..9b3a43fb2 100644 --- a/unitTesting/CMakeLists.txt +++ b/unitTesting/CMakeLists.txt @@ -90,6 +90,7 @@ SET (tests tools/test_mailbox tools/test_matrix tools/test_robot_utils + tools/test_device math/matrix-twist math/matrix-homogeneous diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_device.cpp new file mode 100644 index 000000000..0d2f7954d --- /dev/null +++ b/unitTesting/tools/test_device.cpp @@ -0,0 +1,392 @@ +/* + * Copyright 2018, + * Olivier Stasse, + * + * CNRS + * See LICENSE.txt + * + */ + +#include +#include +#include + +#ifndef WIN32 +#include +#endif + +using namespace std; + +#include +#include +#include + +#include +#include + + +#define BOOST_TEST_MODULE test-device + +void CreateYAMLFILE() { + YAML::Emitter yaml_out; + YAML::Node aNode, yn_map_hw_sot_c, yn_map_sensors; + yn_map_hw_sot_c = aNode["map_hardware_sot_control"]; + yn_map_sensors = aNode["sensors"]; + /* + yn_map_hw_sot_c["waist"]; + yn_map_hw_sot_c["waist"]["hw"] = "POSITION"; + yn_map_hw_sot_c["waist"]["sot"] = "POSITION"; + yn_map_hw_sot_c["waist"]["controlPos"] = 0; + yn_map_hw_sot_c["waist"]["sensors"] = ""; + */ + yn_map_hw_sot_c["LLEG_HIP_P"]; + yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = 6; + yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LLEG_HIP_R"]; + yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = 7; + yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LLEG_HIP_Y"]; + yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = 8; + yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LLEG_KNEE"]; + yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = 9; + yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LLEG_ANKLE_P"]; + yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = 10; + yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LLEG_ANKLE_R"]; + yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = 11; + yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RLEG_HIP_P"]; + yn_map_hw_sot_c["RLEG_HIP_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RLEG_HIP_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = 12; + yn_map_hw_sot_c["RLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RLEG_HIP_R"]; + yn_map_hw_sot_c["RLEG_HIP_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RLEG_HIP_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RLEG_HIP_R"]["controlPos"] = 13; + yn_map_hw_sot_c["RLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RLEG_HIP_Y"]; + yn_map_hw_sot_c["RLEG_HIP_Y"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RLEG_HIP_Y"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = 14; + yn_map_hw_sot_c["RLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RLEG_KNEE"]; + yn_map_hw_sot_c["RLEG_KNEE"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RLEG_KNEE"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = 15; + yn_map_hw_sot_c["RLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RLEG_ANKLE_P"]; + yn_map_hw_sot_c["RLEG_ANKLE_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RLEG_ANKLE_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = 16; + yn_map_hw_sot_c["RLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RLEG_ANKLE_R"]; + yn_map_hw_sot_c["RLEG_ANKLE_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RLEG_ANKLE_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = 17; + yn_map_hw_sot_c["RLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["WAIST_P"]; + yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["WAIST_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["WAIST_P"]["controlPos"] = 18; + yn_map_hw_sot_c["WAIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["WAIST_R"]; + yn_map_hw_sot_c["WAIST_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["WAIST_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["WAIST_R"]["controlPos"] = 19; + yn_map_hw_sot_c["WAIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["CHEST"]; + yn_map_hw_sot_c["CHEST"]["hw"] = "POSITION"; + yn_map_hw_sot_c["CHEST"]["sot"] = "POSITION"; + yn_map_hw_sot_c["CHEST"]["controlPos"] = 20; + yn_map_hw_sot_c["CHEST"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_SHOULDER_P"]; + yn_map_hw_sot_c["RARM_SHOULDER_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_SHOULDER_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = 21; + yn_map_hw_sot_c["RARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_SHOULDER_R"]; + yn_map_hw_sot_c["RARM_SHOULDER_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_SHOULDER_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = 22; + yn_map_hw_sot_c["RARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_SHOULDER_Y"]; + yn_map_hw_sot_c["RARM_SHOULDER_Y"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = 23; + yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_ELBOW"]; + yn_map_hw_sot_c["RARM_ELBOW"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_ELBOW"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = 24; + yn_map_hw_sot_c["RARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_WRIST_Y"]; + yn_map_hw_sot_c["RARM_WRIST_Y"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_WRIST_Y"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = 25; + yn_map_hw_sot_c["RARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_WRIST_P"]; + yn_map_hw_sot_c["RARM_WRIST_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_WRIST_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = 26; + yn_map_hw_sot_c["RARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["RARM_WRIST_R"]; + yn_map_hw_sot_c["RARM_WRIST_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["RARM_WRIST_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = 27; + yn_map_hw_sot_c["RARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]"; + + yn_map_hw_sot_c["LARM_SHOULDER_P"]; + yn_map_hw_sot_c["LARM_SHOULDER_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_SHOULDER_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = 28; + yn_map_hw_sot_c["LARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LARM_SHOULDER_R"]; + yn_map_hw_sot_c["LARM_SHOULDER_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_SHOULDER_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = 29; + yn_map_hw_sot_c["LARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LARM_SHOULDER_Y"]; + yn_map_hw_sot_c["LARM_SHOULDER_Y"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = 30; + yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LARM_ELBOW"]; + yn_map_hw_sot_c["LARM_ELBOW"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_ELBOW"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = 31; + yn_map_hw_sot_c["LARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LARM_WRIST_Y"]; + yn_map_hw_sot_c["LARM_WRIST_Y"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_WRIST_Y"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = 32; + yn_map_hw_sot_c["LARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LARM_WRIST_P"]; + yn_map_hw_sot_c["LARM_WRIST_P"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_WRIST_P"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = 33; + yn_map_hw_sot_c["LARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; + + yn_map_hw_sot_c["LARM_WRIST_R"]; + yn_map_hw_sot_c["LARM_WRIST_R"]["hw"] = "POSITION"; + yn_map_hw_sot_c["LARM_WRIST_R"]["sot"] = "POSITION"; + yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = 34; + yn_map_hw_sot_c["LARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]"; + + yn_map_sensors["force_torque"]; + yn_map_sensors["force_torque"]["left_ankle_ft"]; + yn_map_sensors["force_torque"]["left_ankle_ft"]["sensor_joint"] = "LLEG_ANKLE_R"; + yn_map_sensors["force_torque"]["left_ankle_ft"]["frame"] = "LLEG_LINK_6"; + + yn_map_sensors["force_torque"]; + yn_map_sensors["force_torque"]["right_ankle_ft"]; + yn_map_sensors["force_torque"]["right_ankle_ft"]["sensor_joint"] = "RLEG_ANKLE_R"; + yn_map_sensors["force_torque"]["right_ankle_ft"]["frame"] = "RLEG_LINK_6"; + + yn_map_sensors["force_torque"]; + yn_map_sensors["force_torque"]["left_wrist_ft"]; + yn_map_sensors["force_torque"]["left_wrist_ft"]["sensor_joint"] = "LLEG_WRIST_R"; + yn_map_sensors["force_torque"]["left_wrist_ft"]["frame"] = "LARM_LINK_7"; + + yn_map_sensors["force_torque"]; + yn_map_sensors["force_torque"]["right_wrist_ft"]; + yn_map_sensors["force_torque"]["right_wrist_ft"]["sensor_joint"] = "RLEG_WRIST_R"; + yn_map_sensors["force_torque"]["right_wrist_ft"]["frame"] = "RARM_LINK_7"; + + + YAML::Node yn_imu = yn_map_sensors["imu"]; + yn_imu["base_imu"]; + yn_imu["base_imu"]["frame"] = "LARM_LINK6"; + yn_imu["base_imu"]["gazebo_sensor_J"] = "LARM_LINK6"; + + ofstream of; + of.open("map_hs_sot_gen.yaml", ios::out); + if (of.is_open()) { + of << aNode; + } + of.close(); +} + +int ReadYAMLFILE(dg::sot::Device &aDevice, unsigned int debug_mode) { + std::string yaml_file = "map_hs_sot_gen.yaml"; + YAML::Node map_hs_sot = YAML::LoadFile(yaml_file); + + if (map_hs_sot.IsNull()) { + std::cerr << "Unable to read " << yaml_file << std::endl; + return -1; + } + if (debug_mode > 0) + std::cout << "Reading file : " << yaml_file << std::endl; + YAML::Node map_hs_control = map_hs_sot["map_hardware_sot_control"]; + if (debug_mode > 1) { + std::cout << "map_hs_control.size(): " + << map_hs_control.size() << std::endl; + std::cout << map_hs_control << std::endl; + } + unsigned int i = 0; + for (YAML::const_iterator it = map_hs_control.begin(); + it != map_hs_control.end(); + it++) { + if (debug_mode > 1) { + std::cout << i << " " << std::endl; + std::cout << "key:" << it->first.as() << std::endl; + } + std::string jointName = it->first.as(); + + YAML::Node aNode = it->second; + if (debug_mode > 1) + std::cout << "Type of value: " << aNode.Type() << std::endl; + + for (YAML::const_iterator it2 = aNode.begin(); + it2 != aNode.end(); + it2++) + + { + std::string aKey = it2->first.as(); + if (debug_mode > 1) + std::cout << "-- key:" << aKey << std::endl; + + if (aKey == "hw") { + std::string value = it2->second.as(); + if (debug_mode > 1) + std::cout << "-- Value: " << value << std::endl; + aDevice.setHWControlType(jointName, value); + } else if (aKey == "sot") { + std::string value = it2->second.as(); + if (debug_mode > 1) + std::cout << "-- Value: " << value << std::endl; + aDevice.setSoTControlType(jointName, value); + } else if (aKey == "controlPos") { + unsigned int index = it2->second.as(); + if (debug_mode > 1) + std::cout << "-- index: " << index << std::endl; + aDevice.setControlPos(jointName, index); + } + } + i++; + } + return 0; +} + +namespace dg = dynamicgraph; +int main(int, char **) { + unsigned int debug_mode = 5; + + 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.setURDFModel(robot_description); + CreateYAMLFILE(); + + if (ReadYAMLFILE(aDevice, debug_mode) < 0) + return -1; + + dg::Vector aState(29); + for (unsigned j = 0; j < aState.size(); j++) + aState(j) = 0.0; + aDevice.setState(aState); + + /// Fix constant vector for the control entry + dg::Vector aControlVector(35); + double dt = 0.005; + for (unsigned int i = 0; i < 35; i++) + aControlVector[i] = -0.5; + aDevice.controlSIN.setConstant(aControlVector); + + for (unsigned int i = 0; i < 2000; i++) + aDevice.increment(dt); + + const se3::Model & aModel = aDevice.getModel(); + + const dg::Vector & aPosition = aDevice.stateSOUT_(2001); + double diff = 0, ldiff; + + 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; + if (it_control_type->second.HWcontrol == dgsot::POSITION) + { + int u_index = it_control_type->second.urdf_index; + if (u_index != -1) + { + std::vector< ::urdf::LinkSharedPtr > urdf_links; + vector< ::urdf::JointSharedPtr > urdf_joints; + aModel.getLinks(urdf_links); + for (unsigned j=0; j child_joints = urdf_links[j]->child_joints; + urdf_joints.insert(urdf_joints.end(), boost::make_move_iterator(child_joints.begin()), + boost::make_move_iterator(child_joints.end())); + } + + double lowerLim = urdf_joints[u_index]->limits->lower; + ldiff = (aPosition[lpos_index] - lowerLim); + diff += ldiff; + std::cout << ldiff << " " << urdf_joints[u_index]->name << " " + << aPosition[lpos_index] << " " + << lowerLim << " " + << -urdf_joints[u_index]->limits->velocity + << std::endl; + } + } + } + + if (diff > 1e-3) + return -1; + return 0; +} From ead4cf9865d350cf640847506d74a99887052b4e Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 7 Aug 2019 13:35:18 +0200 Subject: [PATCH 02/43] [Device] Change parsing of yaml according to the config files End of the integration removal (remove stateSOUT) Add of force index Change the index of the control signal (defined by the yaml file with the key joint_names) --- include/sot/core/device.hh | 44 +++-- src/tools/device.cpp | 347 ++++++++++++++++++------------------- 2 files changed, 193 insertions(+), 198 deletions(-) diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 5cc5fa7b4..7a5981824 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -86,6 +86,9 @@ struct JointSoTHWControlType { /// Position of the joint in the torque vector int torque_index; + /// Position of the joint in the force vector + int force_index; + /// Position of the joint in the joint-angles vector int joint_angle_index; @@ -134,6 +137,8 @@ class SOT_CORE_EXPORT Device: public Entity /// Set integration time. void timeStep(double ts) { timestep_ = ts;} + /// Set debug mode. + void setDebugMode(int mode) { debug_mode_ = mode;} protected: /// \brief Current integration step. @@ -204,8 +209,6 @@ class SOT_CORE_EXPORT Device: public Entity /// \name Device current state. /// \{ - /// \brief Output integrated state from control. - dg::Signal stateSOUT_; /// \brief Output attitude provided by the hardware /*! \brief The current state of the robot from the command viewpoint. */ dg::Signal motorcontrolSOUT_; @@ -257,37 +260,40 @@ class SOT_CORE_EXPORT Device: public Entity /// \brief Provides to the robot the control information. void getControl(std::map &anglesOut); ///@} + protected: - void setControlType(const std::string &strCtrlType, ControlType &aCtrlType); - /// Compute the new control, from the given one. + /// \brief Compute the new control, from the given one. /// When the control is in position, checks that the position is within bounds. virtual void updateControl(const Vector & controlIN); + /// \name Signals related methods + ///@{ + /// \brief Creates a signal called Device(DeviceName)::output(vector6)::force_sensor_name + void CreateAForceSignal(const std::string & force_sensor_name); + /// \brief Creates a signal called Device(DeviceName)::output(vector6)::imu_sensor_name + void CreateAnImuSignal(const std::string & imu_sensor_name); + /// \name YAML related methods /// @{ - /// Parse YAML for mapping between hardware and sot + /// Parse YAML for mapping joint names between hardware and sot + /// starting from a YAML-cpp node. + int ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name); + + /// Parse YAML for mapping control modes between hardware and sot /// starting from a YAML-cpp node. - int ParseYAMLMapHardware(YAML::Node &map_hs_control); + int ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode); /// Parse YAML for sensors from a YAML-cpp node. int ParseYAMLSensors(YAML::Node &map_sensors); /// Parse YAML for joint sensors from YAML-cpp node. - int ParseYAMLJointSensor(std::string & joint_name, - YAML::Node &aJointSensors); + int ParseYAMLJointSensor(YAML::Node &aJointSensors); /// @} - - /// \name Signals related methods - ///@{ - /// \brief Creates a signal called Device(DeviceName)::output(vector6)::force_sensor_name - void CreateAForceSignal(const std::string & force_sensor_name); - /// \brief Creates a signal called Device(DeviceName)::output(vector6)::imu_sensor_name - void CreateAnImuSignal(const std::string & imu_sensor_name); - + /// \brief Creates signals based on the joints information parsed by the YAML string. int UpdateSignals(); @@ -323,7 +329,7 @@ class SOT_CORE_EXPORT Device: public Entity // Intermediate index when parsing YAML file. int temperature_index_, velocity_index_, current_index_, torque_index_, - joint_angle_index_, + force_index_, joint_angle_index_, motor_angle_index_ ; @@ -331,6 +337,10 @@ class SOT_CORE_EXPORT Device: public Entity const ::urdf::ModelInterfaceSharedPtr & getModel() { return model_; } + + const std::vector< ::urdf::JointSharedPtr > & getURDFJoints() { + return urdf_joints_; + } }; } // namespace sot } // namespace dynamicgraph diff --git a/src/tools/device.cpp b/src/tools/device.cpp index bc09ffa40..a1c741daf 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -61,6 +61,7 @@ JointSoTHWControlType::JointSoTHWControlType(): ,velocity_index(-1) ,current_index(-1) ,torque_index(-1) + ,force_index(-1) ,joint_angle_index(-1) ,motor_angle_index(-1) { @@ -88,7 +89,6 @@ Device::Device( const std::string& n ) ,control_(6) ,sanityCheck_(true) ,controlSIN( NULL,"Device("+n+")::input(double)::control" ) - ,stateSOUT_( "Device("+n+")::output(vector)::state" ) ,motorcontrolSOUT_ ( "Device("+n+")::output(vector)::motorcontrol" ) ,previousControlSOUT_( "Device("+n+")::output(vector)::previousControl" ) ,robotState_ ("Device("+n+")::output(vector)::robotState") @@ -106,6 +106,7 @@ Device::Device( const std::string& n ) ,velocity_index_(0) ,current_index_(0) ,torque_index_(0) + ,force_index_(0) ,joint_angle_index_(0) ,motor_angle_index_(0) @@ -113,12 +114,11 @@ Device::Device( const std::string& n ) forceZero6.fill (0); signalRegistration( controlSIN - << stateSOUT_ << robotState_ << robotVelocity_ << previousControlSOUT_ << motorcontrolSOUT_); - control_.fill(.0); stateSOUT_.setConstant( control_ ); + control_.fill(.0); motorcontrolSOUT_.setConstant( control_ ); /* --- Commands --- */ @@ -176,7 +176,6 @@ Device::Device( const std::string& n ) void Device::setState( const Vector& st ) { updateControl(st); - stateSOUT_ .setConstant( control_ ); motorcontrolSOUT_ .setConstant( control_ ); } @@ -233,12 +232,7 @@ void Device::setControlPos(const std::string &jointName, const unsigned & index) void Device::setURDFModel(const std::string &aURDFModel) { model_ = ::urdf::parseURDF(aURDFModel); - // if (urdfTree) - // { - // model_.name = urdfTree->getName(); - // model_.initTree(urdfTree); - // // details::parseRootTree(urdfTree->getRoot(),model_,false); - // } + if (!model_) { dgRTLOG() @@ -260,17 +254,18 @@ void Device::setURDFModel(const std::string &aURDFModel) for(unsigned int i=0;iname].urdf_index=i; - std::cout << "jointDevices_ index: " << i - << " model_.names[i]: " << urdf_joints_[i]->name - << std::endl; + if (debug_mode_>1) + { + std::cout << "jointDevices_ index: " << i + << " model_.names[i]: " << urdf_joints_[i]->name + << std::endl; + } } - // Initialize urdf file vector. - control_.resize(urdf_joints_.size()); } void Device::increment() { - int time = stateSOUT_.getTime(); + int time = motorcontrolSOUT_.getTime(); sotDEBUG(25) << "Time : " << time << std::endl; // Run Synchronous commands and evaluate signals outside the main @@ -302,12 +297,14 @@ void Device::increment() controlSIN( time ); const Vector & controlIN = controlSIN.accessCopy(); sotDEBUG(25) << "u" <name << std::endl; } break; - } - + } if (u_index!=-1) { - control_[u_index] = controlIN[lctl_index]; + control_[lctl_index] = controlIN[lctl_index]; // Position from control is directly provided. - if (it_control_type->second.SoTcontrol==POSITION) + if ((it_control_type->second.SoTcontrol==POSITION) && + (urdf_joints_[u_index]->limits)) { double lowerLim = urdf_joints_[u_index]->limits->lower; double upperLim = urdf_joints_[u_index]->limits->upper; - if (control_[u_index] < lowerLim) + if (control_[lctl_index] < lowerLim) { - control_[u_index] = lowerLim; + control_[lctl_index] = lowerLim; } - else if (control_[u_index] > upperLim) + else if (control_[lctl_index] > upperLim) { - control_[u_index] = upperLim; + control_[lctl_index] = upperLim; } } } } } - int Device::ParseYAMLString(const std::string & aYamlString) { YAML::Node map_global = YAML::Load(aYamlString); @@ -400,20 +398,31 @@ int Device::ParseYAMLString(const std::string & aYamlString) it!=map_sot_controller.end(); it++) { + std::string name_elt_of_sot_controller = it->first.as(); + if (debug_mode_>1) { - std::cout << "key:" << it->first.as() << std::endl; - } - std::string name_elt_of_sot_controller = it->first.as(); + std::cout << "key:" << name_elt_of_sot_controller << std::endl; + } YAML::Node elt_of_sot_controller = it->second; - if (name_elt_of_sot_controller=="map_hardware_sot_control") + if (name_elt_of_sot_controller=="joint_names") { - int r=ParseYAMLMapHardware(elt_of_sot_controller); + int r=ParseYAMLMapHardwareJointNames(elt_of_sot_controller); if (r<0) return r; } - else if (name_elt_of_sot_controller=="sensors") + else if (name_elt_of_sot_controller=="map_rc_to_sot_device") + { + int r=ParseYAMLJointSensor(elt_of_sot_controller); + if (r<0) return r; + } + else if (name_elt_of_sot_controller=="control_mode") + { + int r=ParseYAMLMapHardwareControlMode(elt_of_sot_controller); + if (r<0) return r; + } + else if (name_elt_of_sot_controller.find("sensor") != std::string::npos) { int r=ParseYAMLSensors(elt_of_sot_controller); if (r<0) return r; @@ -423,77 +432,119 @@ int Device::ParseYAMLString(const std::string & aYamlString) return 0; } -int Device::ParseYAMLJointSensor(std::string & joint_name, YAML::Node &aJointSensor) +int Device::ParseYAMLJointSensor(YAML::Node &aJointSensor) { - JointSoTHWControlType & aJointSoTHWControlType = jointDevices_[joint_name]; - - if (debug_mode_>1) - { - std::cout << "JointSensor for " << joint_name << std::endl; - } - - for (std::size_t i=0;i(); + std::string aSensor = it->first.as(); if (debug_mode_>1) { std::cout << "Found " << aSensor<< std::endl; } - if (aSensor=="temperature") - { - aJointSoTHWControlType.temperature_index = temperature_index_; - temperature_index_++; - } - else if (aSensor=="velocity") - { - aJointSoTHWControlType.velocity_index = velocity_index_; - velocity_index_++; - } - else if (aSensor=="current") - { - aJointSoTHWControlType.current_index = current_index_; - current_index_++; - } - else if (aSensor=="torque") + JointSHWControlType_iterator it_control_type; + for(it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) { - aJointSoTHWControlType.torque_index = torque_index_; - torque_index_++; - } - else if (aSensor=="joint_angles") - { - aJointSoTHWControlType.joint_angle_index = joint_angle_index_; - joint_angle_index_++; - } - else if (aSensor=="motor_angles") - { - aJointSoTHWControlType.motor_angle_index = motor_angle_index_; - motor_angle_index_++; + if (aSensor=="temperature") + { + it_control_type->second.temperature_index = temperature_index_; + temperature_index_++; + } + else if (aSensor=="velocities") + { + it_control_type->second.velocity_index = velocity_index_; + velocity_index_++; + } + else if (aSensor=="currents") + { + it_control_type->second.current_index = current_index_; + current_index_++; + } + else if (aSensor=="torques") + { + it_control_type->second.torque_index = torque_index_; + torque_index_++; + } + else if (aSensor=="forces") + { + it_control_type->second.force_index = force_index_; + force_index_++; + } + else if (aSensor=="joint_angles") + { + it_control_type->second.joint_angle_index = joint_angle_index_; + joint_angle_index_++; + } + else if (aSensor=="motor_angles") + { + it_control_type->second.motor_angle_index = motor_angle_index_; + motor_angle_index_++; + } } } return 0; } +int Device::ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name) +{ + if (debug_mode_>1) + { + std::cout << "map_joint_name.size(): " + << map_joint_name.size() << std::endl; + std::cout << map_joint_name << std::endl; + } + for (unsigned int i=0;i(); + if (debug_mode_>1) + { + std::cout << "-- Joint: " << jointName << " has index: " << i << std::endl; + } + setControlPos(jointName,i); + } + return 0; +} -int Device::ParseYAMLMapHardware(YAML::Node & map_hs_control) +int Device::ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode) { if (debug_mode_>1) { - std::cout << "map_hs_control.size(): " - << map_hs_control.size() << std::endl; - std::cout << map_hs_control << std::endl; + std::cout << "map_control_mode.size(): " + << map_control_mode.size() << std::endl; + std::cout << map_control_mode << std::endl; } - unsigned int i=0; - for (YAML::const_iterator it=map_hs_control.begin(); - it!=map_hs_control.end(); - it++) + + if (map_control_mode.size() == 0) { + std::string value = map_control_mode.as(); + JointSHWControlType_iterator it_control_type; + for(it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) + { + int u_index = it_control_type->second.urdf_index; + setSoTControlType(urdf_joints_[u_index]->name, value); + } if (debug_mode_>1) { - std::cout << i << " " << std::endl; - std::cout << "key:" << it->first.as() << std::endl; + std::cout << "All joints are controlled in: " << value << std::endl; } + return 0; + } + for (YAML::const_iterator it=map_control_mode.begin(); + it!=map_control_mode.end(); + it++) + { std::string jointName = it->first.as(); + if (debug_mode_>1) + { + std::cout << "joint name: " << jointName << std::endl; + } + YAML::Node aNode = it->second; if (debug_mode_>1) @@ -510,7 +561,7 @@ int Device::ParseYAMLMapHardware(YAML::Node & map_hs_control) { std::cout << "-- key:" << aKey << std::endl; } - if (aKey=="hw") + if (aKey=="hw_control_mode") { std::string value = it2->second.as(); if (debug_mode_>1) @@ -519,7 +570,7 @@ int Device::ParseYAMLMapHardware(YAML::Node & map_hs_control) } setHWControlType(jointName,value); } - else if (aKey=="sot") + else if (aKey=="ros_control_mode") { std::string value = it2->second.as(); if (debug_mode_>1) @@ -528,26 +579,7 @@ int Device::ParseYAMLMapHardware(YAML::Node & map_hs_control) } setSoTControlType(jointName,value); } - else if (aKey=="controlPos") - { - unsigned int index= it2->second.as(); - if (debug_mode_>1) - { - std::cout << "-- index: " << index << std::endl; - } - setControlPos(jointName,index); - } - else if (aKey=="sensors") - { - YAML::Node aNode = it2->second; - if (debug_mode_>1) - { - std::cout << "-- type: " << aNode.Type() << std::endl; - } - ParseYAMLJointSensor(jointName,aNode); - } } - i++; } return 0; } @@ -560,58 +592,25 @@ int Device::ParseYAMLSensors(YAML::Node &map_sensors) std::cerr << "Device::ParseYAMLString: No sensor detected in YamlString " << std::endl; return -1; } - - for (YAML::const_iterator it=map_sensors.begin(); - it!=map_sensors.end(); - it++) + std::string sensor_name = map_sensors.as(); + if (debug_mode_>1) { - if (debug_mode_>1) - { - std::cout << "sensor_type:" << it->first.as() << std::endl; - } - std::string sensor_type = it->first.as(); - - YAML::Node aNode = it->second; - if (debug_mode_>1) - { - std::cout << "Type of value: " << aNode.Type() << std::endl; - } - - // Iterates over types of node. - for (YAML::const_iterator it2=aNode.begin(); - it2!=aNode.end(); - it2++) - { - std::string sensor_name = it2->first.as(); - if (debug_mode_>1) - { - std::cout << "-- sensor_name:" << sensor_name << std::endl; - } + std::cout << "-- sensor_name:" << sensor_name << std::endl; + } + if (sensor_name.find("ft") != std::string::npos) + { + CreateAForceSignal(sensor_name); + } - if (sensor_type=="force_torque") - { - std::string force_sensor_name = it2->second.as(); - if (debug_mode_>1) - { - std::cout << "-- force_sensor_name: " << force_sensor_name << std::endl; - } - CreateAForceSignal(force_sensor_name); - } - else if (sensor_type=="imu") - { - std::string imu_sensor_name = it2->second.as(); - if (debug_mode_>1) - { - std::cout << "-- Value: " << imu_sensor_name << std::endl; - } - CreateAnImuSignal(imu_sensor_name); - } - else - { - std::cerr << "The sensor type " << sensor_type - << " is not recognized" << std::endl; - } - } + else if (sensor_name.find("imu") != std::string::npos) + { + CreateAnImuSignal(sensor_name); + } + else + { + std::cerr << "The sensor " << sensor_name + << " is not recognized" << std::endl; + return 1; } return 0; } @@ -645,7 +644,10 @@ int Device::UpdateSignals() { pseudoTorqueSOUT_ = new Signal("Device("+getName()+")::output(vector)::ptorque"); } - + // if ((force_index_!=0) && (forcesSOUT_.size()!=0)) + // { + // forcesSOUT_ = new Signal("Device("+getName()+")::output(vector)::forces"); + // } if ((current_index_!=0) && (currentsSOUT_!=0)) { currentsSOUT_ = new Signal("Device("+getName()+")::output(vector)::currents"); @@ -908,7 +910,7 @@ void Device::setSensorsGains(map &SensorsIn, int t) void Device::setSensors(map &SensorsIn) { sotDEBUGIN(25) ; - int t = stateSOUT_.getTime () + 1; + int t = motorcontrolSOUT_.getTime () + 1; setSensorsForce(SensorsIn,t); setSensorsIMU(SensorsIn,t); @@ -939,37 +941,20 @@ void Device::cleanupSetSensors(map &SensorsIn) void Device::getControl(map &controlOut) { ODEBUG5FULL("start"); - sotDEBUGIN(25) ; - const Vector & controlIN = controlSIN.accessCopy(); - vector lcontrolOut; - lcontrolOut.resize(controlIN.size()); - + sotDEBUGIN(25); + // Integrate control increment(); sotDEBUG (25) << "control = " << control_ << std::endl; ODEBUG5FULL("control = "<< control_); - // Specify the joint values for the controller. - JointSHWControlType_iterator it_control_type; - for(it_control_type = jointDevices_.begin(); - it_control_type != jointDevices_.end(); - it_control_type++) - { - int lctl_index = it_control_type->second.control_index; - if (it_control_type->second.HWcontrol==TORQUE) - { - lcontrolOut[lctl_index] = controlIN[lctl_index]; - } - else if (it_control_type->second.HWcontrol==POSITION) - { - int u_index = it_control_type->second.urdf_index; - lcontrolOut[lctl_index] = control_[u_index]; - } - } - + vector lcontrolOut; + lcontrolOut.resize(control_.size()); + Eigen::VectorXd::Map(&lcontrolOut[0], control_.size()) = control_; + controlOut["control"].setValues(lcontrolOut); - + ODEBUG5FULL("end"); sotDEBUGOUT(25) ; } From 2b0d6ccc26ce9dd0d7ede01e2baf0ebc8c3ab84c Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 7 Aug 2019 13:36:49 +0200 Subject: [PATCH 03/43] [Device] Add dependencies to yaml-cpp and urdfdom. Remove the pinocchio one. --- CMakeLists.txt | 5 +++-- cmake | 2 +- src/CMakeLists.txt | 28 ++++++++++++++++++---------- unitTesting/CMakeLists.txt | 10 +++++++++- 4 files changed, 31 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 89d600586..8dc6540d0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,8 @@ SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework syste SEARCH_FOR_EIGEN() ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0") +ADD_REQUIRED_DEPENDENCY("yaml-cpp") +ADD_REQUIRED_DEPENDENCY("urdfdom") OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON) OPTION (INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python binding" OFF) @@ -59,13 +61,13 @@ IF(BUILD_PYTHON_INTERFACE) ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0") ENDIF(BUILD_PYTHON_INTERFACE) -ADD_COMPILE_DEPENDENCY ("pinocchio >= 2.0.0") SEARCH_FOR_BOOST() ADD_SUBDIRECTORY(include) ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(unitTesting) + IF (NOT INSTALL_PYTHON_INTERFACE_ONLY) ADD_SUBDIRECTORY(doc) ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) @@ -76,7 +78,6 @@ ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) IF(BUILD_PYTHON_INTERFACE) PYTHON_ADD_MODULE(robot_utils_sot_py src/tools/robot-utils-py.cpp) PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py dynamic-graph) - PKG_CONFIG_USE_DEPENDENCY(robot_utils_sot_py pinocchio) TARGET_LINK_LIBRARIES(robot_utils_sot_py ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${LIBRARY_NAME}) TARGET_LINK_BOOST_PYTHON(robot_utils_sot_py) INSTALL(TARGETS robot_utils_sot_py DESTINATION ${PYTHON_INSTALL_DIR}) diff --git a/cmake b/cmake index 5c8c19f49..0fb087eb8 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f +Subproject commit 0fb087eb8e78db616329de0e4ab55abbe015bbe8 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 492d06117..5cf6e9ad6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -88,6 +88,11 @@ SET(plugins ) +IF (YAML_CPP_FOUND AND URDFDOM_FOUND) + SET(plugins ${plugins} tools/device ) +ENDIF () + + # TODO IF(WIN32) LIST(REMOVE_ITEM plugins @@ -139,17 +144,17 @@ SET(${LIBRARY_NAME}_SOURCES factory/pool.cpp - tools/utils-windows - tools/periodic-call - tools/device - tools/trajectory - tools/robot-utils + tools/utils-windows.cpp + tools/periodic-call.cpp + tools/device.cpp + tools/trajectory.cpp + tools/robot-utils.cpp - matrix/matrix-svd + matrix/matrix-svd.cpp - filters/causal-filter + filters/causal-filter.cpp - utils/stop-watch + utils/stop-watch.cpp ) @@ -162,7 +167,6 @@ SET_TARGET_PROPERTIES(${LIBRARY_NAME} SOVERSION ${PROJECT_VERSION}) PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph) -PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio) IF(BUILD_PYTHON_INTERFACE) PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python) ENDIF(BUILD_PYTHON_INTERFACE) @@ -220,7 +224,11 @@ FOREACH(plugin ${plugins}) ENDIF(UNIX) PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph) - PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} pinocchio) + + IF (YAML_CPP_FOUND AND URDFDOM_FOUND) + PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} yaml-cpp) + PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} urdfdom) + ENDIF () # build python submodule IF(BUILD_PYTHON_INTERFACE) diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt index 9b3a43fb2..c39651a3a 100644 --- a/unitTesting/CMakeLists.txt +++ b/unitTesting/CMakeLists.txt @@ -90,7 +90,6 @@ SET (tests tools/test_mailbox tools/test_matrix tools/test_robot_utils - tools/test_device math/matrix-twist math/matrix-homogeneous @@ -98,6 +97,10 @@ SET (tests matrix/test_operator ) +IF (YAML_CPP_FOUND AND URDFDOM_FOUND) + SET(tests ${tests} tools/test_device ) +ENDIF () + # TODO IF(WIN32) LIST(REMOVE_ITEM tests tools/test_mailbox) @@ -150,6 +153,11 @@ FOREACH(test ${tests}) PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} dynamic-graph-python) ENDIF(BUILD_PYTHON_INTERFACE) + IF (YAML_CPP_FOUND AND URDFDOM_FOUND) + PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} yaml-cpp) + PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} urdfdom) + ENDIF () + # Link against Boost. TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_LIBRARIES} ${Boost_SYSTEM_LIBRARY}) ADD_TEST(${test} ${EXECUTABLE_NAME}) From f90a96f016160ccf082b1251ad8050df0c0d5a67 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Wed, 7 Aug 2019 13:44:13 +0200 Subject: [PATCH 04/43] [Device] Correction of the unitTest of device to respect yaml config Add an example of yaml file config that can be use for the device (the same generated by the unitTest with the function CreateYAMLFILE()). Add two yaml files which reflect how the data are split in two in the sot, these ones are used by the unitTest. The control is set has constant = -0.5 for all joints in position; the device output a control signal respecting the position limits of the joints. --- .../example_of_yaml_config_for_device.yaml | 67 +++ unitTesting/tools/sot_controller.yaml | 38 ++ unitTesting/tools/sot_params.yaml | 9 + unitTesting/tools/test_device.cpp | 505 ++++++++---------- 4 files changed, 322 insertions(+), 297 deletions(-) create mode 100644 unitTesting/tools/example_of_yaml_config_for_device.yaml create mode 100644 unitTesting/tools/sot_controller.yaml create mode 100644 unitTesting/tools/sot_params.yaml diff --git a/unitTesting/tools/example_of_yaml_config_for_device.yaml b/unitTesting/tools/example_of_yaml_config_for_device.yaml new file mode 100644 index 000000000..d0faa7922 --- /dev/null +++ b/unitTesting/tools/example_of_yaml_config_for_device.yaml @@ -0,0 +1,67 @@ +sot_controller: + map_rc_to_sot_device: { motor-angles: motor-angles, joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents, torques: torques, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0, control: control} + joint_names: [waist, 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] + control_mode: + LLEG_HIP_P: + ros_control_mode: POSITION + LLEG_HIP_R: + ros_control_mode: POSITION + LLEG_HIP_Y: + ros_control_mode: POSITION + LLEG_KNEE: + ros_control_mode: POSITION + LLEG_ANKLE_P: + ros_control_mode: POSITION + LLEG_ANKLE_R: + ros_control_mode: POSITION + RLEG_HIP_P: + ros_control_mode: POSITION + RLEG_HIP_R: + ros_control_mode: POSITION + RLEG_HIP_Y: + ros_control_mode: POSITION + RLEG_KNEE: + ros_control_mode: POSITION + RLEG_ANKLE_P: + ros_control_mode: POSITION + RLEG_ANKLE_R: + ros_control_mode: POSITION + WAIST_P: + ros_control_mode: POSITION + WAIST_R: + ros_control_mode: POSITION + CHEST: + ros_control_mode: POSITION + RARM_SHOULDER_P: + ros_control_mode: POSITION + RARM_SHOULDER_R: + ros_control_mode: POSITION + RARM_SHOULDER_Y: + ros_control_mode: POSITION + RARM_ELBOW: + ros_control_mode: POSITION + RARM_WRIST_Y: + ros_control_mode: POSITION + RARM_WRIST_P: + ros_control_mode: POSITION + RARM_WRIST_R: + ros_control_mode: POSITION + LARM_SHOULDER_P: + ros_control_mode: POSITION + LARM_SHOULDER_R: + ros_control_mode: POSITION + LARM_SHOULDER_Y: + ros_control_mode: POSITION + LARM_ELBOW: + ros_control_mode: POSITION + LARM_WRIST_Y: + ros_control_mode: POSITION + LARM_WRIST_P: + ros_control_mode: POSITION + LARM_WRIST_R: + ros_control_mode: POSITION + waist: + ros_control_mode: POSITION + left_ft_sensor: left_ankle_ft + right_ft_sensor: right_ankle_ft + base_imu_sensor: base_imu diff --git a/unitTesting/tools/sot_controller.yaml b/unitTesting/tools/sot_controller.yaml new file mode 100644 index 000000000..d406f6fd0 --- /dev/null +++ b/unitTesting/tools/sot_controller.yaml @@ -0,0 +1,38 @@ +sot_controller: + type: sot_controller/RCSotController + joints: + - waist + - 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/tools/sot_params.yaml b/unitTesting/tools/sot_params.yaml new file mode 100644 index 000000000..df408a268 --- /dev/null +++ b/unitTesting/tools/sot_params.yaml @@ -0,0 +1,9 @@ +sot_controller: + libname: libsot-controller.so + joint_names: [ waist, 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/tools/test_device.cpp b/unitTesting/tools/test_device.cpp index 0d2f7954d..60d553152 100644 --- a/unitTesting/tools/test_device.cpp +++ b/unitTesting/tools/test_device.cpp @@ -9,7 +9,6 @@ #include #include -#include #ifndef WIN32 #include @@ -29,215 +28,152 @@ using namespace std; void CreateYAMLFILE() { YAML::Emitter yaml_out; - YAML::Node aNode, yn_map_hw_sot_c, yn_map_sensors; - yn_map_hw_sot_c = aNode["map_hardware_sot_control"]; - yn_map_sensors = aNode["sensors"]; - /* - yn_map_hw_sot_c["waist"]; - yn_map_hw_sot_c["waist"]["hw"] = "POSITION"; - yn_map_hw_sot_c["waist"]["sot"] = "POSITION"; - yn_map_hw_sot_c["waist"]["controlPos"] = 0; - yn_map_hw_sot_c["waist"]["sensors"] = ""; - */ - yn_map_hw_sot_c["LLEG_HIP_P"]; - yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = 6; - yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LLEG_HIP_R"]; - yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = 7; - yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LLEG_HIP_Y"]; - yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = 8; - yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LLEG_KNEE"]; - yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = 9; - yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LLEG_ANKLE_P"]; - yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = 10; - yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LLEG_ANKLE_R"]; - yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = 11; - yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RLEG_HIP_P"]; - yn_map_hw_sot_c["RLEG_HIP_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RLEG_HIP_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = 12; - yn_map_hw_sot_c["RLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RLEG_HIP_R"]; - yn_map_hw_sot_c["RLEG_HIP_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RLEG_HIP_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RLEG_HIP_R"]["controlPos"] = 13; - yn_map_hw_sot_c["RLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RLEG_HIP_Y"]; - yn_map_hw_sot_c["RLEG_HIP_Y"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RLEG_HIP_Y"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = 14; - yn_map_hw_sot_c["RLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RLEG_KNEE"]; - yn_map_hw_sot_c["RLEG_KNEE"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RLEG_KNEE"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = 15; - yn_map_hw_sot_c["RLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RLEG_ANKLE_P"]; - yn_map_hw_sot_c["RLEG_ANKLE_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RLEG_ANKLE_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = 16; - yn_map_hw_sot_c["RLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RLEG_ANKLE_R"]; - yn_map_hw_sot_c["RLEG_ANKLE_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RLEG_ANKLE_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = 17; - yn_map_hw_sot_c["RLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["WAIST_P"]; - yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["WAIST_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["WAIST_P"]["controlPos"] = 18; - yn_map_hw_sot_c["WAIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["WAIST_R"]; - yn_map_hw_sot_c["WAIST_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["WAIST_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["WAIST_R"]["controlPos"] = 19; - yn_map_hw_sot_c["WAIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["CHEST"]; - yn_map_hw_sot_c["CHEST"]["hw"] = "POSITION"; - yn_map_hw_sot_c["CHEST"]["sot"] = "POSITION"; - yn_map_hw_sot_c["CHEST"]["controlPos"] = 20; - yn_map_hw_sot_c["CHEST"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_SHOULDER_P"]; - yn_map_hw_sot_c["RARM_SHOULDER_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_SHOULDER_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = 21; - yn_map_hw_sot_c["RARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_SHOULDER_R"]; - yn_map_hw_sot_c["RARM_SHOULDER_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_SHOULDER_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = 22; - yn_map_hw_sot_c["RARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_SHOULDER_Y"]; - yn_map_hw_sot_c["RARM_SHOULDER_Y"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = 23; - yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_ELBOW"]; - yn_map_hw_sot_c["RARM_ELBOW"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_ELBOW"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = 24; - yn_map_hw_sot_c["RARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_WRIST_Y"]; - yn_map_hw_sot_c["RARM_WRIST_Y"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_WRIST_Y"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = 25; - yn_map_hw_sot_c["RARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_WRIST_P"]; - yn_map_hw_sot_c["RARM_WRIST_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_WRIST_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = 26; - yn_map_hw_sot_c["RARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["RARM_WRIST_R"]; - yn_map_hw_sot_c["RARM_WRIST_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["RARM_WRIST_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = 27; - yn_map_hw_sot_c["RARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]"; - - yn_map_hw_sot_c["LARM_SHOULDER_P"]; - yn_map_hw_sot_c["LARM_SHOULDER_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_SHOULDER_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = 28; - yn_map_hw_sot_c["LARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LARM_SHOULDER_R"]; - yn_map_hw_sot_c["LARM_SHOULDER_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_SHOULDER_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = 29; - yn_map_hw_sot_c["LARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LARM_SHOULDER_Y"]; - yn_map_hw_sot_c["LARM_SHOULDER_Y"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = 30; - yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LARM_ELBOW"]; - yn_map_hw_sot_c["LARM_ELBOW"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_ELBOW"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = 31; - yn_map_hw_sot_c["LARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LARM_WRIST_Y"]; - yn_map_hw_sot_c["LARM_WRIST_Y"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_WRIST_Y"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = 32; - yn_map_hw_sot_c["LARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LARM_WRIST_P"]; - yn_map_hw_sot_c["LARM_WRIST_P"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_WRIST_P"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = 33; - yn_map_hw_sot_c["LARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]"; - - yn_map_hw_sot_c["LARM_WRIST_R"]; - yn_map_hw_sot_c["LARM_WRIST_R"]["hw"] = "POSITION"; - yn_map_hw_sot_c["LARM_WRIST_R"]["sot"] = "POSITION"; - yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = 34; - yn_map_hw_sot_c["LARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]"; - - yn_map_sensors["force_torque"]; - yn_map_sensors["force_torque"]["left_ankle_ft"]; - yn_map_sensors["force_torque"]["left_ankle_ft"]["sensor_joint"] = "LLEG_ANKLE_R"; - yn_map_sensors["force_torque"]["left_ankle_ft"]["frame"] = "LLEG_LINK_6"; - - yn_map_sensors["force_torque"]; - yn_map_sensors["force_torque"]["right_ankle_ft"]; - yn_map_sensors["force_torque"]["right_ankle_ft"]["sensor_joint"] = "RLEG_ANKLE_R"; - yn_map_sensors["force_torque"]["right_ankle_ft"]["frame"] = "RLEG_LINK_6"; - - yn_map_sensors["force_torque"]; - yn_map_sensors["force_torque"]["left_wrist_ft"]; - yn_map_sensors["force_torque"]["left_wrist_ft"]["sensor_joint"] = "LLEG_WRIST_R"; - yn_map_sensors["force_torque"]["left_wrist_ft"]["frame"] = "LARM_LINK_7"; - - yn_map_sensors["force_torque"]; - yn_map_sensors["force_torque"]["right_wrist_ft"]; - yn_map_sensors["force_torque"]["right_wrist_ft"]["sensor_joint"] = "RLEG_WRIST_R"; - yn_map_sensors["force_torque"]["right_wrist_ft"]["frame"] = "RARM_LINK_7"; - - - YAML::Node yn_imu = yn_map_sensors["imu"]; - yn_imu["base_imu"]; - yn_imu["base_imu"]["frame"] = "LARM_LINK6"; - yn_imu["base_imu"]["gazebo_sensor_J"] = "LARM_LINK6"; + YAML::Node aNode, yn_map_sot_controller, yn_map_rc_to_sot_device, yn_map_joint_names, yn_control_mode; + yn_map_sot_controller = aNode["sot_controller"]; + yn_map_rc_to_sot_device = yn_map_sot_controller["map_rc_to_sot_device"]; + yn_map_joint_names = yn_map_sot_controller["joint_names"]; + yn_control_mode = yn_map_sot_controller["control_mode"]; + + yn_map_rc_to_sot_device["motor-angles"] = "motor-angles"; + yn_map_rc_to_sot_device["joint-angles"] = "joint-angles"; + yn_map_rc_to_sot_device["velocities"] = "velocities"; + yn_map_rc_to_sot_device["forces"] = "forces"; + yn_map_rc_to_sot_device["currents"] = "currents"; + yn_map_rc_to_sot_device["torques"] = "torques"; + yn_map_rc_to_sot_device["accelerometer_0"] = "accelerometer_0"; + yn_map_rc_to_sot_device["gyrometer_0"] = "gyrometer_0"; + yn_map_rc_to_sot_device["control"] = "control"; + + yn_map_joint_names.push_back("waist"); + yn_map_joint_names.push_back("LLEG_HIP_P"); + yn_map_joint_names.push_back("LLEG_HIP_R"); + yn_map_joint_names.push_back("LLEG_HIP_Y"); + yn_map_joint_names.push_back("LLEG_KNEE"); + yn_map_joint_names.push_back("LLEG_ANKLE_P"); + yn_map_joint_names.push_back("LLEG_ANKLE_R"); + yn_map_joint_names.push_back("RLEG_HIP_P"); + yn_map_joint_names.push_back("RLEG_HIP_R"); + yn_map_joint_names.push_back("RLEG_HIP_Y"); + yn_map_joint_names.push_back("RLEG_KNEE"); + yn_map_joint_names.push_back("RLEG_ANKLE_P"); + yn_map_joint_names.push_back("RLEG_ANKLE_R"); + yn_map_joint_names.push_back("WAIST_P"); + yn_map_joint_names.push_back("WAIST_R"); + yn_map_joint_names.push_back("CHEST"); + yn_map_joint_names.push_back("RARM_SHOULDER_P"); + yn_map_joint_names.push_back("RARM_SHOULDER_R"); + yn_map_joint_names.push_back("RARM_SHOULDER_Y"); + yn_map_joint_names.push_back("RARM_ELBOW"); + yn_map_joint_names.push_back("RARM_WRIST_Y"); + yn_map_joint_names.push_back("RARM_WRIST_P"); + yn_map_joint_names.push_back("RARM_WRIST_R"); + yn_map_joint_names.push_back("LARM_SHOULDER_P"); + yn_map_joint_names.push_back("LARM_SHOULDER_R"); + yn_map_joint_names.push_back("LARM_SHOULDER_Y"); + yn_map_joint_names.push_back("LARM_ELBOW"); + yn_map_joint_names.push_back("LARM_WRIST_Y"); + yn_map_joint_names.push_back("LARM_WRIST_P"); + yn_map_joint_names.push_back("LARM_WRIST_R"); + + yn_control_mode["waist"]; + yn_control_mode["waist"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LLEG_HIP_P"]; + yn_control_mode["LLEG_HIP_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LLEG_HIP_R"]; + yn_control_mode["LLEG_HIP_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LLEG_HIP_Y"]; + yn_control_mode["LLEG_HIP_Y"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LLEG_KNEE"]; + yn_control_mode["LLEG_KNEE"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LLEG_ANKLE_P"]; + yn_control_mode["LLEG_ANKLE_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LLEG_ANKLE_R"]; + yn_control_mode["LLEG_ANKLE_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RLEG_HIP_P"]; + yn_control_mode["RLEG_HIP_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RLEG_HIP_R"]; + yn_control_mode["RLEG_HIP_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RLEG_HIP_Y"]; + yn_control_mode["RLEG_HIP_Y"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RLEG_KNEE"]; + yn_control_mode["RLEG_KNEE"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RLEG_ANKLE_P"]; + yn_control_mode["RLEG_ANKLE_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RLEG_ANKLE_R"]; + yn_control_mode["RLEG_ANKLE_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["WAIST_P"]; + yn_control_mode["WAIST_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["WAIST_R"]; + yn_control_mode["WAIST_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["CHEST"]; + yn_control_mode["CHEST"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_SHOULDER_P"]; + yn_control_mode["RARM_SHOULDER_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_SHOULDER_R"]; + yn_control_mode["RARM_SHOULDER_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_SHOULDER_Y"]; + yn_control_mode["RARM_SHOULDER_Y"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_ELBOW"]; + yn_control_mode["RARM_ELBOW"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_WRIST_Y"]; + yn_control_mode["RARM_WRIST_Y"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_WRIST_P"]; + yn_control_mode["RARM_WRIST_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["RARM_WRIST_R"]; + yn_control_mode["RARM_WRIST_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_SHOULDER_P"]; + yn_control_mode["LARM_SHOULDER_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_SHOULDER_R"]; + yn_control_mode["LARM_SHOULDER_R"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_SHOULDER_Y"]; + yn_control_mode["LARM_SHOULDER_Y"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_ELBOW"]; + yn_control_mode["LARM_ELBOW"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_WRIST_Y"]; + yn_control_mode["LARM_WRIST_Y"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_WRIST_P"]; + yn_control_mode["LARM_WRIST_P"]["ros_control_mode"] = "POSITION"; + + yn_control_mode["LARM_WRIST_R"]; + yn_control_mode["LARM_WRIST_R"]["ros_control_mode"] = "POSITION"; + + yn_map_sot_controller["left_ft_sensor"] = "left_ankle_ft"; + + yn_map_sot_controller["right_ft_sensor"] = "right_ankle_ft"; + + yn_map_sot_controller["left_wrist_ft_sensor"] = "left_wrist_ft"; + + yn_map_sot_controller["right_ft_wrist_sensor"] = "right_wrist_ft"; + + yn_map_sot_controller["base_imu_sensor"] = "base_imu"; ofstream of; of.open("map_hs_sot_gen.yaml", ios::out); @@ -247,64 +183,29 @@ void CreateYAMLFILE() { of.close(); } -int ReadYAMLFILE(dg::sot::Device &aDevice, unsigned int debug_mode) { - std::string yaml_file = "map_hs_sot_gen.yaml"; - YAML::Node map_hs_sot = YAML::LoadFile(yaml_file); - - if (map_hs_sot.IsNull()) { - std::cerr << "Unable to read " << yaml_file << std::endl; - return -1; - } - if (debug_mode > 0) - std::cout << "Reading file : " << yaml_file << std::endl; - YAML::Node map_hs_control = map_hs_sot["map_hardware_sot_control"]; - if (debug_mode > 1) { - std::cout << "map_hs_control.size(): " - << map_hs_control.size() << std::endl; - std::cout << map_hs_control << std::endl; - } - unsigned int i = 0; - for (YAML::const_iterator it = map_hs_control.begin(); - it != map_hs_control.end(); - it++) { - if (debug_mode > 1) { - std::cout << i << " " << std::endl; - std::cout << "key:" << it->first.as() << std::endl; - } - std::string jointName = it->first.as(); - - YAML::Node aNode = it->second; - if (debug_mode > 1) - std::cout << "Type of value: " << aNode.Type() << std::endl; - - for (YAML::const_iterator it2 = aNode.begin(); - it2 != aNode.end(); - it2++) - - { - std::string aKey = it2->first.as(); - if (debug_mode > 1) - std::cout << "-- key:" << aKey << std::endl; - - if (aKey == "hw") { - std::string value = it2->second.as(); - if (debug_mode > 1) - std::cout << "-- Value: " << value << std::endl; - aDevice.setHWControlType(jointName, value); - } else if (aKey == "sot") { - std::string value = it2->second.as(); - if (debug_mode > 1) - std::cout << "-- Value: " << value << std::endl; - aDevice.setSoTControlType(jointName, value); - } else if (aKey == "controlPos") { - unsigned int index = it2->second.as(); - if (debug_mode > 1) - std::cout << "-- index: " << index << std::endl; - aDevice.setControlPos(jointName, index); - } - } - i++; - } +int ReadYAMLFILE(dg::sot::Device &aDevice) { + // Reflect how the data are splitted in two yaml files in the sot + // Comment and use the commented code to use the above yaml file + std::ifstream yaml_file_controller("../../unitTesting/tools/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/tools/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); + + // Uncomment if you want to use the above yaml file + // All the data are in one file, which does not reflect reality + + // std::ifstream yaml_file("map_hs_sot_gen.yaml"); + // std::string yaml_string; + // yaml_string.assign((std::istreambuf_iterator(yaml_file) ), + // (std::istreambuf_iterator() ) ); + // aDevice.ParseYAMLString(yaml_string); return 0; } @@ -326,14 +227,17 @@ int main(int, char **) { /// Test reading the URDF file. dg::sot::Device aDevice(std::string("simple_humanoid")); - + aDevice.setDebugMode(debug_mode); aDevice.setURDFModel(robot_description); - CreateYAMLFILE(); - if (ReadYAMLFILE(aDevice, debug_mode) < 0) + // Uncomment if you want to create and use the above yaml file + // All the data are in one file, which does not reflect reality + // CreateYAMLFILE(); + + if (ReadYAMLFILE(aDevice) < 0) return -1; - dg::Vector aState(29); + dg::Vector aState(35); for (unsigned j = 0; j < aState.size(); j++) aState(j) = 0.0; aDevice.setState(aState); @@ -346,12 +250,16 @@ int main(int, char **) { aDevice.controlSIN.setConstant(aControlVector); for (unsigned int i = 0; i < 2000; i++) - aDevice.increment(dt); + aDevice.increment(); - const se3::Model & aModel = aDevice.getModel(); + const urdf::ModelInterfaceSharedPtr aModel = aDevice.getModel(); - const dg::Vector & aPosition = aDevice.stateSOUT_(2001); - double diff = 0, ldiff; + const dg::Vector & aControl = aDevice.motorcontrolSOUT_(2001); + map 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(); @@ -359,34 +267,37 @@ int main(int, char **) { it_control_type++) { int lctl_index = it_control_type->second.control_index; - if (it_control_type->second.HWcontrol == dgsot::POSITION) - { - int u_index = it_control_type->second.urdf_index; - if (u_index != -1) - { - std::vector< ::urdf::LinkSharedPtr > urdf_links; - vector< ::urdf::JointSharedPtr > urdf_joints; - aModel.getLinks(urdf_links); - for (unsigned j=0; j child_joints = urdf_links[j]->child_joints; - urdf_joints.insert(urdf_joints.end(), boost::make_move_iterator(child_joints.begin()), - boost::make_move_iterator(child_joints.end())); - } + 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 = (aPosition[lpos_index] - lowerLim); + ldiff = (aControl[lctl_index] - lowerLim); + ldiffCont = controlOut["control"].getValues()[lctl_index] - lowerLim; diff += ldiff; - std::cout << ldiff << " " << urdf_joints[u_index]->name << " " - << aPosition[lpos_index] << " " - << lowerLim << " " - << -urdf_joints[u_index]->limits->velocity + diffCont += ldiffCont; + std::cout << "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; - if (diff > 1e-3) - return -1; return 0; } From 080aff1136246e60f1df2aeb13ad7533c2304d20 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Mon, 19 Aug 2019 17:46:04 +0200 Subject: [PATCH 05/43] [Device] Remove useless signal, add check torque limits --- include/sot/core/device.hh | 18 ++----- src/tools/device.cpp | 84 ++++++++++++------------------- unitTesting/tools/test_device.cpp | 5 -- 3 files changed, 37 insertions(+), 70 deletions(-) diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 7a5981824..332931e65 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -49,13 +49,12 @@ namespace sot { /// It is used for both the SoT side and the hardware side. enum ControlType { POSITION = 0, - VELOCITY = 1, - ACCELERATION = 2, - TORQUE = 3 + TORQUE = 1, + VELOCITY = 2 }; const std::string ControlType_s[] = { - "POSITION", "VELOCITY", "ACCELERATION", "TORQUE" + "POSITION", "TORQUE", "VELOCITY" }; //@} @@ -170,7 +169,7 @@ class SOT_CORE_EXPORT Device: public Entity /* --- DESTRUCTION --- */ virtual ~Device(); - virtual void setState(const dg::Vector& st); + virtual void setControl(const dg::Vector& cont); /// Set control input type. virtual void setSoTControlType(const std::string &jointNames, @@ -212,7 +211,6 @@ class SOT_CORE_EXPORT Device: public Entity /// \brief Output attitude provided by the hardware /*! \brief The current state of the robot from the command viewpoint. */ dg::Signal motorcontrolSOUT_; - dg::Signal previousControlSOUT_; /// \} /// \name Real robot current state @@ -268,6 +266,7 @@ class SOT_CORE_EXPORT Device: public Entity /// \brief Compute the new control, from the given one. /// When the control is in position, checks that the position is within bounds. + /// When the control is in torque, checks that the torque is within bounds. virtual void updateControl(const Vector & controlIN); /// \name Signals related methods @@ -310,15 +309,8 @@ class SOT_CORE_EXPORT Device: public Entity void setSensorsGains(std::map &SensorsIn, int t); - public: - virtual void setRoot( const dg::Matrix & root ); - virtual void setRoot( const MatrixHomogeneous & worldMwaist ); - private: - // Intermediate variable to avoid dynamic allocation - dg::Vector forceZero6; - // URDF Model of the robot ::urdf::ModelInterfaceSharedPtr model_; std::vector< ::urdf::JointSharedPtr > urdf_joints_; diff --git a/src/tools/device.cpp b/src/tools/device.cpp index a1c741daf..51c78cf88 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -90,7 +90,6 @@ Device::Device( const std::string& n ) ,sanityCheck_(true) ,controlSIN( NULL,"Device("+n+")::input(double)::control" ) ,motorcontrolSOUT_ ( "Device("+n+")::output(vector)::motorcontrol" ) - ,previousControlSOUT_( "Device("+n+")::output(vector)::previousControl" ) ,robotState_ ("Device("+n+")::output(vector)::robotState") ,robotVelocity_ ("Device("+n+")::output(vector)::robotVelocity") ,forcesSOUT_(0) @@ -100,7 +99,6 @@ Device::Device( const std::string& n ) ,currentsSOUT_(0) ,motor_anglesSOUT_(0) ,joint_anglesSOUT_(0) - ,forceZero6 (6) ,debug_mode_(5) ,temperature_index_(0) ,velocity_index_(0) @@ -111,12 +109,9 @@ Device::Device( const std::string& n ) ,motor_angle_index_(0) { - forceZero6.fill (0); - signalRegistration( controlSIN << robotState_ << robotVelocity_ - << previousControlSOUT_ << motorcontrolSOUT_); control_.fill(.0); motorcontrolSOUT_.setConstant( control_ ); @@ -126,37 +121,27 @@ Device::Device( const std::string& n ) std::string docstring; docstring = "\n" - " Set state vector value\n" + " Set control vector value\n" "\n"; addCommand("set", new command::Setter - (*this, &Device::setState, docstring)); - - void(Device::*setRootPtr)(const Matrix&) = &Device::setRoot; - docstring = command::docCommandVoid1("Set the root position.", - "matrix homogeneous"); - addCommand("setRoot", - command::makeCommandVoid1(*this,setRootPtr, - docstring)); + (*this, &Device::setControl, docstring)); /* SET of SoT control input type per joint */ addCommand("setSoTControlType", - command::makeCommandVoid2(*this,&Device::setSoTControlType, - command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", - "Joint name", - "Control type: [TORQUE|ACCELERATION|VELOCITY|POSITION]") - )); - + command::makeCommandVoid2(*this, &Device::setSoTControlType, + command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", + "Joint name", + "Control type: [TORQUE|POSITION|VELOCITY]"))); /* SET of HW control input type per joint */ addCommand("setHWControlType", - command::makeCommandVoid2(*this,&Device::setHWControlType, - command::docCommandVoid2 ("Set HW control input type per joint", - "Joint name", - "Control type: [TORQUE|ACCELERATION|VELOCITY|POSITION]") - )); - + command::makeCommandVoid2(*this, &Device::setHWControlType, + command::docCommandVoid2 ("Set HW control input type per joint", + "Joint name", + "Control type: [TORQUE|POSITION|VELOCITY]"))); + docstring = "\n" " Enable/Disable sanity checks\n" @@ -173,31 +158,12 @@ Device::Device( const std::string& n ) } } -void Device::setState( const Vector& st ) +void Device::setControl( const Vector& cont ) { - updateControl(st); + updateControl(cont); motorcontrolSOUT_ .setConstant( control_ ); } - -void Device::setRoot( const Matrix & root ) -{ - Eigen::Matrix4d _matrix4d(root); - MatrixHomogeneous _root(_matrix4d); - setRoot( _root ); -} - -void Device::setRoot( const MatrixHomogeneous & worldMwaist ) -{ - VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse(); - for( unsigned int i=0;i<3;++i ) - { - control_(i) = worldMwaist.translation()(i); - control_(i+3) = r(i); - } -} - - void Device::setSanityCheck(const bool & enableCheck) { sanityCheck_ = enableCheck; @@ -205,7 +171,7 @@ void Device::setSanityCheck(const bool & enableCheck) void Device::setControlType(const std::string &strCtrlType, ControlType &aCtrlType) { - for(int j=0;j<4;j++) + for(int j=0;j<2;j++) { if (strCtrlType==ControlType_s[j]){ aCtrlType = (ControlType)j; @@ -329,7 +295,7 @@ void Device::increment() { dgRTLOG() << "unknown exception caught while" - << " running periodical commands (after)" << std::endl; + << "running periodical commands (after)" << std::endl; } } @@ -338,7 +304,7 @@ void Device::updateControl(const Vector & controlIN ) { if (sanityCheck_ && controlIN.hasNaN()) { - dgRTLOG () << "Device::integrate: Control has NaN values: " << '\n' + dgRTLOG () << "Device::updateControl: Control has NaN values: " << '\n' << controlIN.transpose() << '\n'; return; } @@ -369,7 +335,7 @@ void Device::updateControl(const Vector & controlIN ) { control_[lctl_index] = controlIN[lctl_index]; - // Position from control is directly provided. + // Check Position limits. if ((it_control_type->second.SoTcontrol==POSITION) && (urdf_joints_[u_index]->limits)) { @@ -384,6 +350,20 @@ void Device::updateControl(const Vector & controlIN ) control_[lctl_index] = upperLim; } } + // Check Torque limits. + if ((it_control_type->second.SoTcontrol==TORQUE) && + (urdf_joints_[u_index]->limits)) + { + double lim = urdf_joints_[u_index]->limits->effort; + if (control_[lctl_index] < -lim) + { + control_[lctl_index] = -lim; + } + else if (control_[lctl_index] > lim) + { + control_[lctl_index] = lim; + } + } } } } @@ -943,7 +923,7 @@ void Device::getControl(map &controlOut) ODEBUG5FULL("start"); sotDEBUGIN(25); - // Integrate control + // Increment control increment(); sotDEBUG (25) << "control = " << control_ << std::endl; diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_device.cpp index 60d553152..14a220366 100644 --- a/unitTesting/tools/test_device.cpp +++ b/unitTesting/tools/test_device.cpp @@ -237,11 +237,6 @@ int main(int, char **) { if (ReadYAMLFILE(aDevice) < 0) return -1; - dg::Vector aState(35); - for (unsigned j = 0; j < aState.size(); j++) - aState(j) = 0.0; - aDevice.setState(aState); - /// Fix constant vector for the control entry dg::Vector aControlVector(35); double dt = 0.005; From 4a7bc9eeb837354fd8733fa6c52134fa18170ff5 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 20 Aug 2019 14:59:39 +0200 Subject: [PATCH 06/43] [Device] Add check on torque limits in unit test --- include/sot/core/device.hh | 2 +- unitTesting/tools/test_device.cpp | 26 +++++++++++++++++++++----- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 332931e65..b6cc6a25e 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -46,7 +46,7 @@ namespace dynamicgraph { namespace sot { /// Specifies the nature of one joint control -/// It is used for both the SoT side and the hardware side. +/// It is used for the hardware side. enum ControlType { POSITION = 0, TORQUE = 1, diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_device.cpp index 14a220366..58c968d39 100644 --- a/unitTesting/tools/test_device.cpp +++ b/unitTesting/tools/test_device.cpp @@ -238,9 +238,8 @@ int main(int, char **) { return -1; /// Fix constant vector for the control entry - dg::Vector aControlVector(35); - double dt = 0.005; - for (unsigned int i = 0; i < 35; i++) + dg::Vector aControlVector(30); + for (unsigned int i = 0; i < 30; i++) aControlVector[i] = -0.5; aDevice.controlSIN.setConstant(aControlVector); @@ -275,12 +274,29 @@ int main(int, char **) { ldiffCont = controlOut["control"].getValues()[lctl_index] - lowerLim; diff += ldiff; diffCont += ldiffCont; - std::cout << "lowerLim: " << lowerLim << "\n" + 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 if (it_control_type->second.SoTcontrol == dgsot::TORQUE) + { + if (u_index != -1 && (urdf_joints[u_index]->limits)) + { + double lim = urdf_joints[u_index]->limits->effort; + ldiff = (aControl[lctl_index] - lim); + ldiffCont = controlOut["control"].getValues()[lctl_index] - lim; + diff += ldiff; + diffCont += ldiffCont; + std::cout << "Torque Lim: " << lim << "\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; } } From a16fa19f060117a76d519ad94550f4681b59678d Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 23 Aug 2019 17:46:15 +0200 Subject: [PATCH 07/43] [Device] Add boost::bind to the signals SOUT_ --- include/sot/core/device.hh | 29 ++++++++++++++++++------- src/tools/device.cpp | 35 +++++++++++++------------------ src/tools/robot-simu.cpp | 11 +++++++--- unitTesting/tools/test_device.cpp | 18 +++------------- 4 files changed, 48 insertions(+), 45 deletions(-) diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index b6cc6a25e..bd4ee59d2 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -45,6 +45,21 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { +/* --------------------------------------------------------------------- */ +/* --- API ------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#if defined (WIN32) +# if defined (device_EXPORTS) +# define Device_EXPORT __declspec(dllexport) +# else +# define Device_EXPORT __declspec(dllimport) +# endif +#else +# define Device_EXPORT +#endif + + /// Specifies the nature of one joint control /// It is used for the hardware side. enum ControlType { @@ -121,7 +136,7 @@ JointSHWControlType_iterator; /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOT_CORE_EXPORT Device: public Entity +class Device_EXPORT Device: public Entity { public: @@ -176,7 +191,7 @@ class SOT_CORE_EXPORT Device: public Entity const std::string &sotCtrlType); virtual void setHWControlType(const std::string &jointNames, const std::string &hwCtrlType); - virtual void increment(); + virtual void increment(const int& t); /// Read directly the URDF model void setURDFModel(const std::string &aURDFModel); @@ -210,7 +225,7 @@ class SOT_CORE_EXPORT Device: public Entity /// \{ /// \brief Output attitude provided by the hardware /*! \brief The current state of the robot from the command viewpoint. */ - dg::Signal motorcontrolSOUT_; + dg::SignalTimeDependent motorcontrolSOUT_; /// \} /// \name Real robot current state @@ -254,20 +269,20 @@ class SOT_CORE_EXPORT Device: public Entity void setupSetSensors(std::map &sensorsIn); void nominalSetSensors(std::map &sensorsIn); void cleanupSetSensors(std::map &sensorsIn); - - /// \brief Provides to the robot the control information. - void getControl(std::map &anglesOut); ///@} protected: + /// \brief Provides to the robot the control information (callback signal motorcontrolSOUT_). + dg::Vector& getControl(dg::Vector &controlOut, const int& t); + void setControlType(const std::string &strCtrlType, ControlType &aCtrlType); /// \brief Compute the new control, from the given one. /// When the control is in position, checks that the position is within bounds. /// When the control is in torque, checks that the torque is within bounds. - virtual void updateControl(const Vector & controlIN); + virtual void updateControl(const dg::Vector & controlIN); /// \name Signals related methods ///@{ diff --git a/src/tools/device.cpp b/src/tools/device.cpp index 51c78cf88..5e72067cc 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -50,7 +50,8 @@ using namespace dynamicgraph; #define ODEBUG5(x) #endif -const std::string Device::CLASS_NAME = "Device"; +DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Device, "Device"); +// const std::string Device::CLASS_NAME = "Device"; const double Device::TIMESTEP_DEFAULT = 0.001; @@ -89,9 +90,11 @@ Device::Device( const std::string& n ) ,control_(6) ,sanityCheck_(true) ,controlSIN( NULL,"Device("+n+")::input(double)::control" ) - ,motorcontrolSOUT_ ( "Device("+n+")::output(vector)::motorcontrol" ) - ,robotState_ ("Device("+n+")::output(vector)::robotState") - ,robotVelocity_ ("Device("+n+")::output(vector)::robotVelocity") + ,motorcontrolSOUT_( boost::bind(&Device::getControl,this,_1,_2), + controlSIN, + "Device("+n+")::output(vector)::motorcontrol" ) + ,robotState_("Device("+n+")::output(vector)::robotState") + ,robotVelocity_("Device("+n+")::output(vector)::robotVelocity") ,forcesSOUT_(0) ,imuSOUT_(0) ,pseudoTorqueSOUT_(0) @@ -113,8 +116,8 @@ Device::Device( const std::string& n ) << robotState_ << robotVelocity_ << motorcontrolSOUT_); - control_.fill(.0); motorcontrolSOUT_.setConstant( control_ ); + control_.fill(.0); /* --- Commands --- */ { @@ -161,7 +164,6 @@ Device::Device( const std::string& n ) void Device::setControl( const Vector& cont ) { updateControl(cont); - motorcontrolSOUT_ .setConstant( control_ ); } void Device::setSanityCheck(const bool & enableCheck) @@ -229,9 +231,9 @@ void Device::setURDFModel(const std::string &aURDFModel) } } -void Device::increment() +void Device::increment(const int& time) { - int time = motorcontrolSOUT_.getTime(); + // int time = motorcontrolSOUT_.getTime(); sotDEBUG(25) << "Time : " << time << std::endl; // Run Synchronous commands and evaluate signals outside the main @@ -260,7 +262,7 @@ void Device::increment() } /* Force the recomputation of the control. */ - controlSIN( time ); + controlSIN.recompute( time ); const Vector & controlIN = controlSIN.accessCopy(); sotDEBUG(25) << "u" < controlOut; - aDevice.getControl(controlOut); - double diff = 0, diffCont = 0, ldiff, ldiffCont; + double diff = 0, ldiff; vector< ::urdf::JointSharedPtr > urdf_joints = aDevice.getURDFJoints(); @@ -271,14 +269,10 @@ 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; } @@ -289,26 +283,20 @@ int main(int, char **) { { double lim = urdf_joints[u_index]->limits->effort; ldiff = (aControl[lctl_index] - lim); - ldiffCont = controlOut["control"].getValues()[lctl_index] - lim; diff += ldiff; - diffCont += ldiffCont; std::cout << "Torque Lim: " << lim << "\n" << "motorcontrolSOUT: " << aControl[lctl_index] << " -- " << "diff: " << ldiff << "\n" - << "controlOut: " << controlOut["control"].getValues()[lctl_index] << " -- " - << "diff: " << ldiffCont << " \n" << 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 ac954e3e49a44ebdf1418b2ad8b78f73e422d348 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 3 Sep 2019 10:39:24 +0200 Subject: [PATCH 08/43] [Device] Add entry signals position, velocity, torque and current These signals are taken by the device accordingly to the types of the actuators (defined by the yaml file). Add their gains signals in entry for a future use. Signals are registered in the entity after the reading of the yaml file to register only the needed one. --- include/sot/core/device.hh | 38 +- src/tools/device.cpp | 795 +++++++++++++----------------- unitTesting/tools/test_device.cpp | 11 +- 3 files changed, 382 insertions(+), 462 deletions(-) diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index bd4ee59d2..517e4ba02 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -65,11 +65,12 @@ namespace sot { enum ControlType { POSITION = 0, TORQUE = 1, - VELOCITY = 2 + VELOCITY = 2, + CURRENT = 3 }; const std::string ControlType_s[] = { - "POSITION", "TORQUE", "VELOCITY" + "POSITION", "TORQUE", "VELOCITY", "CURRENT" }; //@} @@ -184,8 +185,6 @@ class Device_EXPORT Device: public Entity /* --- DESTRUCTION --- */ virtual ~Device(); - virtual void setControl(const dg::Vector& cont); - /// Set control input type. virtual void setSoTControlType(const std::string &jointNames, const std::string &sotCtrlType); @@ -214,17 +213,27 @@ class Device_EXPORT Device: public Entity public: /* --- SIGNALS --- */ - /// Input signal handling the control vector - /// This entity needs a control vector to be send to the hardware. - /// The control vector can be position and effort. - /// It depends on each of the actuator - dg::SignalPtr controlSIN; + /// Input signals handling the control vector + /// Some signals can not be used by the entity + /// It depends on the type of control defined by the yaml file for each actuator + /// For position control the signal stateSIN will be used + /// For velocity control the signal velocitySIN will be used + /// For torque control the signal torqueSIN will be used + /// For current control the signal currentSIN will be used + dg::SignalPtr stateSIN; + dg::SignalPtr velocitySIN; + dg::SignalPtr torqueSIN; + dg::SignalPtr currentSIN; + dg::SignalPtr stateGainsSIN; + dg::SignalPtr velocityGainsSIN; + dg::SignalPtr torqueGainsSIN; /// \name Device current state. /// \{ /// \brief Output attitude provided by the hardware - /*! \brief The current state of the robot from the command viewpoint. */ + /// The control vector can be position, velocity, torque or current. + /// It depends on each of the actuator dg::SignalTimeDependent motorcontrolSOUT_; /// \} @@ -279,10 +288,10 @@ class Device_EXPORT Device: public Entity void setControlType(const std::string &strCtrlType, ControlType &aCtrlType); - /// \brief Compute the new control, from the given one. + /// \brief Compute the new control, from the entry signals (state, torque, velocity, current). /// When the control is in position, checks that the position is within bounds. /// When the control is in torque, checks that the torque is within bounds. - virtual void updateControl(const dg::Vector & controlIN); + virtual void createControlVector(const int& time); /// \name Signals related methods ///@{ @@ -308,6 +317,9 @@ class Device_EXPORT Device: public Entity int ParseYAMLJointSensor(YAML::Node &aJointSensors); /// @} + /// \brief Creates signal motorcontrolSOUT_ based on the control types information parsed by the YAML string. + /// Registers the signals + void RegisterSignals(); /// \brief Creates signals based on the joints information parsed by the YAML string. int UpdateSignals(); @@ -333,6 +345,8 @@ class Device_EXPORT Device: public Entity // Debug mode int debug_mode_; + std::vector control_types_; + // Intermediate index when parsing YAML file. int temperature_index_, velocity_index_, current_index_, torque_index_, diff --git a/src/tools/device.cpp b/src/tools/device.cpp index 5e72067cc..7f458e581 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -3,7 +3,7 @@ * Author: Olivier Stasse * * Please check LICENSE.txt for licensing - * + * */ /* --------------------------------------------------------------------- */ @@ -27,6 +27,8 @@ using namespace dynamicgraph::sot; using namespace dynamicgraph; #define DBGFILE "/tmp/device.txt" +#define INPUT_CONTROL_SIGNALS +#define INPUT_GAINS_SIGNALS stateGainsSIN << velocityGainsSIN << torqueGainsSIN #if 0 #define RESETDEBUG5() { std::ofstream DebugFile; \ @@ -57,27 +59,25 @@ const double Device::TIMESTEP_DEFAULT = 0.001; JointSoTHWControlType::JointSoTHWControlType(): control_index(-1) - ,urdf_index(-1) - ,temperature_index(-1) - ,velocity_index(-1) - ,current_index(-1) - ,torque_index(-1) - ,force_index(-1) - ,joint_angle_index(-1) - ,motor_angle_index(-1) -{ + , urdf_index(-1) + , temperature_index(-1) + , velocity_index(-1) + , current_index(-1) + , torque_index(-1) + , force_index(-1) + , joint_angle_index(-1) + , motor_angle_index(-1) { } /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -Device::~Device( ) -{ - for( unsigned int i=0; i - (*this, &Device::setControl, docstring)); - /* SET of SoT control input type per joint */ addCommand("setSoTControlType", command::makeCommandVoid2(*this, &Device::setSoTControlType, - command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", - "Joint name", - "Control type: [TORQUE|POSITION|VELOCITY]"))); + command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", + "Joint name", + "Control type: [TORQUE|POSITION|VELOCITY]"))); /* SET of HW control input type per joint */ addCommand("setHWControlType", command::makeCommandVoid2(*this, &Device::setHWControlType, - command::docCommandVoid2 ("Set HW control input type per joint", - "Joint name", - "Control type: [TORQUE|POSITION|VELOCITY]"))); + command::docCommandVoid2 ("Set HW control input type per joint", + "Joint name", + "Control type: [TORQUE|POSITION|VELOCITY]"))); docstring = - "\n" - " Enable/Disable sanity checks\n" - "\n"; + "\n" + " Enable/Disable sanity checks\n" + "\n"; addCommand("setSanityCheck", new command::Setter (*this, &Device::setSanityCheck, docstring)); @@ -161,69 +155,80 @@ Device::Device( const std::string& n ) } } -void Device::setControl( const Vector& cont ) -{ - updateControl(cont); +void Device::RegisterSignals() { + SignalArray sigArray; + for (unsigned int i=0; i urdf_links; model_->getLinks(urdf_links); - for (unsigned j=0; j child_joints = urdf_links[j]->child_joints; - urdf_joints_.insert(urdf_joints_.end(), boost::make_move_iterator(child_joints.begin()), + urdf_joints_.insert(urdf_joints_.end(), boost::make_move_iterator(child_joints.begin()), boost::make_move_iterator(child_joints.end())); } std::cout << "urdf_joints_.size(): " << urdf_joints_.size() << std::endl; - for(unsigned int i=0;iname].urdf_index=i; - if (debug_mode_>1) - { + for (unsigned int i = 0; i < urdf_joints_.size(); i++) { + jointDevices_[urdf_joints_[i]->name].urdf_index = i; + if (debug_mode_ > 1) { std::cout << "jointDevices_ index: " << i << " model_.names[i]: " << urdf_joints_[i]->name << std::endl; @@ -231,232 +236,205 @@ void Device::setURDFModel(const std::string &aURDFModel) } } -void Device::increment(const int& time) -{ +void Device::increment(const int& time) { // int time = motorcontrolSOUT_.getTime(); sotDEBUG(25) << "Time : " << time << std::endl; // Run Synchronous commands and evaluate signals outside the main // connected component of the graph. - try - { - periodicCallBefore_.run(time+1); - } - catch (std::exception& e) - { + try { + periodicCallBefore_.run(time + 1); + } catch (std::exception& e) { dgRTLOG() << "exception caught while running periodical commands (before): " << e.what () << std::endl; - } - catch (const char* str) - { + } catch (const char* str) { dgRTLOG() << "exception caught while running periodical commands (before): " << str << std::endl; - } - catch (...) - { + } catch (...) { dgRTLOG() << "unknown exception caught while" << " running periodical commands (before)" << std::endl; } - /* Force the recomputation of the control. */ - controlSIN.recompute( time ); - const Vector & controlIN = controlSIN.accessCopy(); - sotDEBUG(25) << "u" <second.control_index; int u_index = it_control_type->second.urdf_index; - if (lctl_index==-1) - { - if (debug_mode_>1) - { + if (lctl_index == -1) { + if (debug_mode_ > 1) { std::cerr << "No control index for joint " << urdf_joints_[u_index]->name << std::endl; } break; - } - if (u_index!=-1) - { - control_[lctl_index] = controlIN[lctl_index]; - - // Check Position limits. - if ((it_control_type->second.SoTcontrol==POSITION) && - (urdf_joints_[u_index]->limits)) - { - double lowerLim = urdf_joints_[u_index]->limits->lower; - double upperLim = urdf_joints_[u_index]->limits->upper; - if (control_[lctl_index] < lowerLim) - { - control_[lctl_index] = lowerLim; + } + if (u_index != -1) { + // POSITION -> stateSIN + Check Position limits. + if (it_control_type->second.SoTcontrol == POSITION) { + control_[lctl_index] = state[lctl_index]; + + if (urdf_joints_[u_index]->limits) { + double lowerLim = urdf_joints_[u_index]->limits->lower; + double upperLim = urdf_joints_[u_index]->limits->upper; + if (control_[lctl_index] < lowerLim) { + control_[lctl_index] = lowerLim; + } + else if (control_[lctl_index] > upperLim) { + control_[lctl_index] = upperLim; + } } - else if (control_[lctl_index] > upperLim) - { - control_[lctl_index] = upperLim; - } } - // Check Torque limits. - if ((it_control_type->second.SoTcontrol==TORQUE) && - (urdf_joints_[u_index]->limits)) - { - double lim = urdf_joints_[u_index]->limits->effort; - if (control_[lctl_index] < -lim) - { - control_[lctl_index] = -lim; + // TORQUE -> torqueSIN + Check Torque limits. + else if (it_control_type->second.SoTcontrol == TORQUE) { + control_[lctl_index] = torque[lctl_index]; + + if (urdf_joints_[u_index]->limits) { + double lim = urdf_joints_[u_index]->limits->effort; + if (control_[lctl_index] < -lim) { + control_[lctl_index] = -lim; + } + else if (control_[lctl_index] > lim) { + control_[lctl_index] = lim; + } } - else if (control_[lctl_index] > lim) - { - control_[lctl_index] = lim; - } + } + // VELOCITY -> velSIN + else if (it_control_type->second.SoTcontrol == VELOCITY) { + control_[lctl_index] = vel[lctl_index]; + } + // CURRENT -> currentSIN + else if (it_control_type->second.SoTcontrol == CURRENT) { + control_[lctl_index] = current[lctl_index]; } } - } + } + if (sanityCheck_ && control_.hasNaN()) { + dgRTLOG () << "Device::updateControl: Control has NaN values: " << '\n' + << control_.transpose() << '\n'; + return; + } } -int Device::ParseYAMLString(const std::string & aYamlString) -{ +int Device::ParseYAMLString(const std::string & aYamlString) { YAML::Node map_global = YAML::Load(aYamlString); YAML::Node map_sot_controller = map_global["sot_controller"]; - for (YAML::const_iterator it=map_sot_controller.begin(); - it!=map_sot_controller.end(); - it++) - { + for (YAML::const_iterator it = map_sot_controller.begin(); + it != map_sot_controller.end(); + it++) { std::string name_elt_of_sot_controller = it->first.as(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "key:" << name_elt_of_sot_controller << std::endl; - } + } YAML::Node elt_of_sot_controller = it->second; - - if (name_elt_of_sot_controller=="joint_names") - { - int r=ParseYAMLMapHardwareJointNames(elt_of_sot_controller); - if (r<0) return r; - } - else if (name_elt_of_sot_controller=="map_rc_to_sot_device") - { - int r=ParseYAMLJointSensor(elt_of_sot_controller); - if (r<0) return r; - } - else if (name_elt_of_sot_controller=="control_mode") - { - int r=ParseYAMLMapHardwareControlMode(elt_of_sot_controller); - if (r<0) return r; - } - else if (name_elt_of_sot_controller.find("sensor") != std::string::npos) - { - int r=ParseYAMLSensors(elt_of_sot_controller); - if (r<0) return r; + + if (name_elt_of_sot_controller == "joint_names") { + int r = ParseYAMLMapHardwareJointNames(elt_of_sot_controller); + if (r < 0) return r; + } else if (name_elt_of_sot_controller == "map_rc_to_sot_device") { + int r = ParseYAMLJointSensor(elt_of_sot_controller); + if (r < 0) return r; + } else if (name_elt_of_sot_controller == "control_mode") { + int r = ParseYAMLMapHardwareControlMode(elt_of_sot_controller); + if (r < 0) return r; + } else if (name_elt_of_sot_controller.find("sensor") != std::string::npos) { + int r = ParseYAMLSensors(elt_of_sot_controller); + if (r < 0) return r; } } + RegisterSignals(); UpdateSignals(); return 0; } -int Device::ParseYAMLJointSensor(YAML::Node &aJointSensor) -{ - for (YAML::const_iterator it=aJointSensor.begin(); - it!=aJointSensor.end(); - it++) - { +int Device::ParseYAMLJointSensor(YAML::Node &aJointSensor) { + for (YAML::const_iterator it = aJointSensor.begin(); + it != aJointSensor.end(); + it++) { std::string aSensor = it->first.as(); - if (debug_mode_>1) - { - std::cout << "Found " << aSensor<< std::endl; + if (debug_mode_ > 1) { + std::cout << "Found " << aSensor << std::endl; } JointSHWControlType_iterator it_control_type; - for(it_control_type = jointDevices_.begin(); - it_control_type != jointDevices_.end(); - it_control_type++) - { - if (aSensor=="temperature") - { + for (it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) { + if (aSensor == "temperature") { it_control_type->second.temperature_index = temperature_index_; temperature_index_++; - } - else if (aSensor=="velocities") - { + } else if (aSensor == "velocities") { it_control_type->second.velocity_index = velocity_index_; velocity_index_++; - } - else if (aSensor=="currents") - { + } else if (aSensor == "currents") { it_control_type->second.current_index = current_index_; current_index_++; - } - else if (aSensor=="torques") - { + } else if (aSensor == "torques") { it_control_type->second.torque_index = torque_index_; torque_index_++; - } - else if (aSensor=="forces") - { + } else if (aSensor == "forces") { it_control_type->second.force_index = force_index_; force_index_++; - } - else if (aSensor=="joint_angles") - { + } else if (aSensor == "joint_angles") { it_control_type->second.joint_angle_index = joint_angle_index_; joint_angle_index_++; - } - else if (aSensor=="motor_angles") - { + } else if (aSensor == "motor_angles") { it_control_type->second.motor_angle_index = motor_angle_index_; motor_angle_index_++; } @@ -465,96 +443,83 @@ int Device::ParseYAMLJointSensor(YAML::Node &aJointSensor) return 0; } -int Device::ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name) -{ - if (debug_mode_>1) - { +int Device::ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name) { + if (control_.size() != map_joint_name.size()) { + control_.resize(map_joint_name.size()); + } + + if (debug_mode_ > 1) { std::cout << "map_joint_name.size(): " - << map_joint_name.size() << std::endl; + << map_joint_name.size() << std::endl; std::cout << map_joint_name << std::endl; } - for (unsigned int i=0;i(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "-- Joint: " << jointName << " has index: " << i << std::endl; - } - setControlPos(jointName,i); + } + setControlPos(jointName, i); } return 0; } -int Device::ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode) -{ - if (debug_mode_>1) - { +int Device::ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode) { + if (debug_mode_ > 1) { std::cout << "map_control_mode.size(): " - << map_control_mode.size() << std::endl; + << map_control_mode.size() << std::endl; std::cout << map_control_mode << std::endl; } - if (map_control_mode.size() == 0) - { + if (map_control_mode.size() == 0) { std::string value = map_control_mode.as(); JointSHWControlType_iterator it_control_type; - for(it_control_type = jointDevices_.begin(); - it_control_type != jointDevices_.end(); - it_control_type++) - { - int u_index = it_control_type->second.urdf_index; + for (it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) { + int u_index = it_control_type->second.urdf_index; setSoTControlType(urdf_joints_[u_index]->name, value); - } - if (debug_mode_>1) - { + control_types_.push_back(value); + } + if (debug_mode_ > 1) { std::cout << "All joints are controlled in: " << value << std::endl; } return 0; } - for (YAML::const_iterator it=map_control_mode.begin(); - it!=map_control_mode.end(); - it++) - { + for (YAML::const_iterator it = map_control_mode.begin(); + it != map_control_mode.end(); + it++) { std::string jointName = it->first.as(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "joint name: " << jointName << std::endl; } YAML::Node aNode = it->second; - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "Type of value: " << aNode.Type() << std::endl; } - for (YAML::const_iterator it2=aNode.begin(); - it2!=aNode.end(); - it2++) - { + for (YAML::const_iterator it2 = aNode.begin(); + it2 != aNode.end(); + it2++) { std::string aKey = it2->first.as(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "-- key:" << aKey << std::endl; } - if (aKey=="hw_control_mode") - { + if (aKey == "hw_control_mode") { std::string value = it2->second.as(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "-- Value: " << value << std::endl; } - setHWControlType(jointName,value); - } - else if (aKey=="ros_control_mode") - { + setHWControlType(jointName, value); + } else if (aKey == "ros_control_mode") { std::string value = it2->second.as(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "-- Value: " << value << std::endl; - } - setSoTControlType(jointName,value); + } + setSoTControlType(jointName, value); + control_types_.push_back(value); } } } @@ -562,29 +527,22 @@ int Device::ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode) } /* Sensor signals */ -int Device::ParseYAMLSensors(YAML::Node &map_sensors) -{ - if (map_sensors.IsNull()) - { +int Device::ParseYAMLSensors(YAML::Node &map_sensors) { + if (map_sensors.IsNull()) { std::cerr << "Device::ParseYAMLString: No sensor detected in YamlString " << std::endl; return -1; } std::string sensor_name = map_sensors.as(); - if (debug_mode_>1) - { + if (debug_mode_ > 1) { std::cout << "-- sensor_name:" << sensor_name << std::endl; } - if (sensor_name.find("ft") != std::string::npos) - { + if (sensor_name.find("ft") != std::string::npos) { CreateAForceSignal(sensor_name); } - else if (sensor_name.find("imu") != std::string::npos) - { + else if (sensor_name.find("imu") != std::string::npos) { CreateAnImuSignal(sensor_name); - } - else - { + } else { std::cerr << "The sensor " << sensor_name << " is not recognized" << std::endl; return 1; @@ -593,21 +551,19 @@ int Device::ParseYAMLSensors(YAML::Node &map_sensors) } -void Device::CreateAForceSignal(const std::string & force_sensor_name) -{ +void Device::CreateAForceSignal(const std::string & force_sensor_name) { dynamicgraph::Signal * aForceSOUT_; /* --- SIGNALS --- */ - aForceSOUT_ = new Signal("Device("+getName()+")::output(vector6)::" + + aForceSOUT_ = new Signal("Device(" + getName() + ")::output(vector6)::" + force_sensor_name); forcesSOUT_.push_back(aForceSOUT_); signalRegistration(*aForceSOUT_); } -void Device::CreateAnImuSignal(const std::string &imu_sensor_name) -{ +void Device::CreateAnImuSignal(const std::string &imu_sensor_name) { IMUSOUT * anImuSOUT_; /* --- SIGNALS --- */ - anImuSOUT_ = new IMUSOUT(imu_sensor_name,getName()); + anImuSOUT_ = new IMUSOUT(imu_sensor_name, getName()); imuSOUT_.push_back(anImuSOUT_); signalRegistration(anImuSOUT_->attitudeSOUT << anImuSOUT_->accelerometerSOUT @@ -615,34 +571,28 @@ void Device::CreateAnImuSignal(const std::string &imu_sensor_name) } -int Device::UpdateSignals() -{ - if ((torque_index_!=0) && (pseudoTorqueSOUT_!=0)) - { - pseudoTorqueSOUT_ = new Signal("Device("+getName()+")::output(vector)::ptorque"); +int Device::UpdateSignals() { + if ((torque_index_ != 0) && (pseudoTorqueSOUT_ != 0)) { + pseudoTorqueSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::ptorque"); } // if ((force_index_!=0) && (forcesSOUT_.size()!=0)) // { // forcesSOUT_ = new Signal("Device("+getName()+")::output(vector)::forces"); // } - if ((current_index_!=0) && (currentsSOUT_!=0)) - { - currentsSOUT_ = new Signal("Device("+getName()+")::output(vector)::currents"); + if ((current_index_ != 0) && (currentsSOUT_ != 0)) { + currentsSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::currents"); } - if ((temperature_index_!=0) && (temperatureSOUT_!=0)) - { - temperatureSOUT_ = new Signal("Device("+getName()+")::output(vector)::temperatures"); + if ((temperature_index_ != 0) && (temperatureSOUT_ != 0)) { + temperatureSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::temperatures"); } - if ((motor_angle_index_!=0) && (motor_anglesSOUT_!=0)) - { - motor_anglesSOUT_ = new Signal("Device("+getName()+")::output(vector)::motor_angles"); + if ((motor_angle_index_ != 0) && (motor_anglesSOUT_ != 0)) { + motor_anglesSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::motor_angles"); } - if ((joint_angle_index_!=0) && (joint_anglesSOUT_!=0)) - { - joint_anglesSOUT_ = new Signal("Device("+getName()+")::output(vector)::joint_angles"); + if ((joint_angle_index_ != 0) && (joint_anglesSOUT_ != 0)) { + joint_anglesSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::joint_angles"); } return 0; @@ -652,29 +602,24 @@ int Device::UpdateSignals() /* --- DISPLAY ------------------------------------------------------------ */ -void Device::display ( std::ostream& os ) const -{ - os < &SensorsIn, int t) -{ +void Device::setSensorsForce(map &SensorsIn, int t) { Eigen::Matrix dgforces; - + sotDEBUGIN(15); - map::iterator it; + map::iterator it; it = SensorsIn.find("forces"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { // Implements force recollection. const vector& forcesIn = it->second.getValues(); - for(std::size_t i=0;isetConstant(dgforces); @@ -684,25 +629,20 @@ void Device::setSensorsForce(map &SensorsIn, int t) sotDEBUGIN(15); } -void Device::setSensorsIMU(map &SensorsIn, int t) -{ - Eigen::Matrix aVector3d; - - map::iterator it; +void Device::setSensorsIMU(map &SensorsIn, int t) { + Eigen::Matrix aVector3d; + + map::iterator it; //TODO: Confirm if this can be made quaternion - for(std::size_t k=0;k& attitude = it->second.getValues (); Eigen::Matrix pose; - - for (unsigned int i = 0; i < 3; ++i) - { - for (unsigned int j = 0; j < 3; ++j) - { + + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { pose (i, j) = attitude[i * 3 + j]; } } @@ -711,24 +651,20 @@ void Device::setSensorsIMU(map &SensorsIn, int t) } it = SensorsIn.find("accelerometer_0"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const vector& accelerometer = SensorsIn ["accelerometer_0"].getValues (); - for (std::size_t i=0; i<3; ++i) - { + for (std::size_t i = 0; i < 3; ++i) { aVector3d(i) = accelerometer [i]; } imuSOUT_[k]->accelerometerSOUT.setConstant (aVector3d); imuSOUT_[k]->accelerometerSOUT.setTime (t); } - + it = SensorsIn.find("gyrometer_0"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const vector& gyrometer = SensorsIn ["gyrometer_0"].getValues (); - for (std::size_t i=0; i<3; ++i) - { + for (std::size_t i = 0; i < 3; ++i) { aVector3d(i) = gyrometer [i]; } imuSOUT_[k]->gyrometerSOUT.setConstant (aVector3d); @@ -737,27 +673,22 @@ void Device::setSensorsIMU(map &SensorsIn, int t) } } -void Device::setSensorsEncoders(map &SensorsIn, int t) -{ +void Device::setSensorsEncoders(map &SensorsIn, int t) { dg::Vector dgRobotState, motor_angles, joint_angles; - map::iterator it; + map::iterator it; - if (motor_anglesSOUT_!=0) - { + if (motor_anglesSOUT_ != 0) { it = SensorsIn.find("motor-angles"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const vector& anglesIn = it->second.getValues(); dgRobotState.resize (anglesIn.size () + 6); motor_angles.resize(anglesIn.size ()); - for (unsigned i = 0; i < 6; ++i) - { + for (unsigned i = 0; i < 6; ++i) { dgRobotState (i) = 0.; } - for (unsigned i = 0; i < anglesIn.size(); ++i) - { + for (unsigned i = 0; i < anglesIn.size(); ++i) { dgRobotState (i + 6) = anglesIn[i]; - motor_angles(i)= anglesIn[i]; + motor_angles(i) = anglesIn[i]; } robotState_.setConstant(dgRobotState); robotState_.setTime(t); @@ -766,15 +697,12 @@ void Device::setSensorsEncoders(map &SensorsIn, int } } - if (joint_anglesSOUT_!=0) - { + if (joint_anglesSOUT_ != 0) { it = SensorsIn.find("joint-angles"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const vector& joint_anglesIn = it->second.getValues(); joint_angles.resize (joint_anglesIn.size () ); - for (unsigned i = 0; i < joint_anglesIn.size(); ++i) - { + for (unsigned i = 0; i < joint_anglesIn.size(); ++i) { joint_angles (i) = joint_anglesIn[i]; } joint_anglesSOUT_->setConstant(joint_angles); @@ -783,23 +711,19 @@ void Device::setSensorsEncoders(map &SensorsIn, int } } -void Device::setSensorsVelocities(map &SensorsIn, int t) -{ +void Device::setSensorsVelocities(map &SensorsIn, int t) { dg::Vector dgRobotVelocity; - - map::iterator it; - + + map::iterator it; + it = SensorsIn.find("velocities"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const vector& velocitiesIn = it->second.getValues(); dgRobotVelocity.resize (velocitiesIn.size () + 6); - for (unsigned i = 0; i < 6; ++i) - { + for (unsigned i = 0; i < 6; ++i) { dgRobotVelocity (i) = 0.; } - for (unsigned i = 0; i < velocitiesIn.size(); ++i) - { + for (unsigned i = 0; i < velocitiesIn.size(); ++i) { dgRobotVelocity (i + 6) = velocitiesIn[i]; } robotVelocity_.setConstant(dgRobotVelocity); @@ -807,21 +731,17 @@ void Device::setSensorsVelocities(map &SensorsIn, in } } -void Device::setSensorsTorquesCurrents(map &SensorsIn, int t) -{ - dg::Vector torques,currents; - - map::iterator it; +void Device::setSensorsTorquesCurrents(map &SensorsIn, int t) { + dg::Vector torques, currents; - if (pseudoTorqueSOUT_!=0) - { + map::iterator it; + + if (pseudoTorqueSOUT_ != 0) { it = SensorsIn.find("torques"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const std::vector& vtorques = SensorsIn["torques"].getValues(); torques.resize(vtorques.size()); - for(std::size_t i = 0; i < vtorques.size(); ++i) - { + for (std::size_t i = 0; i < vtorques.size(); ++i) { torques (i) = vtorques [i]; } pseudoTorqueSOUT_->setConstant(torques); @@ -829,15 +749,12 @@ void Device::setSensorsTorquesCurrents(map &SensorsI } } - if (currentsSOUT_!=0) - { + if (currentsSOUT_ != 0) { it = SensorsIn.find("currents"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const std::vector& vcurrents = SensorsIn["currents"].getValues(); currents.resize(vcurrents.size()); - for(std::size_t i = 0; i < vcurrents.size(); ++i) - { + for (std::size_t i = 0; i < vcurrents.size(); ++i) { currents (i) = vcurrents[i]; } currentsSOUT_->setConstant(currents); @@ -846,20 +763,16 @@ void Device::setSensorsTorquesCurrents(map &SensorsI } } -void Device::setSensorsGains(map &SensorsIn, int t) -{ +void Device::setSensorsGains(map &SensorsIn, int t) { dg::Vector p_gains, d_gains; - - map::iterator it; - if (p_gainsSOUT_!=0) - { + + map::iterator it; + if (p_gainsSOUT_ != 0) { it = SensorsIn.find("p_gains"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const std::vector& vp_gains = SensorsIn["p_gains"].getValues(); p_gains.resize(vp_gains.size()); - for(std::size_t i = 0; i < vp_gains.size(); ++i) - { + for (std::size_t i = 0; i < vp_gains.size(); ++i) { p_gains (i) = vp_gains[i]; } p_gainsSOUT_->setConstant(p_gains); @@ -867,15 +780,12 @@ void Device::setSensorsGains(map &SensorsIn, int t) } } - if (d_gainsSOUT_!=0) - { + if (d_gainsSOUT_ != 0) { it = SensorsIn.find("d_gains"); - if (it!=SensorsIn.end()) - { + if (it != SensorsIn.end()) { const std::vector& vd_gains = SensorsIn["d_gains"].getValues(); d_gains.resize(vd_gains.size()); - for(std::size_t i = 0; i < vd_gains.size(); ++i) - { + for (std::size_t i = 0; i < vd_gains.size(); ++i) { d_gains (i) = vd_gains[i]; } d_gainsSOUT_->setConstant(d_gains); @@ -884,39 +794,34 @@ void Device::setSensorsGains(map &SensorsIn, int t) } } -void Device::setSensors(map &SensorsIn) -{ +void Device::setSensors(map &SensorsIn) { sotDEBUGIN(25) ; int t = motorcontrolSOUT_.getTime () + 1; - setSensorsForce(SensorsIn,t); - setSensorsIMU(SensorsIn,t); - setSensorsEncoders(SensorsIn,t); - setSensorsVelocities(SensorsIn,t); - setSensorsTorquesCurrents(SensorsIn,t); - setSensorsGains(SensorsIn,t); + setSensorsForce(SensorsIn, t); + setSensorsIMU(SensorsIn, t); + setSensorsEncoders(SensorsIn, t); + setSensorsVelocities(SensorsIn, t); + setSensorsTorquesCurrents(SensorsIn, t); + setSensorsGains(SensorsIn, t); sotDEBUGOUT(25); } -void Device::setupSetSensors(map &SensorsIn) -{ +void Device::setupSetSensors(map &SensorsIn) { setSensors (SensorsIn); } -void Device::nominalSetSensors(map &SensorsIn) -{ +void Device::nominalSetSensors(map &SensorsIn) { setSensors (SensorsIn); } -void Device::cleanupSetSensors(map &SensorsIn) -{ +void Device::cleanupSetSensors(map &SensorsIn) { setSensors (SensorsIn); } -dg::Vector& Device::getControl(dg::Vector &controlOut, const int& t) -{ +dg::Vector& Device::getControl(dg::Vector &controlOut, const int& t) { ODEBUG5FULL("start"); sotDEBUGIN(25); @@ -924,7 +829,7 @@ dg::Vector& Device::getControl(dg::Vector &controlOut, const int& t) increment(t); sotDEBUG (25) << "control = " << control_ << std::endl; - ODEBUG5FULL("control = "<< control_); + ODEBUG5FULL("control = " << control_); controlOut = control_; diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_device.cpp index cbe1eb0bf..8cff9b756 100644 --- a/unitTesting/tools/test_device.cpp +++ b/unitTesting/tools/test_device.cpp @@ -237,11 +237,12 @@ int main(int, char **) { if (ReadYAMLFILE(aDevice) < 0) return -1; - /// Fix constant vector for the control entry - dg::Vector aControlVector(30); - for (unsigned int i = 0; i < 30; i++) - aControlVector[i] = -0.5; - aDevice.controlSIN.setConstant(aControlVector); + /// Fix constant vector for the control entry in position + dg::Vector aStateVector(30); + for (unsigned int i = 0; i < 30; i++){ + aStateVector[i] = -0.5; + } + aDevice.stateSIN.setConstant(aStateVector); // entry signal in position for (unsigned int i = 0; i < 2000; i++) aDevice.motorcontrolSOUT_.recompute(i); From 99757087d549e8e5f23ff82309e872de938e5302 Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 3 Sep 2019 10:40:23 +0200 Subject: [PATCH 09/43] [Device] Format files using google style --- include/sot/core/device.hh | 15 +++++----- src/tools/device.cpp | 48 +++++++++++++------------------ unitTesting/tools/test_device.cpp | 48 +++++++++++++------------------ 3 files changed, 47 insertions(+), 64 deletions(-) diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 517e4ba02..a12a3f124 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -49,12 +49,12 @@ namespace sot { /* --- API ------------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -#if defined (WIN32) +#if defined (WIN32) # if defined (device_EXPORTS) # define Device_EXPORT __declspec(dllexport) -# else +# else # define Device_EXPORT __declspec(dllimport) -# endif +# endif #else # define Device_EXPORT #endif @@ -137,8 +137,7 @@ JointSHWControlType_iterator; /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class Device_EXPORT Device: public Entity -{ +class Device_EXPORT Device: public Entity { public: static const std::string CLASS_NAME; @@ -219,7 +218,7 @@ class Device_EXPORT Device: public Entity /// For position control the signal stateSIN will be used /// For velocity control the signal velocitySIN will be used /// For torque control the signal torqueSIN will be used - /// For current control the signal currentSIN will be used + /// For current control the signal currentSIN will be used dg::SignalPtr stateSIN; dg::SignalPtr velocitySIN; dg::SignalPtr torqueSIN; @@ -279,7 +278,7 @@ class Device_EXPORT Device: public Entity void nominalSetSensors(std::map &sensorsIn); void cleanupSetSensors(std::map &sensorsIn); ///@} - + protected: /// \brief Provides to the robot the control information (callback signal motorcontrolSOUT_). @@ -316,7 +315,7 @@ class Device_EXPORT Device: public Entity /// Parse YAML for joint sensors from YAML-cpp node. int ParseYAMLJointSensor(YAML::Node &aJointSensors); /// @} - + /// \brief Creates signal motorcontrolSOUT_ based on the control types information parsed by the YAML string. /// Registers the signals void RegisterSignals(); diff --git a/src/tools/device.cpp b/src/tools/device.cpp index 7f458e581..db36f50a3 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -156,30 +156,27 @@ Device::Device( const std::string& n ) } void Device::RegisterSignals() { - SignalArray sigArray; - for (unsigned int i=0; i sigArray; + for (unsigned int i = 0; i < control_types_.size(); i++) { + if (control_types_[i] == "POSITION") { sigArray << stateSIN; - motorcontrolSOUT_.addDependency(stateSIN); - } - else if (control_types_[i]=="VELOCITY"){ + motorcontrolSOUT_.addDependency(stateSIN); + } else if (control_types_[i] == "VELOCITY") { sigArray << velocitySIN; motorcontrolSOUT_.addDependency(velocitySIN); - } - else if (control_types_[i]=="TORQUE"){ + } else if (control_types_[i] == "TORQUE") { sigArray << torqueSIN; motorcontrolSOUT_.addDependency(torqueSIN); - } - else if (control_types_[i]=="CURRENT"){ + } else if (control_types_[i] == "CURRENT") { sigArray << currentSIN; motorcontrolSOUT_.addDependency(currentSIN); - } + } } - + signalRegistration( sigArray - << robotState_ - << robotVelocity_ - << motorcontrolSOUT_); + << robotState_ + << robotVelocity_ + << motorcontrolSOUT_); } void Device::setSanityCheck(const bool & enableCheck) { @@ -288,23 +285,20 @@ void Device::createControlVector(const int& time) { Vector vel; Vector torque; Vector current; - for (unsigned int i=0; ilimits->upper; if (control_[lctl_index] < lowerLim) { control_[lctl_index] = lowerLim; - } - else if (control_[lctl_index] > upperLim) { + } else if (control_[lctl_index] > upperLim) { control_[lctl_index] = upperLim; } } @@ -346,8 +339,7 @@ void Device::createControlVector(const int& time) { double lim = urdf_joints_[u_index]->limits->effort; if (control_[lctl_index] < -lim) { control_[lctl_index] = -lim; - } - else if (control_[lctl_index] > lim) { + } else if (control_[lctl_index] > lim) { control_[lctl_index] = lim; } } diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_device.cpp index 8cff9b756..f95872766 100644 --- a/unitTesting/tools/test_device.cpp +++ b/unitTesting/tools/test_device.cpp @@ -33,7 +33,7 @@ void CreateYAMLFILE() { yn_map_rc_to_sot_device = yn_map_sot_controller["map_rc_to_sot_device"]; yn_map_joint_names = yn_map_sot_controller["joint_names"]; yn_control_mode = yn_map_sot_controller["control_mode"]; - + yn_map_rc_to_sot_device["motor-angles"] = "motor-angles"; yn_map_rc_to_sot_device["joint-angles"] = "joint-angles"; yn_map_rc_to_sot_device["velocities"] = "velocities"; @@ -43,7 +43,7 @@ void CreateYAMLFILE() { yn_map_rc_to_sot_device["accelerometer_0"] = "accelerometer_0"; yn_map_rc_to_sot_device["gyrometer_0"] = "gyrometer_0"; yn_map_rc_to_sot_device["control"] = "control"; - + yn_map_joint_names.push_back("waist"); yn_map_joint_names.push_back("LLEG_HIP_P"); yn_map_joint_names.push_back("LLEG_HIP_R"); @@ -77,7 +77,7 @@ void CreateYAMLFILE() { yn_control_mode["waist"]; yn_control_mode["waist"]["ros_control_mode"] = "POSITION"; - + yn_control_mode["LLEG_HIP_P"]; yn_control_mode["LLEG_HIP_P"]["ros_control_mode"] = "POSITION"; @@ -186,22 +186,22 @@ void CreateYAMLFILE() { int ReadYAMLFILE(dg::sot::Device &aDevice) { // Reflect how the data are splitted in two yaml files in the sot // Comment and use the commented code to use the above yaml file - std::ifstream yaml_file_controller("../../unitTesting/tools/sot_controller.yaml"); + std::ifstream yaml_file_controller("../../unitTesting/tools/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/tools/sot_params.yaml"); + std::ifstream yaml_file_params("../../unitTesting/tools/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); - // Uncomment if you want to use the above yaml file + // Uncomment if you want to use the above yaml file // All the data are in one file, which does not reflect reality - - // std::ifstream yaml_file("map_hs_sot_gen.yaml"); + + // std::ifstream yaml_file("map_hs_sot_gen.yaml"); // std::string yaml_string; // yaml_string.assign((std::istreambuf_iterator(yaml_file) ), // (std::istreambuf_iterator() ) ); @@ -230,18 +230,18 @@ int main(int, char **) { aDevice.setDebugMode(debug_mode); aDevice.setURDFModel(robot_description); - // Uncomment if you want to create and use the above yaml file + // Uncomment if you want to create and use the above yaml file // All the data are in one file, which does not reflect reality - // CreateYAMLFILE(); + // CreateYAMLFILE(); if (ReadYAMLFILE(aDevice) < 0) return -1; /// Fix constant vector for the control entry in position dg::Vector aStateVector(30); - for (unsigned int i = 0; i < 30; i++){ + for (unsigned int i = 0; i < 30; i++) { aStateVector[i] = -0.5; - } + } aDevice.stateSIN.setConstant(aStateVector); // entry signal in position for (unsigned int i = 0; i < 2000; i++) @@ -257,42 +257,34 @@ 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 if (it_control_type->second.SoTcontrol == dgsot::TORQUE) - { - if (u_index != -1 && (urdf_joints[u_index]->limits)) - { + } else if (it_control_type->second.SoTcontrol == dgsot::TORQUE) { + if (u_index != -1 && (urdf_joints[u_index]->limits)) { double lim = urdf_joints[u_index]->limits->effort; ldiff = (aControl[lctl_index] - lim); diff += ldiff; std::cout << "Torque Lim: " << lim << "\n" << "motorcontrolSOUT: " << aControl[lctl_index] << " -- " - << "diff: " << ldiff << "\n" + << "diff: " << ldiff << "\n" << std::endl; } - } - else - { + } else { std::cout << "motorcontrolSOUT: " << aControl[lctl_index] << std::endl; } } From 92220ee354ef51546b526c4fe8659b6ca377907b Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Tue, 5 Nov 2019 14:27:12 +0100 Subject: [PATCH 10/43] [Device] Change name to generic-device --- include/CMakeLists.txt | 1 + include/sot/core/device.hh | 499 +++---- include/sot/core/generic-device.hh | 373 ++++++ src/CMakeLists.txt | 3 +- src/tools/device.cpp | 1174 +++++++---------- src/tools/generic-device.cpp | 831 ++++++++++++ unitTesting/CMakeLists.txt | 2 +- ...est_device.cpp => test_generic_device.cpp} | 6 +- 8 files changed, 1847 insertions(+), 1042 deletions(-) create mode 100644 include/sot/core/generic-device.hh create mode 100644 src/tools/generic-device.cpp rename unitTesting/tools/{test_device.cpp => test_generic_device.cpp} (98%) diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 845b64a22..b4cf56a89 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -14,6 +14,7 @@ SET(NEWHEADERS sot/core/debug.hh sot/core/derivator.hh sot/core/device.hh + sot/core/generic-device.hh sot/core/event.hh sot/core/exception-abstract.hh sot/core/exception-dynamic.hh diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index a12a3f124..6994cc33a 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -1,11 +1,9 @@ /* - * Copyright 2010-2018, CNRS + * Copyright 2010, * Florent Lamiraux - * Olivier Stasse * * CNRS * - * See LICENSE.txt */ #ifndef SOT_DEVICE_HH @@ -15,356 +13,173 @@ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -#include -#include - -/// URDF DOM -#include -#include -#include - -/// YAML CPP -#include - /* -- MaaL --- */ #include namespace dg = dynamicgraph; /* SOT */ -/// dg #include #include -/// sot-core #include "sot/core/periodic-call.hh" #include #include "sot/core/api.hh" -#include - -namespace dgsot = dynamicgraph::sot; -namespace dg = dynamicgraph; namespace dynamicgraph { -namespace sot { - -/* --------------------------------------------------------------------- */ -/* --- API ------------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -#if defined (WIN32) -# if defined (device_EXPORTS) -# define Device_EXPORT __declspec(dllexport) -# else -# define Device_EXPORT __declspec(dllimport) -# endif -#else -# define Device_EXPORT -#endif - - -/// Specifies the nature of one joint control -/// It is used for the hardware side. -enum ControlType { - POSITION = 0, - TORQUE = 1, - VELOCITY = 2, - CURRENT = 3 -}; - -const std::string ControlType_s[] = { - "POSITION", "TORQUE", "VELOCITY", "CURRENT" -}; - -//@} - -/// \brief Store information on each joint. -struct JointSoTHWControlType { - /// Defines the control from the Stack-of-Tasks side (for instance, position) - ControlType SoTcontrol; - /// Defines the control from the hardware side. - ControlType HWcontrol; - /// Position of the joint in the control vector. - int control_index; - /// Position of the joint in the URDF index. - int urdf_index; - - /// Various indexes for the sensor signals. - /// This may vary if some joints does not support this feature. - ///@{ - /// Position of the joint in the temperature vector - int temperature_index; - - /// Position of the joint in the velocity vector - int velocity_index; - - /// Position of the joint in the current vector - int current_index; - - /// Position of the joint in the torque vector - int torque_index; - - /// Position of the joint in the force vector - int force_index; - - /// Position of the joint in the joint-angles vector - int joint_angle_index; - - /// Position of the joint in the motor-angles vector - int motor_angle_index; - - ///@} - JointSoTHWControlType(); -}; - -struct IMUSOUT { - std::string imu_sensor_name; - dg::Signal attitudeSOUT; - dg::Signal accelerometerSOUT; - dg::Signal gyrometerSOUT; - IMUSOUT(const std::string &limu_sensor_name, - const std::string &device_name): - imu_sensor_name(limu_sensor_name) - , attitudeSOUT("Device(" + device_name + - ")::output(vector6)::" + imu_sensor_name + "_attitudeSOUT") - , accelerometerSOUT("Device(" + device_name + - ")::output(vector3)::" + imu_sensor_name + "_accelerometerSOUT") - , gyrometerSOUT("Device(" + device_name + - ")::output(vector3)::" + imu_sensor_name + "_gyrometerSOUT") - {} -}; - -typedef std::map::iterator -JointSHWControlType_iterator; -/* --------------------------------------------------------------------- */ -/* --- CLASS ----------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -class Device_EXPORT Device: public Entity { - - public: - static const std::string CLASS_NAME; - virtual const std::string& getClassName(void) const { - return CLASS_NAME; - } - static const double TIMESTEP_DEFAULT; - - /// Maps of joint devices. - std::map jointDevices_; - - /// Set integration time. - void timeStep(double ts) { timestep_ = ts;} - /// Set debug mode. - void setDebugMode(int mode) { debug_mode_ = mode;} - - protected: - /// \brief Current integration step. - double timestep_; - - /// \name Vectors related to the state. - ///@{ - - /// Control vector of the robot wrt urdf file. - Eigen::VectorXd control_; - - ///@} - - - bool sanityCheck_; - - /// Specifies the control input by each element of the state vector. - std::map sotControlType_; - std::map hwControlType_; - - /// - PeriodicCall periodicCallBefore_; - PeriodicCall periodicCallAfter_; - - public: - - /* --- CONSTRUCTION --- */ - Device(const std::string& name); - /* --- DESTRUCTION --- */ - virtual ~Device(); - - /// Set control input type. - virtual void setSoTControlType(const std::string &jointNames, - const std::string &sotCtrlType); - virtual void setHWControlType(const std::string &jointNames, - const std::string &hwCtrlType); - virtual void increment(const int& t); - /// Read directly the URDF model - void setURDFModel(const std::string &aURDFModel); - - /// \name Sanity check parameterization - /// \{ - void setSanityCheck (const bool & enableCheck); - /// \} - - /// \name Set index in vector (position, velocity, acceleration, control) - /// \{ - void setControlPos(const std::string &jointName, - const unsigned & index); - /// \} - public: /* --- DISPLAY --- */ - virtual void display(std::ostream& os) const; - SOT_CORE_EXPORT friend std::ostream& - operator<<(std::ostream& os, const Device& r) { - r.display(os); return os; - } - - public: /* --- SIGNALS --- */ - - /// Input signals handling the control vector - /// Some signals can not be used by the entity - /// It depends on the type of control defined by the yaml file for each actuator - /// For position control the signal stateSIN will be used - /// For velocity control the signal velocitySIN will be used - /// For torque control the signal torqueSIN will be used - /// For current control the signal currentSIN will be used - dg::SignalPtr stateSIN; - dg::SignalPtr velocitySIN; - dg::SignalPtr torqueSIN; - dg::SignalPtr currentSIN; - dg::SignalPtr stateGainsSIN; - dg::SignalPtr velocityGainsSIN; - dg::SignalPtr torqueGainsSIN; - - - /// \name Device current state. - /// \{ - /// \brief Output attitude provided by the hardware - /// The control vector can be position, velocity, torque or current. - /// It depends on each of the actuator - dg::SignalTimeDependent motorcontrolSOUT_; - /// \} - - /// \name Real robot current state - /// This corresponds to the real encoders values and take into - /// account the stabilization step. Therefore, this usually - /// does *not* match the state control input signal. - /// \{ - /// Motor positions - dg::Signal robotState_; - /// Motor velocities - dg::Signal robotVelocity_; - /// The force torque sensors - std::vector*> forcesSOUT_; - /// The imu sensors - std::vector imuSOUT_; - /// Motor or pseudo torques (estimated or build from PID) - dg::Signal * pseudoTorqueSOUT_; - /// Temperature signal - dg::Signal * temperatureSOUT_; - /// Current signal - dg::Signal * currentsSOUT_; - /// Motor angles signal - dg::Signal * motor_anglesSOUT_; - /// Joint angles signal - dg::Signal * joint_anglesSOUT_; - /// P gains signal - dg::Signal * p_gainsSOUT_; - /// D gains signal - dg::Signal * d_gainsSOUT_; - /// \} - - /// Parse a YAML string for configuration. - int ParseYAMLString(const std::string &aYamlString); - - /// \name Robot Side - ///@{ - - /// \brief Allows the robot to fill in the internal variables of the device - /// to publish data on the signals. - void setSensors(std::map &sensorsIn); - void setupSetSensors(std::map &sensorsIn); - void nominalSetSensors(std::map &sensorsIn); - void cleanupSetSensors(std::map &sensorsIn); - ///@} - - protected: - - /// \brief Provides to the robot the control information (callback signal motorcontrolSOUT_). - dg::Vector& getControl(dg::Vector &controlOut, const int& t); - - void setControlType(const std::string &strCtrlType, - ControlType &aCtrlType); - - /// \brief Compute the new control, from the entry signals (state, torque, velocity, current). - /// When the control is in position, checks that the position is within bounds. - /// When the control is in torque, checks that the torque is within bounds. - virtual void createControlVector(const int& time); - - /// \name Signals related methods - ///@{ - /// \brief Creates a signal called Device(DeviceName)::output(vector6)::force_sensor_name - void CreateAForceSignal(const std::string & force_sensor_name); - /// \brief Creates a signal called Device(DeviceName)::output(vector6)::imu_sensor_name - void CreateAnImuSignal(const std::string & imu_sensor_name); - - /// \name YAML related methods - /// @{ - /// Parse YAML for mapping joint names between hardware and sot - /// starting from a YAML-cpp node. - int ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name); - - /// Parse YAML for mapping control modes between hardware and sot - /// starting from a YAML-cpp node. - int ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode); - - /// Parse YAML for sensors from a YAML-cpp node. - int ParseYAMLSensors(YAML::Node &map_sensors); - - /// Parse YAML for joint sensors from YAML-cpp node. - int ParseYAMLJointSensor(YAML::Node &aJointSensors); - /// @} - - /// \brief Creates signal motorcontrolSOUT_ based on the control types information parsed by the YAML string. - /// Registers the signals - void RegisterSignals(); - /// \brief Creates signals based on the joints information parsed by the YAML string. - int UpdateSignals(); - - ///@} - /// Get freeflyer pose - const MatrixHomogeneous& freeFlyerPose() const; - - /// Protected methods for internal variables filling - void setSensorsForce(std::map &SensorsIn, int t); - void setSensorsIMU(std::map &SensorsIn, int t); - void setSensorsEncoders(std::map &SensorsIn, int t); - void setSensorsVelocities(std::map &SensorsIn, int t); - void setSensorsTorquesCurrents(std::map &SensorsIn, int t); - - void setSensorsGains(std::map &SensorsIn, int t); - - private: - - // URDF Model of the robot - ::urdf::ModelInterfaceSharedPtr model_; - std::vector< ::urdf::JointSharedPtr > urdf_joints_; - - // Debug mode - int debug_mode_; - - std::vector control_types_; - - // Intermediate index when parsing YAML file. - int temperature_index_, velocity_index_, - current_index_, torque_index_, - force_index_, joint_angle_index_, - motor_angle_index_ - ; - - public: - const ::urdf::ModelInterfaceSharedPtr & getModel() { - return model_; - } - - const std::vector< ::urdf::JointSharedPtr > & getURDFJoints() { - return urdf_joints_; - } -}; -} // namespace sot + namespace sot { + + /// Define the type of input expected by the robot + enum ControlInput + { + CONTROL_INPUT_NO_INTEGRATION=0, + CONTROL_INPUT_ONE_INTEGRATION=1, + CONTROL_INPUT_TWO_INTEGRATION=2, + CONTROL_INPUT_SIZE=3 + }; + + const std::string ControlInput_s[] = + { + "noInteg", "oneInteg", "twoInteg" + }; + + /* --------------------------------------------------------------------- */ + /* --- CLASS ----------------------------------------------------------- */ + /* --------------------------------------------------------------------- */ + + class SOT_CORE_EXPORT Device + :public Entity + { + public: + static const std::string CLASS_NAME; + virtual const std::string& getClassName(void) const { + return CLASS_NAME; + } + + enum ForceSignalSource + { + FORCE_SIGNAL_RLEG, + FORCE_SIGNAL_LLEG, + FORCE_SIGNAL_RARM, + FORCE_SIGNAL_LARM + }; + + protected: + dg::Vector state_; + dg::Vector velocity_; + bool sanityCheck_; + dg::Vector vel_control_; + ControlInput controlInputType_; + bool withForceSignals[4]; + PeriodicCall periodicCallBefore_; + PeriodicCall periodicCallAfter_; + double timestep_; + + /// \name Robot bounds used for sanity checks + /// \{ + Vector upperPosition_; + Vector upperVelocity_; + Vector upperTorque_; + Vector lowerPosition_; + Vector lowerVelocity_; + Vector lowerTorque_; + /// \} + public: + + /* --- CONSTRUCTION --- */ + Device(const std::string& name); + /* --- DESTRUCTION --- */ + virtual ~Device(); + + virtual void setStateSize(const unsigned int& size); + virtual void setState(const dg::Vector& st); + void setVelocitySize(const unsigned int& size); + virtual void setVelocity(const dg::Vector & vel); + virtual void setSecondOrderIntegration(); + virtual void setNoIntegration(); + virtual void setControlInputType(const std::string& cit); + virtual void increment(const double & dt = 5e-2); + + /// \name Sanity check parameterization + /// \{ + void setSanityCheck (const bool & enableCheck); + void setPositionBounds(const Vector& lower, const Vector& upper); + void setVelocityBounds(const Vector& lower, const Vector& upper); + void setTorqueBounds (const Vector& lower, const Vector& upper); + /// \} + + public: /* --- DISPLAY --- */ + virtual void display(std::ostream& os) const; + virtual void cmdDisplay(); + SOT_CORE_EXPORT friend std::ostream& + operator<<(std::ostream& os,const Device& r) { + r.display(os); return os; + } + + public: /* --- SIGNALS --- */ + + dynamicgraph::SignalPtr controlSIN; + dynamicgraph::SignalPtr attitudeSIN; + dynamicgraph::SignalPtr zmpSIN; + + /// \name Device current state. + /// \{ + dynamicgraph::Signal stateSOUT; + dynamicgraph::Signal velocitySOUT; + dynamicgraph::Signal attitudeSOUT; + /*! \brief The current state of the robot from the command viewpoint. */ + dynamicgraph::Signal motorcontrolSOUT; + dynamicgraph::Signal previousControlSOUT; + /*! \brief The ZMP reference send by the previous controller. */ + dynamicgraph::Signal ZMPPreviousControllerSOUT; + /// \} + + /// \name Real robot current state + /// This corresponds to the real encoders values and take into + /// account the stabilization step. Therefore, this usually + /// does *not* match the state control input signal. + /// \{ + /// Motor positions + dynamicgraph::Signal robotState_; + /// Motor velocities + dynamicgraph::Signal robotVelocity_; + /// The force torque sensors + dynamicgraph::Signal* forcesSOUT[4]; + /// Motor torques + /// \todo why pseudo ? + dynamicgraph::Signal pseudoTorqueSOUT; + /// \} + + protected: + /// Compute roll pitch yaw angles of freeflyer joint. + void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control, + double dt); + /// Store Position of free flyer joint + MatrixHomogeneous ffPose_; + /// Compute the new position, from the current control. + /// + /// When sanity checks are enabled, this checks that the control is within + /// bounds. There are three cases, depending on what the control is: + /// - position: checks that the position is within bounds, + /// - velocity: checks that the velocity and the future position are + /// within bounds, + /// - acceleration: checks that the acceleration, the future velocity and + /// position are within bounds. + /// \todo in order to check the acceleration, we need + /// pinocchio and the contact forces in order to estimate + /// the joint torques for the given acceleration. + virtual void integrate( const double & dt ); + protected: + /// Get freeflyer pose + const MatrixHomogeneous& freeFlyerPose() const; + public: + virtual void setRoot( const dg::Matrix & root ); + + + virtual void setRoot( const MatrixHomogeneous & worldMwaist ); + private: + // Intermediate variable to avoid dynamic allocation + dg::Vector forceZero6; + }; + } // namespace sot } // namespace dynamicgraph -#endif /* #ifndef SOT_DEVICE_HH */ - +#endif /* #ifndef SOT_DEVICE_HH */ \ No newline at end of file diff --git a/include/sot/core/generic-device.hh b/include/sot/core/generic-device.hh new file mode 100644 index 000000000..b0e72022e --- /dev/null +++ b/include/sot/core/generic-device.hh @@ -0,0 +1,373 @@ +/* + * Copyright 2010-2018, CNRS + * Florent Lamiraux + * Olivier Stasse + * + * CNRS + * + * See LICENSE.txt + */ + +#ifndef SOT_GENERIC_DEVICE_HH +#define SOT_GENERIC_DEVICE_HH + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#include +#include + +/// URDF DOM +#include +#include +#include + +/// YAML CPP +#include + +/* -- MaaL --- */ +#include +namespace dg = dynamicgraph; +/* SOT */ +/// dg +#include +#include +/// sot-core +#include "sot/core/periodic-call.hh" +#include +#include "sot/core/api.hh" +#include + +namespace dgsot = dynamicgraph::sot; +namespace dg = dynamicgraph; + +namespace dynamicgraph { +namespace sot { + +/* --------------------------------------------------------------------- */ +/* --- API ------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +#if defined (WIN32) +# if defined (generic_device_EXPORTS) +# define GENERICDEVICE_EXPORT __declspec(dllexport) +# else +# define GENERICDEVICE_EXPORT __declspec(dllimport) +# endif +#else +# define GENERICDEVICE_EXPORT +#endif + + +/// Specifies the nature of one joint control +/// It is used for the hardware side. +enum ControlType { + POSITION = 0, + TORQUE = 1, + VELOCITY = 2, + CURRENT = 3 +}; + +const std::string ControlType_s[] = { + "POSITION", "TORQUE", "VELOCITY", "CURRENT" +}; + +//@} + +/// \brief Store information on each joint. +struct JointSoTHWControlType { + /// Defines the control from the Stack-of-Tasks side (for instance, position) + ControlType SoTcontrol; + /// Defines the control from the hardware side. + ControlType HWcontrol; + /// Position of the joint in the control vector. + int control_index; + /// Position of the joint in the URDF index. + int urdf_index; + + /// Various indexes for the sensor signals. + /// This may vary if some joints does not support this feature. + ///@{ + /// Position of the joint in the temperature vector + int temperature_index; + + /// Position of the joint in the velocity vector + int velocity_index; + + /// Position of the joint in the current vector + int current_index; + + /// Position of the joint in the torque vector + int torque_index; + + /// Position of the joint in the force vector + int force_index; + + /// Position of the joint in the joint-angles vector + int joint_angle_index; + + /// Position of the joint in the motor-angles vector + int motor_angle_index; + + ///@} + JointSoTHWControlType(); +}; + +struct IMUSOUT { + std::string imu_sensor_name; + dg::Signal attitudeSOUT; + dg::Signal accelerometerSOUT; + dg::Signal gyrometerSOUT; + IMUSOUT(const std::string &limu_sensor_name, + const std::string &device_name): + imu_sensor_name(limu_sensor_name) + , attitudeSOUT("GenericDevice(" + device_name + + ")::output(vector6)::" + imu_sensor_name + "_attitudeSOUT") + , accelerometerSOUT("GenericDevice(" + device_name + + ")::output(vector3)::" + imu_sensor_name + "_accelerometerSOUT") + , gyrometerSOUT("GenericDevice(" + device_name + + ")::output(vector3)::" + imu_sensor_name + "_gyrometerSOUT") + {} +}; + +typedef std::map::iterator +JointSHWControlType_iterator; +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +class GENERICDEVICE_EXPORT GenericDevice: 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; + + /// Maps of joint devices. + std::map jointDevices_; + + /// Set integration time. + void timeStep(double ts) { timestep_ = ts;} + /// Set debug mode. + void setDebugMode(int mode) { debug_mode_ = mode;} + + protected: + /// \brief Current integration step. + double timestep_; + + /// \name Vectors related to the state. + ///@{ + + /// Control vector of the robot wrt urdf file. + Eigen::VectorXd control_; + + ///@} + + + bool sanityCheck_; + + /// Specifies the control input by each element of the state vector. + std::map sotControlType_; + std::map hwControlType_; + + /// + PeriodicCall periodicCallBefore_; + PeriodicCall periodicCallAfter_; + + public: + + /* --- CONSTRUCTION --- */ + GenericDevice(const std::string& name); + /* --- DESTRUCTION --- */ + virtual ~GenericDevice(); + + /// Set control input type. + virtual void setSoTControlType(const std::string &jointNames, + const std::string &sotCtrlType); + virtual void setHWControlType(const std::string &jointNames, + const std::string &hwCtrlType); + virtual void increment(const int& t); + /// Read directly the URDF model + void setURDFModel(const std::string &aURDFModel); + + /// \name Sanity check parameterization + /// \{ + void setSanityCheck (const bool & enableCheck); + /// \} + + /// \name Set index in vector (position, velocity, acceleration, control) + /// \{ + void setControlPos(const std::string &jointName, + const unsigned & index); + /// \} + public: /* --- DISPLAY --- */ + virtual void display(std::ostream& os) const; + SOT_CORE_EXPORT friend std::ostream& + operator<<(std::ostream& os, const GenericDevice& r) { + r.display(os); return os; + } + + public: /* --- SIGNALS --- */ + + /// Input signals handling the control vector + /// Some signals can not be used by the entity + /// It depends on the type of control defined by the yaml file for each actuator + /// For position control the signal stateSIN will be used + /// For velocity control the signal velocitySIN will be used + /// For torque control the signal torqueSIN will be used + /// For current control the signal currentSIN will be used + dg::SignalPtr stateSIN; + dg::SignalPtr velocitySIN; + dg::SignalPtr torqueSIN; + dg::SignalPtr currentSIN; + dg::SignalPtr stateGainsSIN; + dg::SignalPtr velocityGainsSIN; + dg::SignalPtr torqueGainsSIN; + + + /// \name GenericDevice current state. + /// \{ + /// \brief Output attitude provided by the hardware + /// The control vector can be position, velocity, torque or current. + /// It depends on each of the actuator + dg::SignalTimeDependent motorcontrolSOUT_; + /// \} + + /// \name Real robot current state + /// This corresponds to the real encoders values and take into + /// account the stabilization step. Therefore, this usually + /// does *not* match the state control input signal. + /// \{ + /// Motor positions + dg::Signal robotState_; + /// Motor velocities + dg::Signal robotVelocity_; + /// The force torque sensors + std::vector*> forcesSOUT_; + /// The imu sensors + std::vector imuSOUT_; + /// Motor or pseudo torques (estimated or build from PID) + dg::Signal * pseudoTorqueSOUT_; + /// Temperature signal + dg::Signal * temperatureSOUT_; + /// Current signal + dg::Signal * currentsSOUT_; + /// Motor angles signal + dg::Signal * motor_anglesSOUT_; + /// Joint angles signal + dg::Signal * joint_anglesSOUT_; + /// P gains signal + dg::Signal * p_gainsSOUT_; + /// D gains signal + dg::Signal * d_gainsSOUT_; + /// \} + + /// Parse a YAML string for configuration. + int ParseYAMLString(const std::string &aYamlString); + + /// \name Robot Side + ///@{ + + /// \brief Allows the robot to fill in the internal variables of the device + /// to publish data on the signals. + void setSensors(std::map &sensorsIn); + void setupSetSensors(std::map &sensorsIn); + void nominalSetSensors(std::map &sensorsIn); + void cleanupSetSensors(std::map &sensorsIn); + ///@} + + protected: + + /// \brief Provides to the robot the control information (callback signal motorcontrolSOUT_). + dg::Vector& getControl(dg::Vector &controlOut, const int& t); + + void setControlType(const std::string &strCtrlType, + ControlType &aCtrlType); + + /// \brief Compute the new control, from the entry signals (state, torque, velocity, current). + /// When the control is in position, checks that the position is within bounds. + /// When the control is in torque, checks that the torque is within bounds. + virtual void createControlVector(const int& time); + + /// \name Signals related methods + ///@{ + /// \brief Creates a signal called GenericDevice(DeviceName)::output(vector6)::force_sensor_name + void CreateAForceSignal(const std::string & force_sensor_name); + /// \brief Creates a signal called GenericDevice(DeviceName)::output(vector6)::imu_sensor_name + void CreateAnImuSignal(const std::string & imu_sensor_name); + + /// \name YAML related methods + /// @{ + /// Parse YAML for mapping joint names between hardware and sot + /// starting from a YAML-cpp node. + int ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name); + + /// Parse YAML for mapping control modes between hardware and sot + /// starting from a YAML-cpp node. + int ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode); + + /// Parse YAML for sensors from a YAML-cpp node. + int ParseYAMLSensors(YAML::Node &map_sensors); + + /// Parse YAML for joint sensors from YAML-cpp node. + int ParseYAMLJointSensor(YAML::Node &aJointSensors); + /// @} + + /// \brief Creates signal motorcontrolSOUT_ based on the control types information parsed by the YAML string. + /// Registers the signals + void RegisterSignals(); + /// \brief Creates signals based on the joints information parsed by the YAML string. + int UpdateSignals(); + + ///@} + /// Get freeflyer pose + const MatrixHomogeneous& freeFlyerPose() const; + + /// Protected methods for internal variables filling + void setSensorsForce(std::map &SensorsIn, int t); + void setSensorsIMU(std::map &SensorsIn, int t); + void setSensorsEncoders(std::map &SensorsIn, int t); + void setSensorsVelocities(std::map &SensorsIn, int t); + void setSensorsTorquesCurrents(std::map &SensorsIn, int t); + + void setSensorsGains(std::map &SensorsIn, int t); + + private: + + // URDF Model of the robot + ::urdf::ModelInterfaceSharedPtr model_; + std::vector< ::urdf::JointSharedPtr > urdf_joints_; + + // Debug mode + int debug_mode_; + + std::vector control_types_; + + // Intermediate index when parsing YAML file. + int temperature_index_, velocity_index_, + current_index_, torque_index_, + force_index_, joint_angle_index_, + motor_angle_index_ + ; + + public: + const ::urdf::ModelInterfaceSharedPtr & getModel() { + return model_; + } + + const std::vector< ::urdf::JointSharedPtr > & getURDFJoints() { + return urdf_joints_; + } +}; +} // namespace sot +} // namespace dynamicgraph + + +#endif /* #ifndef SOT_GENERIC_DEVICE_HH */ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5cf6e9ad6..e5c067270 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -89,7 +89,7 @@ SET(plugins ) IF (YAML_CPP_FOUND AND URDFDOM_FOUND) - SET(plugins ${plugins} tools/device ) + SET(plugins ${plugins} tools/generic-device ) ENDIF () @@ -147,6 +147,7 @@ SET(${LIBRARY_NAME}_SOURCES tools/utils-windows.cpp tools/periodic-call.cpp tools/device.cpp + tools/generic-device.cpp tools/trajectory.cpp tools/robot-utils.cpp diff --git a/src/tools/device.cpp b/src/tools/device.cpp index db36f50a3..401254864 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -1,8 +1,8 @@ /* - * Copyright 2019, CNRS - * Author: Olivier Stasse + * Copyright 2010, + * Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux * - * Please check LICENSE.txt for licensing + * CNRS * */ @@ -13,6 +13,7 @@ /* SOT */ #define ENABLE_RT_LOG +#include #include "sot/core/device.hh" #include using namespace std; @@ -20,133 +21,218 @@ using namespace std; #include #include #include +#include #include #include +#include using namespace dynamicgraph::sot; using namespace dynamicgraph; -#define DBGFILE "/tmp/device.txt" -#define INPUT_CONTROL_SIGNALS -#define INPUT_GAINS_SIGNALS stateGainsSIN << velocityGainsSIN << torqueGainsSIN - -#if 0 -#define RESETDEBUG5() { std::ofstream DebugFile; \ - DebugFile.open(DBGFILE,std::ofstream::out); \ - DebugFile.close();} -#define ODEBUG5FULL(x) { std::ofstream DebugFile; \ - DebugFile.open(DBGFILE,std::ofstream::app); \ - DebugFile << __FILE__ << ":" \ - << __FUNCTION__ << "(#" \ - << __LINE__ << "):" << x << std::endl; \ - DebugFile.close();} -#define ODEBUG5(x) { std::ofstream DebugFile; \ - DebugFile.open(DBGFILE,std::ofstream::app); \ - DebugFile << x << std::endl; \ - DebugFile.close();} - -#else -// Void the macro -#define RESETDEBUG5() -#define ODEBUG5FULL(x) -#define ODEBUG5(x) -#endif - -DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Device, "Device"); -// const std::string Device::CLASS_NAME = "Device"; -const double Device::TIMESTEP_DEFAULT = 0.001; - - -JointSoTHWControlType::JointSoTHWControlType(): - control_index(-1) - , urdf_index(-1) - , temperature_index(-1) - , velocity_index(-1) - , current_index(-1) - , torque_index(-1) - , force_index(-1) - , joint_angle_index(-1) - , motor_angle_index(-1) { -} +const std::string Device::CLASS_NAME = "Device"; + /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -Device::~Device( ) { - for ( unsigned int i = 0; i < forcesSOUT_.size(); ++i ) { - delete forcesSOUT_[i]; - } +void Device:: +integrateRollPitchYaw +(Vector& state, + const Vector& control, + double dt) +{ + using Eigen::AngleAxisd; + using Eigen::Vector3d; + using Eigen::QuaternionMapd; - for ( unsigned int i = 0; i < imuSOUT_.size(); ++i ) { - delete imuSOUT_[i]; - } + 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.head<6>()*dt, qout); -Device::Device( const std::string& n ) - : Entity(n) - , timestep_(TIMESTEP_DEFAULT) - , control_(6) - , sanityCheck_(true) - , stateSIN( NULL, "Device(" + n + ")::input(double)::state" ) - , velocitySIN( NULL, "Device(" + n + ")::input(double)::velocity" ) - , torqueSIN( NULL, "Device(" + n + ")::input(double)::torque" ) - , currentSIN( NULL, "Device(" + n + ")::input(double)::current" ) - , stateGainsSIN( NULL, "Device(" + n + ")::input(double)::stateGains" ) - , velocityGainsSIN( NULL, "Device(" + n + ")::input(double)::velocityGains" ) - , torqueGainsSIN( NULL, "Device(" + n + ")::input(double)::torqueGains" ) - , motorcontrolSOUT_(boost::bind(&Device::getControl, this, _1, _2), - INPUT_CONTROL_SIGNALS(0), - "Device(" + n + ")::output(vector)::motorcontrol" ) - , robotState_("Device(" + n + ")::output(vector)::robotState") - , robotVelocity_("Device(" + n + ")::output(vector)::robotVelocity") - , forcesSOUT_(0) - , imuSOUT_(0) - , pseudoTorqueSOUT_(0) - , temperatureSOUT_(0) - , currentsSOUT_(0) - , motor_anglesSOUT_(0) - , joint_anglesSOUT_(0) - , debug_mode_(5) - , temperature_index_(0) - , velocity_index_(0) - , current_index_(0) - , torque_index_(0) - , force_index_(0) - , joint_angle_index_(0) - , motor_angle_index_(0) + // Update freeflyer pose + ffPose_.translation() = qout.head<3>(); + state.head<3>() = qout.head<3>(); + ffPose_.linear() = + QuaternionMapd(qout.tail<4>().data()).toRotationMatrix(); + state.segment<3>(3) = ffPose_.linear().eulerAngles(2,1,0).reverse(); +} + +const MatrixHomogeneous& +Device::freeFlyerPose() const { + return ffPose_; +} - control_.fill(.0); +Device:: +~Device( ) +{ + for( unsigned int i=0; i<4; ++i ) { + delete forcesSOUT[i]; + } +} + +Device:: +Device( const std::string& n ) + :Entity(n) + ,state_(6) + ,sanityCheck_(true) + ,controlInputType_(CONTROL_INPUT_ONE_INTEGRATION) + ,controlSIN( NULL,"Device("+n+")::input(double)::control" ) + ,attitudeSIN(NULL,"Device("+ n +")::input(vector3)::attitudeIN") + ,zmpSIN(NULL,"Device("+n+")::input(vector3)::zmp") + ,stateSOUT( "Device("+n+")::output(vector)::state" ) + //,attitudeSIN(NULL,"Device::input(matrixRot)::attitudeIN") + ,velocitySOUT( "Device("+n+")::output(vector)::velocity" ) + ,attitudeSOUT( "Device("+n+")::output(matrixRot)::attitude" ) + ,motorcontrolSOUT ( "Device("+n+")::output(vector)::motorcontrol" ) + ,previousControlSOUT( "Device("+n+")::output(vector)::previousControl" ) + ,ZMPPreviousControllerSOUT + ( "Device("+n+ + ")::output(vector)::zmppreviouscontroller" ) + ,robotState_ ("Device("+n+")::output(vector)::robotState") + ,robotVelocity_ ("Device("+n+")::output(vector)::robotVelocity") + ,pseudoTorqueSOUT("Device("+n+")::output(vector)::ptorque" ) + + ,ffPose_() + ,forceZero6 (6) +{ + forceZero6.fill (0); + /* --- SIGNALS --- */ + for( int i=0;i<4;++i ){ withForceSignals[i] = false; } + forcesSOUT[0] = + new Signal("Device("+n+")::output(vector6)::forceRLEG"); + forcesSOUT[1] = + new Signal("Device("+n+")::output(vector6)::forceLLEG"); + forcesSOUT[2] = + new Signal("Device("+n+")::output(vector6)::forceRARM"); + forcesSOUT[3] = + new Signal("Device("+n+")::output(vector6)::forceLARM"); + + signalRegistration( controlSIN< + (*this, &Device::setStateSize, docstring)); + docstring = + "\n" + " Set state vector value\n" + "\n"; + addCommand("set", + new command::Setter + (*this, &Device::setState, docstring)); - /* SET of SoT control input type per joint */ - addCommand("setSoTControlType", - command::makeCommandVoid2(*this, &Device::setSoTControlType, - command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", - "Joint name", - "Control type: [TORQUE|POSITION|VELOCITY]"))); + docstring = + "\n" + " Set velocity vector value\n" + "\n"; + addCommand("setVelocity", + new command::Setter + (*this, &Device::setVelocity, docstring)); + + void(Device::*setRootPtr)(const Matrix&) = &Device::setRoot; + docstring + = command::docCommandVoid1("Set the root position.", + "matrix homogeneous"); + addCommand("setRoot", + command::makeCommandVoid1(*this,setRootPtr, + docstring)); + + /* Second Order Integration set. */ + docstring = + "\n" + " Set the position calculous starting from \n" + " acceleration measure instead of velocity \n" + "\n"; + + addCommand + ("setSecondOrderIntegration", + command::makeCommandVoid0 + (*this,&Device::setSecondOrderIntegration, + docstring)); + + /* Display information */ + docstring = + "\n" + " Display information on device \n" + "\n"; + addCommand + ("display", + command::makeCommandVoid0 + (*this,&Device::cmdDisplay,docstring)); + + /* SET of control input type. */ + docstring = + "\n" + " Set the type of control input which can be \n" + " acceleration, velocity, or position\n" + "\n"; - /* SET of HW control input type per joint */ - addCommand("setHWControlType", - command::makeCommandVoid2(*this, &Device::setHWControlType, - command::docCommandVoid2 ("Set HW control input type per joint", - "Joint name", - "Control type: [TORQUE|POSITION|VELOCITY]"))); + addCommand("setControlInputType", + new command::Setter + (*this, &Device::setControlInputType, docstring)); docstring = - "\n" - " Enable/Disable sanity checks\n" - "\n"; + "\n" + " Enable/Disable sanity checks\n" + "\n"; addCommand("setSanityCheck", new command::Setter (*this, &Device::setSanityCheck, docstring)); + addCommand("setPositionBounds", + command::makeCommandVoid2 + (*this,&Device::setPositionBounds, + command::docCommandVoid2 + ("Set robot position bounds", + "vector: lower bounds", + "vector: upper bounds"))); + + addCommand("setVelocityBounds", + command::makeCommandVoid2 + (*this,&Device::setVelocityBounds, + command::docCommandVoid2 + ("Set robot velocity bounds", + "vector: lower bounds", + "vector: upper bounds"))); + + addCommand("setTorqueBounds", + command::makeCommandVoid2 + (*this,&Device::setTorqueBounds, + command::docCommandVoid2 + ("Set robot torque bounds", + "vector: lower bounds", + "vector: upper bounds"))); + + addCommand("getTimeStep", + command::makeDirectGetter + (*this, &this->timestep_, + command::docDirectGetter + ("Time step", "double"))); // Handle commands and signals called in a synchronous way. periodicCallBefore_.addSpecificCommands(*this, commandMap, "before."); @@ -155,679 +241,377 @@ Device::Device( const std::string& n ) } } -void Device::RegisterSignals() { - SignalArray sigArray; - for (unsigned int i = 0; i < control_types_.size(); i++) { - if (control_types_[i] == "POSITION") { - sigArray << stateSIN; - motorcontrolSOUT_.addDependency(stateSIN); - } else if (control_types_[i] == "VELOCITY") { - sigArray << velocitySIN; - motorcontrolSOUT_.addDependency(velocitySIN); - } else if (control_types_[i] == "TORQUE") { - sigArray << torqueSIN; - motorcontrolSOUT_.addDependency(torqueSIN); - } else if (control_types_[i] == "CURRENT") { - sigArray << currentSIN; - motorcontrolSOUT_.addDependency(currentSIN); - } - } +void Device:: +setStateSize( const unsigned int& size ) +{ + state_.resize(size); state_.fill( .0 ); + stateSOUT .setConstant( state_ ); + previousControlSOUT.setConstant( state_ ); + pseudoTorqueSOUT.setConstant( state_ ); + motorcontrolSOUT .setConstant( state_ ); + + Device::setVelocitySize(size); - signalRegistration( sigArray - << robotState_ - << robotVelocity_ - << motorcontrolSOUT_); + Vector zmp(3); zmp.fill( .0 ); + ZMPPreviousControllerSOUT .setConstant( zmp ); } -void Device::setSanityCheck(const bool & enableCheck) { - sanityCheck_ = enableCheck; +void Device:: +setVelocitySize( const unsigned int& size ) +{ + velocity_.resize(size); + velocity_.fill(.0); + velocitySOUT.setConstant( velocity_ ); } -void Device::setControlType(const std::string &strCtrlType, ControlType &aCtrlType) { - for (int j = 0; j < 2; j++) { - if (strCtrlType == ControlType_s[j]) { - aCtrlType = (ControlType)j; +void Device:: +setState( const Vector& st ) +{ + if (sanityCheck_) { + const Vector::Index& s = st.size(); + switch (controlInputType_) { + case CONTROL_INPUT_TWO_INTEGRATION: + dgRTLOG() + << "Sanity check for this control is not well supported. " + "In order to make it work, use pinocchio and the contact forces " + "to estimate the joint torques for the given acceleration.\n"; + if ( s != lowerTorque_.size() + || s != upperTorque_.size() ) + throw std::invalid_argument ("Upper and/or lower torque bounds " + "do not match state size. Set them first with setTorqueBounds"); + case CONTROL_INPUT_ONE_INTEGRATION: + if ( s != lowerVelocity_.size() + || s != upperVelocity_.size() ) + throw std::invalid_argument ("Upper and/or lower velocity bounds " + "do not match state size." + " Set them first with setVelocityBounds"); + case CONTROL_INPUT_NO_INTEGRATION: + break; + default: + throw std::invalid_argument ("Invalid control mode type."); } } + state_ = st; + stateSOUT .setConstant( state_ ); + motorcontrolSOUT .setConstant( state_ ); } -void Device::setSoTControlType(const std::string &jointNames, const std::string &strCtrlType) { - setControlType(strCtrlType, jointDevices_[jointNames].SoTcontrol); +void Device:: +setVelocity( const Vector& vel ) +{ + velocity_ = vel; + velocitySOUT .setConstant( velocity_ ); } -void Device::setHWControlType(const std::string &jointNames, const std::string &strCtrlType) { - setControlType(strCtrlType, jointDevices_[jointNames].HWcontrol); +void Device:: +setRoot( const Matrix & root ) +{ + Eigen::Matrix4d _matrix4d(root); + MatrixHomogeneous _root(_matrix4d); + setRoot( _root ); } -void Device::setControlPos(const std::string &jointName, const unsigned & index) { - jointDevices_[jointName].control_index = index; +void Device:: +setRoot( const MatrixHomogeneous & worldMwaist ) +{ + VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse(); + Vector q = state_; + q = worldMwaist.translation(); // abusive ... but working. + for( unsigned int i=0;i<3;++i ) q(i+3) = r(i); } +void Device:: +setSecondOrderIntegration() +{ + controlInputType_ = CONTROL_INPUT_TWO_INTEGRATION; + velocity_.resize(state_.size()); + velocity_.setZero(); + velocitySOUT.setConstant( velocity_ ); +} -void Device::setURDFModel(const std::string &aURDFModel) { - model_ = ::urdf::parseURDF(aURDFModel); +void Device:: +setNoIntegration() +{ + controlInputType_ = CONTROL_INPUT_NO_INTEGRATION; + velocity_.resize(state_.size()); + velocity_.setZero(); + velocitySOUT.setConstant( velocity_ ); +} - if (!model_) { - dgRTLOG() - << "The XML stream does not contain a valid URDF model." << std::endl; - return; +void Device:: +setControlInputType(const std::string& cit) +{ + for(int i=0; i urdf_links; - model_->getLinks(urdf_links); - for (unsigned j = 0; j < urdf_links.size(); j++) { - std::vector child_joints = urdf_links[j]->child_joints; - urdf_joints_.insert(urdf_joints_.end(), boost::make_move_iterator(child_joints.begin()), - boost::make_move_iterator(child_joints.end())); +void Device:: +setPositionBounds(const Vector& lower, const Vector& upper) +{ + std::ostringstream oss; + if (lower.size() != state_.size()) { + oss << "Lower bound size should be " << state_.size(); + throw std::invalid_argument (oss.str()); } + if (upper.size() != state_.size()) { + oss << "Upper bound size should be " << state_.size(); + throw std::invalid_argument (oss.str()); + } + lowerPosition_ = lower; + upperPosition_ = upper; +} - std::cout << "urdf_joints_.size(): " << urdf_joints_.size() << std::endl; - for (unsigned int i = 0; i < urdf_joints_.size(); i++) { - jointDevices_[urdf_joints_[i]->name].urdf_index = i; - if (debug_mode_ > 1) { - std::cout << "jointDevices_ index: " << i - << " model_.names[i]: " << urdf_joints_[i]->name - << std::endl; - } +void Device:: +setVelocityBounds(const Vector& lower, const Vector& upper) +{ + std::ostringstream oss; + if (lower.size() != velocity_.size()) { + oss << "Lower bound size should be " << velocity_.size(); + throw std::invalid_argument (oss.str()); + } + if (upper.size() != velocity_.size()) { + oss << "Upper bound size should be " << velocity_.size(); + throw std::invalid_argument (oss.str()); + } + lowerVelocity_ = lower; + upperVelocity_ = upper; +} + +void Device:: +setTorqueBounds (const Vector& lower, const Vector& upper) +{ + // TODO I think the torque bounds size are state_.size()-6... + std::ostringstream oss; + if (lower.size() != state_.size()) { + oss << "Lower bound size should be " << state_.size(); + throw std::invalid_argument (oss.str()); + } + if (upper.size() != state_.size()) { + oss << "Lower bound size should be " << state_.size(); + throw std::invalid_argument (oss.str()); } + lowerTorque_ = lower; + upperTorque_ = upper; } -void Device::increment(const int& time) { - // int time = motorcontrolSOUT_.getTime(); +void Device:: +increment( const double & dt ) +{ + int time = stateSOUT.getTime(); sotDEBUG(25) << "Time : " << time << std::endl; // Run Synchronous commands and evaluate signals outside the main // connected component of the graph. - try { - periodicCallBefore_.run(time + 1); - } catch (std::exception& e) { + try + { + periodicCallBefore_.run(time+1); + } + catch (std::exception& e) + { dgRTLOG() << "exception caught while running periodical commands (before): " << e.what () << std::endl; - } catch (const char* str) { + } + catch (const char* str) + { dgRTLOG() << "exception caught while running periodical commands (before): " << str << std::endl; - } catch (...) { + } + catch (...) + { dgRTLOG() << "unknown exception caught while" << " running periodical commands (before)" << std::endl; } - /* Create control from the input vectors. */ - createControlVector(time); - sotDEBUG(25) << "q" << time << " = " << control_ << endl; + /* Force the recomputation of the control. */ + controlSIN( time ); + sotDEBUG(25) << "u" <second.control_index; - int u_index = it_control_type->second.urdf_index; - - if (lctl_index == -1) { - if (debug_mode_ > 1) { - std::cerr << "No control index for joint " - << urdf_joints_[u_index]->name << std::endl; - } - break; - } - if (u_index != -1) { - // POSITION -> stateSIN + Check Position limits. - if (it_control_type->second.SoTcontrol == POSITION) { - control_[lctl_index] = state[lctl_index]; - - if (urdf_joints_[u_index]->limits) { - double lowerLim = urdf_joints_[u_index]->limits->lower; - double upperLim = urdf_joints_[u_index]->limits->upper; - if (control_[lctl_index] < lowerLim) { - control_[lctl_index] = lowerLim; - } else if (control_[lctl_index] > upperLim) { - control_[lctl_index] = upperLim; - } - } - } - // TORQUE -> torqueSIN + Check Torque limits. - else if (it_control_type->second.SoTcontrol == TORQUE) { - control_[lctl_index] = torque[lctl_index]; - - if (urdf_joints_[u_index]->limits) { - double lim = urdf_joints_[u_index]->limits->effort; - if (control_[lctl_index] < -lim) { - control_[lctl_index] = -lim; - } else if (control_[lctl_index] > lim) { - control_[lctl_index] = lim; - } - } - } - // VELOCITY -> velSIN - else if (it_control_type->second.SoTcontrol == VELOCITY) { - control_[lctl_index] = vel[lctl_index]; - } - // CURRENT -> currentSIN - else if (it_control_type->second.SoTcontrol == CURRENT) { - control_[lctl_index] = current[lctl_index]; - } - } - } - if (sanityCheck_ && control_.hasNaN()) { - dgRTLOG () << "Device::updateControl: Control has NaN values: " << '\n' - << control_.transpose() << '\n'; - return; + << " running periodical commands (after)" << std::endl; } -} - -int Device::ParseYAMLString(const std::string & aYamlString) { - YAML::Node map_global = YAML::Load(aYamlString); - YAML::Node map_sot_controller = map_global["sot_controller"]; - - for (YAML::const_iterator it = map_sot_controller.begin(); - it != map_sot_controller.end(); - it++) { - std::string name_elt_of_sot_controller = it->first.as(); - - if (debug_mode_ > 1) { - std::cout << "key:" << name_elt_of_sot_controller << std::endl; - } - YAML::Node elt_of_sot_controller = it->second; - - if (name_elt_of_sot_controller == "joint_names") { - int r = ParseYAMLMapHardwareJointNames(elt_of_sot_controller); - if (r < 0) return r; - } else if (name_elt_of_sot_controller == "map_rc_to_sot_device") { - int r = ParseYAMLJointSensor(elt_of_sot_controller); - if (r < 0) return r; - } else if (name_elt_of_sot_controller == "control_mode") { - int r = ParseYAMLMapHardwareControlMode(elt_of_sot_controller); - if (r < 0) return r; - } else if (name_elt_of_sot_controller.find("sensor") != std::string::npos) { - int r = ParseYAMLSensors(elt_of_sot_controller); - if (r < 0) return r; - } - } - RegisterSignals(); - UpdateSignals(); - return 0; + // Others signals. + motorcontrolSOUT .setConstant( state_ ); } -int Device::ParseYAMLJointSensor(YAML::Node &aJointSensor) { - for (YAML::const_iterator it = aJointSensor.begin(); - it != aJointSensor.end(); - it++) { - std::string aSensor = it->first.as(); - if (debug_mode_ > 1) { - std::cout << "Found " << aSensor << std::endl; - } - JointSHWControlType_iterator it_control_type; - for (it_control_type = jointDevices_.begin(); - it_control_type != jointDevices_.end(); - it_control_type++) { - if (aSensor == "temperature") { - it_control_type->second.temperature_index = temperature_index_; - temperature_index_++; - } else if (aSensor == "velocities") { - it_control_type->second.velocity_index = velocity_index_; - velocity_index_++; - } else if (aSensor == "currents") { - it_control_type->second.current_index = current_index_; - current_index_++; - } else if (aSensor == "torques") { - it_control_type->second.torque_index = torque_index_; - torque_index_++; - } else if (aSensor == "forces") { - it_control_type->second.force_index = force_index_; - force_index_++; - } else if (aSensor == "joint_angles") { - it_control_type->second.joint_angle_index = joint_angle_index_; - joint_angle_index_++; - } else if (aSensor == "motor_angles") { - it_control_type->second.motor_angle_index = motor_angle_index_; - motor_angle_index_++; - } - } - } - return 0; +// Return true if it saturates. +inline bool +saturateBounds (double& val, const double& lower, const double& upper) +{ + assert (lower <= upper); + if (val < lower) { val = lower; return true; } + if (upper < val) { val = upper; return true; } + return false; } -int Device::ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name) { - if (control_.size() != map_joint_name.size()) { - control_.resize(map_joint_name.size()); +#define CHECK_BOUNDS(val, lower, upper, what) \ + for (int i = 0; i < val.size(); ++i) { \ + double old = val(i); \ + if (saturateBounds (val(i), lower(i), upper(i))) \ + dgRTLOG () << "Robot " what " bound violation at DoF " << i << \ + ": requested " << old << " but set " << val(i) << '\n'; \ } - if (debug_mode_ > 1) { - std::cout << "map_joint_name.size(): " - << map_joint_name.size() << std::endl; - std::cout << map_joint_name << std::endl; - } - for (unsigned int i = 0; i < map_joint_name.size(); i++) { - std::string jointName = map_joint_name[i].as(); - if (debug_mode_ > 1) { - std::cout << "-- Joint: " << jointName << " has index: " << i << std::endl; - } - setControlPos(jointName, i); - } - return 0; -} +void Device::integrate( const double & dt ) +{ + const Vector & controlIN = controlSIN.accessCopy(); -int Device::ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode) { - if (debug_mode_ > 1) { - std::cout << "map_control_mode.size(): " - << map_control_mode.size() << std::endl; - std::cout << map_control_mode << std::endl; + if (sanityCheck_ && controlIN.hasNaN()) + { + dgRTLOG () << "Device::integrate: Control has NaN values: " << '\n' + << controlIN.transpose() << '\n'; + return; } - if (map_control_mode.size() == 0) { - std::string value = map_control_mode.as(); - JointSHWControlType_iterator it_control_type; - for (it_control_type = jointDevices_.begin(); - it_control_type != jointDevices_.end(); - it_control_type++) { - int u_index = it_control_type->second.urdf_index; - setSoTControlType(urdf_joints_[u_index]->name, value); - control_types_.push_back(value); - } - if (debug_mode_ > 1) { - std::cout << "All joints are controlled in: " << value << std::endl; - } - return 0; + if (controlInputType_==CONTROL_INPUT_NO_INTEGRATION) + { + assert(state_.size()==controlIN.size()+6); + state_.tail(controlIN.size()) = controlIN; + return; } - for (YAML::const_iterator it = map_control_mode.begin(); - it != map_control_mode.end(); - it++) { - std::string jointName = it->first.as(); - if (debug_mode_ > 1) { - std::cout << "joint name: " << jointName << std::endl; - } + if( vel_control_.size() == 0 ) + vel_control_ = Vector::Zero(controlIN.size()); - YAML::Node aNode = it->second; + // If control size is state size - 6, integrate joint angles, + // if control and state are of same size, integrate 6 first degrees of + // freedom as a translation and roll pitch yaw. - if (debug_mode_ > 1) { - std::cout << "Type of value: " << aNode.Type() << std::endl; - } - - for (YAML::const_iterator it2 = aNode.begin(); - it2 != aNode.end(); - it2++) { - std::string aKey = it2->first.as(); - if (debug_mode_ > 1) { - std::cout << "-- key:" << aKey << std::endl; - } - if (aKey == "hw_control_mode") { - std::string value = it2->second.as(); - if (debug_mode_ > 1) { - std::cout << "-- Value: " << value << std::endl; - } - setHWControlType(jointName, value); - } else if (aKey == "ros_control_mode") { - std::string value = it2->second.as(); - if (debug_mode_ > 1) { - std::cout << "-- Value: " << value << std::endl; - } - setSoTControlType(jointName, value); - control_types_.push_back(value); - } - } - } - return 0; -} - -/* Sensor signals */ -int Device::ParseYAMLSensors(YAML::Node &map_sensors) { - if (map_sensors.IsNull()) { - std::cerr << "Device::ParseYAMLString: No sensor detected in YamlString " << std::endl; - return -1; - } - std::string sensor_name = map_sensors.as(); - if (debug_mode_ > 1) { - std::cout << "-- sensor_name:" << sensor_name << std::endl; + if (controlInputType_==CONTROL_INPUT_TWO_INTEGRATION) + { + // TODO check acceleration + // Position increment + vel_control_ = velocity_.tail(controlIN.size()) + (0.5*dt)*controlIN; + // Velocity integration. + velocity_.tail(controlIN.size()) += controlIN*dt; } - if (sensor_name.find("ft") != std::string::npos) { - CreateAForceSignal(sensor_name); + else + { + vel_control_ = controlIN; } - else if (sensor_name.find("imu") != std::string::npos) { - CreateAnImuSignal(sensor_name); - } else { - std::cerr << "The sensor " << sensor_name - << " is not recognized" << std::endl; - return 1; + // Velocity bounds check + if (sanityCheck_) { + CHECK_BOUNDS(velocity_, lowerVelocity_, upperVelocity_, "velocity"); } - return 0; -} - -void Device::CreateAForceSignal(const std::string & force_sensor_name) { - dynamicgraph::Signal * aForceSOUT_; - /* --- SIGNALS --- */ - aForceSOUT_ = new Signal("Device(" + getName() + ")::output(vector6)::" + - force_sensor_name); - forcesSOUT_.push_back(aForceSOUT_); - signalRegistration(*aForceSOUT_); -} - -void Device::CreateAnImuSignal(const std::string &imu_sensor_name) { - IMUSOUT * anImuSOUT_; - /* --- SIGNALS --- */ - anImuSOUT_ = new IMUSOUT(imu_sensor_name, getName()); - imuSOUT_.push_back(anImuSOUT_); - signalRegistration(anImuSOUT_->attitudeSOUT - << anImuSOUT_->accelerometerSOUT - << anImuSOUT_->gyrometerSOUT); -} - - -int Device::UpdateSignals() { - if ((torque_index_ != 0) && (pseudoTorqueSOUT_ != 0)) { - pseudoTorqueSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::ptorque"); - } - // if ((force_index_!=0) && (forcesSOUT_.size()!=0)) - // { - // forcesSOUT_ = new Signal("Device("+getName()+")::output(vector)::forces"); - // } - if ((current_index_ != 0) && (currentsSOUT_ != 0)) { - currentsSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::currents"); + // Freeflyer integration + if (vel_control_.size() == state_.size()) { + integrateRollPitchYaw(state_, vel_control_, dt); } - if ((temperature_index_ != 0) && (temperatureSOUT_ != 0)) { - temperatureSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::temperatures"); - } - - if ((motor_angle_index_ != 0) && (motor_anglesSOUT_ != 0)) { - motor_anglesSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::motor_angles"); - } + // Position integration + state_.tail(controlIN.size()) += vel_control_ * dt; - if ((joint_angle_index_ != 0) && (joint_anglesSOUT_ != 0)) { - joint_anglesSOUT_ = new Signal("Device(" + getName() + ")::output(vector)::joint_angles"); + // Position bounds check + if (sanityCheck_) { + CHECK_BOUNDS(state_, lowerPosition_, upperPosition_, "position"); } - - return 0; } - - /* --- DISPLAY ------------------------------------------------------------ */ -void Device::display ( std::ostream& os ) const { - os << name << ": " << control_ << endl; -} - -/* Helpers for the controller */ -void Device::setSensorsForce(map &SensorsIn, int t) { - Eigen::Matrix dgforces; - - sotDEBUGIN(15); - map::iterator it; - it = SensorsIn.find("forces"); - if (it != SensorsIn.end()) { - // Implements force recollection. - const vector& forcesIn = it->second.getValues(); - for (std::size_t i = 0; i < forcesSOUT_.size(); ++i) { - sotDEBUG(15) << "Force sensor " << i << std::endl; - for (int j = 0; j < 6; ++j) { - dgforces(j) = forcesIn[i * 6 + j]; - sotDEBUG(15) << "Force value " << j << ":" << dgforces(j) << std::endl; - } - forcesSOUT_[i]->setConstant(dgforces); - forcesSOUT_[i]->setTime (t); - } - } - sotDEBUGIN(15); -} - -void Device::setSensorsIMU(map &SensorsIn, int t) { - Eigen::Matrix aVector3d; - - map::iterator it; - - //TODO: Confirm if this can be made quaternion - for (std::size_t k = 0; k < imuSOUT_.size(); ++k) { - it = SensorsIn.find("attitude"); - if (it != SensorsIn.end()) { - const vector& attitude = it->second.getValues (); - Eigen::Matrix pose; - - for (unsigned int i = 0; i < 3; ++i) { - for (unsigned int j = 0; j < 3; ++j) { - pose (i, j) = attitude[i * 3 + j]; - } - } - imuSOUT_[k]->attitudeSOUT.setConstant (pose); - imuSOUT_[k]->attitudeSOUT.setTime (t); - } - - it = SensorsIn.find("accelerometer_0"); - if (it != SensorsIn.end()) { - const vector& accelerometer = - SensorsIn ["accelerometer_0"].getValues (); - for (std::size_t i = 0; i < 3; ++i) { - aVector3d(i) = accelerometer [i]; - } - imuSOUT_[k]->accelerometerSOUT.setConstant (aVector3d); - imuSOUT_[k]->accelerometerSOUT.setTime (t); - } - - it = SensorsIn.find("gyrometer_0"); - if (it != SensorsIn.end()) { - const vector& gyrometer = SensorsIn ["gyrometer_0"].getValues (); - for (std::size_t i = 0; i < 3; ++i) { - aVector3d(i) = gyrometer [i]; - } - imuSOUT_[k]->gyrometerSOUT.setConstant (aVector3d); - imuSOUT_[k]->gyrometerSOUT.setTime (t); - } - } -} - -void Device::setSensorsEncoders(map &SensorsIn, int t) { - dg::Vector dgRobotState, motor_angles, joint_angles; - map::iterator it; - - if (motor_anglesSOUT_ != 0) { - it = SensorsIn.find("motor-angles"); - if (it != SensorsIn.end()) { - const vector& anglesIn = it->second.getValues(); - dgRobotState.resize (anglesIn.size () + 6); - motor_angles.resize(anglesIn.size ()); - for (unsigned i = 0; i < 6; ++i) { - dgRobotState (i) = 0.; - } - for (unsigned i = 0; i < anglesIn.size(); ++i) { - dgRobotState (i + 6) = anglesIn[i]; - motor_angles(i) = anglesIn[i]; - } - robotState_.setConstant(dgRobotState); - robotState_.setTime(t); - motor_anglesSOUT_->setConstant(motor_angles); - motor_anglesSOUT_->setTime(t); - } - } - - if (joint_anglesSOUT_ != 0) { - it = SensorsIn.find("joint-angles"); - if (it != SensorsIn.end()) { - const vector& joint_anglesIn = it->second.getValues(); - joint_angles.resize (joint_anglesIn.size () ); - for (unsigned i = 0; i < joint_anglesIn.size(); ++i) { - joint_angles (i) = joint_anglesIn[i]; - } - joint_anglesSOUT_->setConstant(joint_angles); - joint_anglesSOUT_->setTime(t); - } - } -} - -void Device::setSensorsVelocities(map &SensorsIn, int t) { - dg::Vector dgRobotVelocity; - - map::iterator it; - - it = SensorsIn.find("velocities"); - if (it != SensorsIn.end()) { - const vector& velocitiesIn = it->second.getValues(); - dgRobotVelocity.resize (velocitiesIn.size () + 6); - for (unsigned i = 0; i < 6; ++i) { - dgRobotVelocity (i) = 0.; - } - for (unsigned i = 0; i < velocitiesIn.size(); ++i) { - dgRobotVelocity (i + 6) = velocitiesIn[i]; - } - robotVelocity_.setConstant(dgRobotVelocity); - robotVelocity_.setTime(t); - } -} - -void Device::setSensorsTorquesCurrents(map &SensorsIn, int t) { - dg::Vector torques, currents; - - map::iterator it; - - if (pseudoTorqueSOUT_ != 0) { - it = SensorsIn.find("torques"); - if (it != SensorsIn.end()) { - const std::vector& vtorques = SensorsIn["torques"].getValues(); - torques.resize(vtorques.size()); - for (std::size_t i = 0; i < vtorques.size(); ++i) { - torques (i) = vtorques [i]; - } - pseudoTorqueSOUT_->setConstant(torques); - pseudoTorqueSOUT_->setTime(t); - } - } - - if (currentsSOUT_ != 0) { - it = SensorsIn.find("currents"); - if (it != SensorsIn.end()) { - const std::vector& vcurrents = SensorsIn["currents"].getValues(); - currents.resize(vcurrents.size()); - for (std::size_t i = 0; i < vcurrents.size(); ++i) { - currents (i) = vcurrents[i]; - } - currentsSOUT_->setConstant(currents); - currentsSOUT_->setTime(t); - } - } -} - -void Device::setSensorsGains(map &SensorsIn, int t) { - dg::Vector p_gains, d_gains; - - map::iterator it; - if (p_gainsSOUT_ != 0) { - it = SensorsIn.find("p_gains"); - if (it != SensorsIn.end()) { - const std::vector& vp_gains = SensorsIn["p_gains"].getValues(); - p_gains.resize(vp_gains.size()); - for (std::size_t i = 0; i < vp_gains.size(); ++i) { - p_gains (i) = vp_gains[i]; - } - p_gainsSOUT_->setConstant(p_gains); - p_gainsSOUT_->setTime(t); - } - } - - if (d_gainsSOUT_ != 0) { - it = SensorsIn.find("d_gains"); - if (it != SensorsIn.end()) { - const std::vector& vd_gains = SensorsIn["d_gains"].getValues(); - d_gains.resize(vd_gains.size()); - for (std::size_t i = 0; i < vd_gains.size(); ++i) { - d_gains (i) = vd_gains[i]; - } - d_gainsSOUT_->setConstant(d_gains); - d_gainsSOUT_->setTime(t); - } - } -} - -void Device::setSensors(map &SensorsIn) { - sotDEBUGIN(25) ; - int t = motorcontrolSOUT_.getTime () + 1; - - setSensorsForce(SensorsIn, t); - setSensorsIMU(SensorsIn, t); - setSensorsEncoders(SensorsIn, t); - setSensorsVelocities(SensorsIn, t); - setSensorsTorquesCurrents(SensorsIn, t); - setSensorsGains(SensorsIn, t); - - sotDEBUGOUT(25); -} - -void Device::setupSetSensors(map &SensorsIn) { - setSensors (SensorsIn); -} - -void Device::nominalSetSensors(map &SensorsIn) { - setSensors (SensorsIn); -} - - -void Device::cleanupSetSensors(map &SensorsIn) { - setSensors (SensorsIn); -} - -dg::Vector& Device::getControl(dg::Vector &controlOut, const int& t) { - ODEBUG5FULL("start"); - sotDEBUGIN(25); - - // Increment control - increment(t); - sotDEBUG (25) << "control = " << control_ << std::endl; - - ODEBUG5FULL("control = " << control_); - - controlOut = control_; - - return controlOut; - - ODEBUG5FULL("end"); - sotDEBUGOUT(25) ; +void Device::display ( std::ostream& os ) const +{ + os << name <<": "< +using namespace std; + +#include +#include +#include +#include +#include + +using namespace dynamicgraph::sot; +using namespace dynamicgraph; + +#define DBGFILE "/tmp/generic-device.txt" +#define INPUT_CONTROL_SIGNALS +#define INPUT_GAINS_SIGNALS stateGainsSIN << velocityGainsSIN << torqueGainsSIN + +#if 0 +#define RESETDEBUG5() { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::out); \ + DebugFile.close();} +#define ODEBUG5FULL(x) { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::app); \ + DebugFile << __FILE__ << ":" \ + << __FUNCTION__ << "(#" \ + << __LINE__ << "):" << x << std::endl; \ + DebugFile.close();} +#define ODEBUG5(x) { std::ofstream DebugFile; \ + DebugFile.open(DBGFILE,std::ofstream::app); \ + DebugFile << x << std::endl; \ + DebugFile.close();} + +#else +// Void the macro +#define RESETDEBUG5() +#define ODEBUG5FULL(x) +#define ODEBUG5(x) +#endif + +DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GenericDevice, "GenericDevice"); +const double GenericDevice::TIMESTEP_DEFAULT = 0.001; + + +JointSoTHWControlType::JointSoTHWControlType(): + control_index(-1) + , urdf_index(-1) + , temperature_index(-1) + , velocity_index(-1) + , current_index(-1) + , torque_index(-1) + , force_index(-1) + , joint_angle_index(-1) + , motor_angle_index(-1) { +} +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +GenericDevice::~GenericDevice( ) { + for ( unsigned int i = 0; i < forcesSOUT_.size(); ++i ) { + delete forcesSOUT_[i]; + } + + for ( unsigned int i = 0; i < imuSOUT_.size(); ++i ) { + delete imuSOUT_[i]; + } + +} + + +GenericDevice::GenericDevice( const std::string& n ) + : Entity(n) + , timestep_(TIMESTEP_DEFAULT) + , control_(6) + , sanityCheck_(true) + , stateSIN( NULL, "GenericDevice(" + n + ")::input(double)::state" ) + , velocitySIN( NULL, "GenericDevice(" + n + ")::input(double)::velocity" ) + , torqueSIN( NULL, "GenericDevice(" + n + ")::input(double)::torque" ) + , currentSIN( NULL, "GenericDevice(" + n + ")::input(double)::current" ) + , stateGainsSIN( NULL, "GenericDevice(" + n + ")::input(double)::stateGains" ) + , velocityGainsSIN( NULL, "GenericDevice(" + n + ")::input(double)::velocityGains" ) + , torqueGainsSIN( NULL, "GenericDevice(" + n + ")::input(double)::torqueGains" ) + , motorcontrolSOUT_(boost::bind(&GenericDevice::getControl, this, _1, _2), + INPUT_CONTROL_SIGNALS(0), + "GenericDevice(" + n + ")::output(vector)::motorcontrol" ) + , robotState_("GenericDevice(" + n + ")::output(vector)::robotState") + , robotVelocity_("GenericDevice(" + n + ")::output(vector)::robotVelocity") + , forcesSOUT_(0) + , imuSOUT_(0) + , pseudoTorqueSOUT_(0) + , temperatureSOUT_(0) + , currentsSOUT_(0) + , motor_anglesSOUT_(0) + , joint_anglesSOUT_(0) + , debug_mode_(5) + , temperature_index_(0) + , velocity_index_(0) + , current_index_(0) + , torque_index_(0) + , force_index_(0) + , joint_angle_index_(0) + , motor_angle_index_(0) + +{ + + control_.fill(.0); + + /* --- Commands --- */ + { + std::string docstring; + + /* SET of SoT control input type per joint */ + addCommand("setSoTControlType", + command::makeCommandVoid2(*this, &GenericDevice::setSoTControlType, + command::docCommandVoid2 ("Set the type of control input per joint on the SoT side", + "Joint name", + "Control type: [TORQUE|POSITION|VELOCITY]"))); + + /* SET of HW control input type per joint */ + addCommand("setHWControlType", + command::makeCommandVoid2(*this, &GenericDevice::setHWControlType, + command::docCommandVoid2 ("Set HW control input type per joint", + "Joint name", + "Control type: [TORQUE|POSITION|VELOCITY]"))); + + docstring = + "\n" + " Enable/Disable sanity checks\n" + "\n"; + addCommand("setSanityCheck", + new command::Setter + (*this, &GenericDevice::setSanityCheck, docstring)); + + + // Handle commands and signals called in a synchronous way. + periodicCallBefore_.addSpecificCommands(*this, commandMap, "before."); + periodicCallAfter_.addSpecificCommands(*this, commandMap, "after."); + + } +} + +void GenericDevice::RegisterSignals() { + SignalArray sigArray; + for (unsigned int i = 0; i < control_types_.size(); i++) { + if (control_types_[i] == "POSITION") { + sigArray << stateSIN; + motorcontrolSOUT_.addDependency(stateSIN); + } else if (control_types_[i] == "VELOCITY") { + sigArray << velocitySIN; + motorcontrolSOUT_.addDependency(velocitySIN); + } else if (control_types_[i] == "TORQUE") { + sigArray << torqueSIN; + motorcontrolSOUT_.addDependency(torqueSIN); + } else if (control_types_[i] == "CURRENT") { + sigArray << currentSIN; + motorcontrolSOUT_.addDependency(currentSIN); + } + } + + signalRegistration( sigArray + << robotState_ + << robotVelocity_ + << motorcontrolSOUT_); +} + +void GenericDevice::setSanityCheck(const bool & enableCheck) { + sanityCheck_ = enableCheck; +} + +void GenericDevice::setControlType(const std::string &strCtrlType, ControlType &aCtrlType) { + for (int j = 0; j < 2; j++) { + if (strCtrlType == ControlType_s[j]) { + aCtrlType = (ControlType)j; + } + } +} + +void GenericDevice::setSoTControlType(const std::string &jointNames, const std::string &strCtrlType) { + setControlType(strCtrlType, jointDevices_[jointNames].SoTcontrol); +} + +void GenericDevice::setHWControlType(const std::string &jointNames, const std::string &strCtrlType) { + setControlType(strCtrlType, jointDevices_[jointNames].HWcontrol); +} + +void GenericDevice::setControlPos(const std::string &jointName, const unsigned & index) { + jointDevices_[jointName].control_index = index; +} + + +void GenericDevice::setURDFModel(const std::string &aURDFModel) { + model_ = ::urdf::parseURDF(aURDFModel); + + if (!model_) { + dgRTLOG() + << "The XML stream does not contain a valid URDF model." << std::endl; + return; + } + + /// Build the map between urdf file and the alphabetical order. + std::vector< ::urdf::LinkSharedPtr > urdf_links; + model_->getLinks(urdf_links); + for (unsigned j = 0; j < urdf_links.size(); j++) { + std::vector child_joints = urdf_links[j]->child_joints; + urdf_joints_.insert(urdf_joints_.end(), boost::make_move_iterator(child_joints.begin()), + boost::make_move_iterator(child_joints.end())); + } + + std::cout << "urdf_joints_.size(): " << urdf_joints_.size() << std::endl; + for (unsigned int i = 0; i < urdf_joints_.size(); i++) { + jointDevices_[urdf_joints_[i]->name].urdf_index = i; + if (debug_mode_ > 1) { + std::cout << "jointDevices_ index: " << i + << " model_.names[i]: " << urdf_joints_[i]->name + << std::endl; + } + } +} + +void GenericDevice::increment(const int& time) { + // int time = motorcontrolSOUT_.getTime(); + sotDEBUG(25) << "Time : " << time << std::endl; + + // Run Synchronous commands and evaluate signals outside the main + // connected component of the graph. + try { + periodicCallBefore_.run(time + 1); + } catch (std::exception& e) { + dgRTLOG() + << "exception caught while running periodical commands (before): " + << e.what () << std::endl; + } catch (const char* str) { + dgRTLOG() + << "exception caught while running periodical commands (before): " + << str << std::endl; + } catch (...) { + dgRTLOG() + << "unknown exception caught while" + << " running periodical commands (before)" << std::endl; + } + + /* Create control from the input vectors. */ + createControlVector(time); + + sotDEBUG(25) << "q" << time << " = " << control_ << endl; + + // Run Synchronous commands and evaluate signals outside the main + // connected component of the graph. + try { + periodicCallAfter_.run(time + 1); + } catch (std::exception& e) { + dgRTLOG() + << "exception caught while running periodical commands (after): " + << e.what () << std::endl; + } catch (const char* str) { + dgRTLOG() + << "exception caught while running periodical commands (after): " + << str << std::endl; + } catch (...) { + dgRTLOG() + << "unknown exception caught while" + << "running periodical commands (after)" << std::endl; + } +} + +void GenericDevice::createControlVector(const int& time) { + /* Force the recomputation of the input vectors. */ + Vector state; + Vector vel; + Vector torque; + Vector current; + for (unsigned int i = 0; i < control_types_.size(); i++) { + if (control_types_[i] == "POSITION") { + stateSIN.recompute(time); + state = stateSIN.accessCopy(); + } else if (control_types_[i] == "VELOCITY") { + velocitySIN.recompute(time); + vel = velocitySIN.accessCopy(); + } else if (control_types_[i] == "TORQUE") { + torqueSIN.recompute(time); + torque = torqueSIN.accessCopy(); + } else if (control_types_[i] == "CURRENT") { + currentSIN.recompute(time); + current = currentSIN.accessCopy(); + } + } + + JointSHWControlType_iterator it_control_type; + for (it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) { + + int lctl_index = it_control_type->second.control_index; + int u_index = it_control_type->second.urdf_index; + + if (lctl_index == -1) { + if (debug_mode_ > 1) { + std::cerr << "No control index for joint " + << urdf_joints_[u_index]->name << std::endl; + } + break; + } + if (u_index != -1) { + // POSITION -> stateSIN + Check Position limits. + if (it_control_type->second.SoTcontrol == POSITION) { + control_[lctl_index] = state[lctl_index]; + + if (urdf_joints_[u_index]->limits) { + double lowerLim = urdf_joints_[u_index]->limits->lower; + double upperLim = urdf_joints_[u_index]->limits->upper; + if (control_[lctl_index] < lowerLim) { + control_[lctl_index] = lowerLim; + } else if (control_[lctl_index] > upperLim) { + control_[lctl_index] = upperLim; + } + } + } + // TORQUE -> torqueSIN + Check Torque limits. + else if (it_control_type->second.SoTcontrol == TORQUE) { + control_[lctl_index] = torque[lctl_index]; + + if (urdf_joints_[u_index]->limits) { + double lim = urdf_joints_[u_index]->limits->effort; + if (control_[lctl_index] < -lim) { + control_[lctl_index] = -lim; + } else if (control_[lctl_index] > lim) { + control_[lctl_index] = lim; + } + } + } + // VELOCITY -> velSIN + else if (it_control_type->second.SoTcontrol == VELOCITY) { + control_[lctl_index] = vel[lctl_index]; + } + // CURRENT -> currentSIN + else if (it_control_type->second.SoTcontrol == CURRENT) { + control_[lctl_index] = current[lctl_index]; + } + } + } + if (sanityCheck_ && control_.hasNaN()) { + dgRTLOG () << "GenericDevice::updateControl: Control has NaN values: " << '\n' + << control_.transpose() << '\n'; + return; + } +} + +int GenericDevice::ParseYAMLString(const std::string & aYamlString) { + YAML::Node map_global = YAML::Load(aYamlString); + + YAML::Node map_sot_controller = map_global["sot_controller"]; + + for (YAML::const_iterator it = map_sot_controller.begin(); + it != map_sot_controller.end(); + it++) { + std::string name_elt_of_sot_controller = it->first.as(); + + if (debug_mode_ > 1) { + std::cout << "key:" << name_elt_of_sot_controller << std::endl; + } + + YAML::Node elt_of_sot_controller = it->second; + + if (name_elt_of_sot_controller == "joint_names") { + int r = ParseYAMLMapHardwareJointNames(elt_of_sot_controller); + if (r < 0) return r; + } else if (name_elt_of_sot_controller == "map_rc_to_sot_device") { + int r = ParseYAMLJointSensor(elt_of_sot_controller); + if (r < 0) return r; + } else if (name_elt_of_sot_controller == "control_mode") { + int r = ParseYAMLMapHardwareControlMode(elt_of_sot_controller); + if (r < 0) return r; + } else if (name_elt_of_sot_controller.find("sensor") != std::string::npos) { + int r = ParseYAMLSensors(elt_of_sot_controller); + if (r < 0) return r; + } + } + RegisterSignals(); + UpdateSignals(); + return 0; +} + +int GenericDevice::ParseYAMLJointSensor(YAML::Node &aJointSensor) { + for (YAML::const_iterator it = aJointSensor.begin(); + it != aJointSensor.end(); + it++) { + std::string aSensor = it->first.as(); + if (debug_mode_ > 1) { + std::cout << "Found " << aSensor << std::endl; + } + JointSHWControlType_iterator it_control_type; + for (it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) { + if (aSensor == "temperature") { + it_control_type->second.temperature_index = temperature_index_; + temperature_index_++; + } else if (aSensor == "velocities") { + it_control_type->second.velocity_index = velocity_index_; + velocity_index_++; + } else if (aSensor == "currents") { + it_control_type->second.current_index = current_index_; + current_index_++; + } else if (aSensor == "torques") { + it_control_type->second.torque_index = torque_index_; + torque_index_++; + } else if (aSensor == "forces") { + it_control_type->second.force_index = force_index_; + force_index_++; + } else if (aSensor == "joint_angles") { + it_control_type->second.joint_angle_index = joint_angle_index_; + joint_angle_index_++; + } else if (aSensor == "motor_angles") { + it_control_type->second.motor_angle_index = motor_angle_index_; + motor_angle_index_++; + } + } + } + return 0; +} + +int GenericDevice::ParseYAMLMapHardwareJointNames(YAML::Node & map_joint_name) { + if (control_.size() != map_joint_name.size()) { + control_.resize(map_joint_name.size()); + } + + if (debug_mode_ > 1) { + std::cout << "map_joint_name.size(): " + << map_joint_name.size() << std::endl; + std::cout << map_joint_name << std::endl; + } + for (unsigned int i = 0; i < map_joint_name.size(); i++) { + std::string jointName = map_joint_name[i].as(); + if (debug_mode_ > 1) { + std::cout << "-- Joint: " << jointName << " has index: " << i << std::endl; + } + setControlPos(jointName, i); + } + return 0; +} + +int GenericDevice::ParseYAMLMapHardwareControlMode(YAML::Node & map_control_mode) { + if (debug_mode_ > 1) { + std::cout << "map_control_mode.size(): " + << map_control_mode.size() << std::endl; + std::cout << map_control_mode << std::endl; + } + + if (map_control_mode.size() == 0) { + std::string value = map_control_mode.as(); + JointSHWControlType_iterator it_control_type; + for (it_control_type = jointDevices_.begin(); + it_control_type != jointDevices_.end(); + it_control_type++) { + int u_index = it_control_type->second.urdf_index; + setSoTControlType(urdf_joints_[u_index]->name, value); + control_types_.push_back(value); + } + if (debug_mode_ > 1) { + std::cout << "All joints are controlled in: " << value << std::endl; + } + return 0; + } + + for (YAML::const_iterator it = map_control_mode.begin(); + it != map_control_mode.end(); + it++) { + std::string jointName = it->first.as(); + if (debug_mode_ > 1) { + std::cout << "joint name: " << jointName << std::endl; + } + + YAML::Node aNode = it->second; + + if (debug_mode_ > 1) { + std::cout << "Type of value: " << aNode.Type() << std::endl; + } + + for (YAML::const_iterator it2 = aNode.begin(); + it2 != aNode.end(); + it2++) { + std::string aKey = it2->first.as(); + if (debug_mode_ > 1) { + std::cout << "-- key:" << aKey << std::endl; + } + if (aKey == "hw_control_mode") { + std::string value = it2->second.as(); + if (debug_mode_ > 1) { + std::cout << "-- Value: " << value << std::endl; + } + setHWControlType(jointName, value); + } else if (aKey == "ros_control_mode") { + std::string value = it2->second.as(); + if (debug_mode_ > 1) { + std::cout << "-- Value: " << value << std::endl; + } + setSoTControlType(jointName, value); + control_types_.push_back(value); + } + } + } + return 0; +} + +/* Sensor signals */ +int GenericDevice::ParseYAMLSensors(YAML::Node &map_sensors) { + if (map_sensors.IsNull()) { + std::cerr << "GenericDevice::ParseYAMLString: No sensor detected in YamlString " << std::endl; + return -1; + } + std::string sensor_name = map_sensors.as(); + if (debug_mode_ > 1) { + std::cout << "-- sensor_name:" << sensor_name << std::endl; + } + if (sensor_name.find("ft") != std::string::npos) { + CreateAForceSignal(sensor_name); + } + + else if (sensor_name.find("imu") != std::string::npos) { + CreateAnImuSignal(sensor_name); + } else { + std::cerr << "The sensor " << sensor_name + << " is not recognized" << std::endl; + return 1; + } + return 0; +} + + +void GenericDevice::CreateAForceSignal(const std::string & force_sensor_name) { + dynamicgraph::Signal * aForceSOUT_; + /* --- SIGNALS --- */ + aForceSOUT_ = new Signal("GenericDevice(" + getName() + ")::output(vector6)::" + + force_sensor_name); + forcesSOUT_.push_back(aForceSOUT_); + signalRegistration(*aForceSOUT_); +} + +void GenericDevice::CreateAnImuSignal(const std::string &imu_sensor_name) { + IMUSOUT * anImuSOUT_; + /* --- SIGNALS --- */ + anImuSOUT_ = new IMUSOUT(imu_sensor_name, getName()); + imuSOUT_.push_back(anImuSOUT_); + signalRegistration(anImuSOUT_->attitudeSOUT + << anImuSOUT_->accelerometerSOUT + << anImuSOUT_->gyrometerSOUT); +} + + +int GenericDevice::UpdateSignals() { + if ((torque_index_ != 0) && (pseudoTorqueSOUT_ != 0)) { + pseudoTorqueSOUT_ = new Signal("GenericDevice(" + getName() + ")::output(vector)::ptorque"); + } + // if ((force_index_!=0) && (forcesSOUT_.size()!=0)) + // { + // forcesSOUT_ = new Signal("GenericDevice("+getName()+")::output(vector)::forces"); + // } + if ((current_index_ != 0) && (currentsSOUT_ != 0)) { + currentsSOUT_ = new Signal("GenericDevice(" + getName() + ")::output(vector)::currents"); + } + + if ((temperature_index_ != 0) && (temperatureSOUT_ != 0)) { + temperatureSOUT_ = new Signal("GenericDevice(" + getName() + ")::output(vector)::temperatures"); + } + + if ((motor_angle_index_ != 0) && (motor_anglesSOUT_ != 0)) { + motor_anglesSOUT_ = new Signal("GenericDevice(" + getName() + ")::output(vector)::motor_angles"); + } + + if ((joint_angle_index_ != 0) && (joint_anglesSOUT_ != 0)) { + joint_anglesSOUT_ = new Signal("GenericDevice(" + getName() + ")::output(vector)::joint_angles"); + } + + return 0; +} + + + +/* --- DISPLAY ------------------------------------------------------------ */ + +void GenericDevice::display ( std::ostream& os ) const { + os << name << ": " << control_ << endl; +} + +/* Helpers for the controller */ +void GenericDevice::setSensorsForce(map &SensorsIn, int t) { + Eigen::Matrix dgforces; + + sotDEBUGIN(15); + map::iterator it; + it = SensorsIn.find("forces"); + if (it != SensorsIn.end()) { + // Implements force recollection. + const vector& forcesIn = it->second.getValues(); + for (std::size_t i = 0; i < forcesSOUT_.size(); ++i) { + sotDEBUG(15) << "Force sensor " << i << std::endl; + for (int j = 0; j < 6; ++j) { + dgforces(j) = forcesIn[i * 6 + j]; + sotDEBUG(15) << "Force value " << j << ":" << dgforces(j) << std::endl; + } + forcesSOUT_[i]->setConstant(dgforces); + forcesSOUT_[i]->setTime (t); + } + } + sotDEBUGIN(15); +} + +void GenericDevice::setSensorsIMU(map &SensorsIn, int t) { + Eigen::Matrix aVector3d; + + map::iterator it; + + //TODO: Confirm if this can be made quaternion + for (std::size_t k = 0; k < imuSOUT_.size(); ++k) { + it = SensorsIn.find("attitude"); + if (it != SensorsIn.end()) { + const vector& attitude = it->second.getValues (); + Eigen::Matrix pose; + + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { + pose (i, j) = attitude[i * 3 + j]; + } + } + imuSOUT_[k]->attitudeSOUT.setConstant (pose); + imuSOUT_[k]->attitudeSOUT.setTime (t); + } + + it = SensorsIn.find("accelerometer_0"); + if (it != SensorsIn.end()) { + const vector& accelerometer = + SensorsIn ["accelerometer_0"].getValues (); + for (std::size_t i = 0; i < 3; ++i) { + aVector3d(i) = accelerometer [i]; + } + imuSOUT_[k]->accelerometerSOUT.setConstant (aVector3d); + imuSOUT_[k]->accelerometerSOUT.setTime (t); + } + + it = SensorsIn.find("gyrometer_0"); + if (it != SensorsIn.end()) { + const vector& gyrometer = SensorsIn ["gyrometer_0"].getValues (); + for (std::size_t i = 0; i < 3; ++i) { + aVector3d(i) = gyrometer [i]; + } + imuSOUT_[k]->gyrometerSOUT.setConstant (aVector3d); + imuSOUT_[k]->gyrometerSOUT.setTime (t); + } + } +} + +void GenericDevice::setSensorsEncoders(map &SensorsIn, int t) { + dg::Vector dgRobotState, motor_angles, joint_angles; + map::iterator it; + + if (motor_anglesSOUT_ != 0) { + it = SensorsIn.find("motor-angles"); + if (it != SensorsIn.end()) { + const vector& anglesIn = it->second.getValues(); + dgRobotState.resize (anglesIn.size () + 6); + motor_angles.resize(anglesIn.size ()); + for (unsigned i = 0; i < 6; ++i) { + dgRobotState (i) = 0.; + } + for (unsigned i = 0; i < anglesIn.size(); ++i) { + dgRobotState (i + 6) = anglesIn[i]; + motor_angles(i) = anglesIn[i]; + } + robotState_.setConstant(dgRobotState); + robotState_.setTime(t); + motor_anglesSOUT_->setConstant(motor_angles); + motor_anglesSOUT_->setTime(t); + } + } + + if (joint_anglesSOUT_ != 0) { + it = SensorsIn.find("joint-angles"); + if (it != SensorsIn.end()) { + const vector& joint_anglesIn = it->second.getValues(); + joint_angles.resize (joint_anglesIn.size () ); + for (unsigned i = 0; i < joint_anglesIn.size(); ++i) { + joint_angles (i) = joint_anglesIn[i]; + } + joint_anglesSOUT_->setConstant(joint_angles); + joint_anglesSOUT_->setTime(t); + } + } +} + +void GenericDevice::setSensorsVelocities(map &SensorsIn, int t) { + dg::Vector dgRobotVelocity; + + map::iterator it; + + it = SensorsIn.find("velocities"); + if (it != SensorsIn.end()) { + const vector& velocitiesIn = it->second.getValues(); + dgRobotVelocity.resize (velocitiesIn.size () + 6); + for (unsigned i = 0; i < 6; ++i) { + dgRobotVelocity (i) = 0.; + } + for (unsigned i = 0; i < velocitiesIn.size(); ++i) { + dgRobotVelocity (i + 6) = velocitiesIn[i]; + } + robotVelocity_.setConstant(dgRobotVelocity); + robotVelocity_.setTime(t); + } +} + +void GenericDevice::setSensorsTorquesCurrents(map &SensorsIn, int t) { + dg::Vector torques, currents; + + map::iterator it; + + if (pseudoTorqueSOUT_ != 0) { + it = SensorsIn.find("torques"); + if (it != SensorsIn.end()) { + const std::vector& vtorques = SensorsIn["torques"].getValues(); + torques.resize(vtorques.size()); + for (std::size_t i = 0; i < vtorques.size(); ++i) { + torques (i) = vtorques [i]; + } + pseudoTorqueSOUT_->setConstant(torques); + pseudoTorqueSOUT_->setTime(t); + } + } + + if (currentsSOUT_ != 0) { + it = SensorsIn.find("currents"); + if (it != SensorsIn.end()) { + const std::vector& vcurrents = SensorsIn["currents"].getValues(); + currents.resize(vcurrents.size()); + for (std::size_t i = 0; i < vcurrents.size(); ++i) { + currents (i) = vcurrents[i]; + } + currentsSOUT_->setConstant(currents); + currentsSOUT_->setTime(t); + } + } +} + +void GenericDevice::setSensorsGains(map &SensorsIn, int t) { + dg::Vector p_gains, d_gains; + + map::iterator it; + if (p_gainsSOUT_ != 0) { + it = SensorsIn.find("p_gains"); + if (it != SensorsIn.end()) { + const std::vector& vp_gains = SensorsIn["p_gains"].getValues(); + p_gains.resize(vp_gains.size()); + for (std::size_t i = 0; i < vp_gains.size(); ++i) { + p_gains (i) = vp_gains[i]; + } + p_gainsSOUT_->setConstant(p_gains); + p_gainsSOUT_->setTime(t); + } + } + + if (d_gainsSOUT_ != 0) { + it = SensorsIn.find("d_gains"); + if (it != SensorsIn.end()) { + const std::vector& vd_gains = SensorsIn["d_gains"].getValues(); + d_gains.resize(vd_gains.size()); + for (std::size_t i = 0; i < vd_gains.size(); ++i) { + d_gains (i) = vd_gains[i]; + } + d_gainsSOUT_->setConstant(d_gains); + d_gainsSOUT_->setTime(t); + } + } +} + +void GenericDevice::setSensors(map &SensorsIn) { + sotDEBUGIN(25) ; + int t = motorcontrolSOUT_.getTime () + 1; + + setSensorsForce(SensorsIn, t); + setSensorsIMU(SensorsIn, t); + setSensorsEncoders(SensorsIn, t); + setSensorsVelocities(SensorsIn, t); + setSensorsTorquesCurrents(SensorsIn, t); + setSensorsGains(SensorsIn, t); + + sotDEBUGOUT(25); +} + +void GenericDevice::setupSetSensors(map &SensorsIn) { + setSensors (SensorsIn); +} + +void GenericDevice::nominalSetSensors(map &SensorsIn) { + setSensors (SensorsIn); +} + + +void GenericDevice::cleanupSetSensors(map &SensorsIn) { + setSensors (SensorsIn); +} + +dg::Vector& GenericDevice::getControl(dg::Vector &controlOut, const int& t) { + ODEBUG5FULL("start"); + sotDEBUGIN(25); + + // Increment control + increment(t); + sotDEBUG (25) << "control = " << control_ << std::endl; + + ODEBUG5FULL("control = " << control_); + + controlOut = control_; + + return controlOut; + + ODEBUG5FULL("end"); + sotDEBUGOUT(25) ; +} \ No newline at end of file diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt index c39651a3a..29d1df15b 100644 --- a/unitTesting/CMakeLists.txt +++ b/unitTesting/CMakeLists.txt @@ -98,7 +98,7 @@ SET (tests ) IF (YAML_CPP_FOUND AND URDFDOM_FOUND) - SET(tests ${tests} tools/test_device ) + SET(tests ${tests} tools/test_generic_device ) ENDIF () # TODO diff --git a/unitTesting/tools/test_device.cpp b/unitTesting/tools/test_generic_device.cpp similarity index 98% rename from unitTesting/tools/test_device.cpp rename to unitTesting/tools/test_generic_device.cpp index f95872766..e79d5e334 100644 --- a/unitTesting/tools/test_device.cpp +++ b/unitTesting/tools/test_generic_device.cpp @@ -18,7 +18,7 @@ using namespace std; #include #include -#include +#include #include #include @@ -183,7 +183,7 @@ void CreateYAMLFILE() { of.close(); } -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 // Comment and use the commented code to use the above yaml file std::ifstream yaml_file_controller("../../unitTesting/tools/sot_controller.yaml"); @@ -226,7 +226,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 c239120519dd40fb47b0c8b0c390d8795e4b936f Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Tue, 23 Jun 2020 19:15:01 +0200 Subject: [PATCH 11/43] At initialization read /robot_description model and save it in the internal sot based parameter-server. --- include/sot/core/parameter-server.hh | 5 ++++ include/sot/core/robot-utils.hh | 1 + src/tools/parameter-server.cpp | 36 ++++++++++++++++++++++++++++ src/tools/robot-utils.cpp | 15 ++++++++++++ 4 files changed, 57 insertions(+) diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh index 729f7c3af..d648563ce 100644 --- a/include/sot/core/parameter-server.hh +++ b/include/sot/core/parameter-server.hh @@ -66,6 +66,11 @@ public: void init(const double &dt, const std::string &urdfFile, const std::string &robotRef); + /// Initialize + /// The dtparam is found from ros_param + /// The urdf model is found by reading /robot_description + /// The robot name is found using the name inside robot_description + void init_simple(); /* --- SIGNALS --- */ /* --- COMMANDS --- */ diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh index 0cf7fcf28..a6da9a110 100644 --- a/include/sot/core/robot-utils.hh +++ b/include/sot/core/robot-utils.hh @@ -246,6 +246,7 @@ RobotUtilShrPtr RefVoidRobotUtil(); RobotUtilShrPtr getRobotUtil(std::string &robotName); bool isNameInRobotUtil(std::string &robotName); RobotUtilShrPtr createRobotUtil(std::string &robotName); +std::shared_ptr< std::vector > getListOfRobots(); bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot); diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index e87c0c2f5..1ac612459 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -60,6 +60,9 @@ ParameterServer::ParameterServer(const std::string &name) "Time period in seconds (double)", "URDF file path (string)", "Robot reference (string)"))); + addCommand("init_simple", + makeCommandVoid0(*this, &ParameterServer::init_simple, + docCommandVoid0("Initialize the entity."))); addCommand("setNameToId", makeCommandVoid2(*this, &ParameterServer::setNameToId, @@ -138,6 +141,39 @@ ParameterServer::ParameterServer(const std::string &name) "(string) ParameterName"))); } +void ParameterServer::init_simple() { + + m_emergency_stop_triggered = false; + m_initSucceeded = true; + + std::string localName; + std::shared_ptr< std::vector > + listOfRobots = sot::getListOfRobots(); + + std::cerr << "listOfRobots.size()=" + << listOfRobots->size() + << std::endl; + + if (listOfRobots->size()==1) + localName = (*listOfRobots)[0]; + + std::cerr << "localName" << localName.c_str() + << std::endl; + + if (!isNameInRobotUtil(localName)) { + m_robot_util = createRobotUtil(localName); + } else { + m_robot_util = getRobotUtil(localName); + } + + addCommand( + "getJointsUrdfToSot", + makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot, + docDirectSetter("Display map Joints From URDF to SoT.", + "Vector of integer for mapping"))); + +} + void ParameterServer::init(const double &dt, const std::string &urdfFile, const std::string &robotRef) { if (dt <= 0.0) diff --git a/src/tools/robot-utils.cpp b/src/tools/robot-utils.cpp index 899b385d7..fbc545825 100644 --- a/src/tools/robot-utils.cpp +++ b/src/tools/robot-utils.cpp @@ -438,6 +438,21 @@ bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) { std::map sgl_map_name_to_robot_util; +std::shared_ptr > getListOfRobots() { + std::shared_ptr > res = + std::make_shared >(); + + std::map::iterator it = + sgl_map_name_to_robot_util.begin(); + while (it != sgl_map_name_to_robot_util.end()) + { + res->push_back(it->first); + it++; + } + + return res; +} + RobotUtilShrPtr getRobotUtil(std::string &robotName) { std::map::iterator it = sgl_map_name_to_robot_util.find(robotName); From 609349004bce6339d59baad64118633f31607b3f Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Wed, 24 Jun 2020 13:05:46 +0200 Subject: [PATCH 12/43] Clean up code, add exception-tools and take dt as an input for init_simple. --- include/sot/core/exception-tools.hh | 3 ++- include/sot/core/parameter-server.hh | 4 ++-- src/tools/parameter-server.cpp | 25 +++++++++++++++---------- 3 files changed, 19 insertions(+), 13 deletions(-) diff --git a/include/sot/core/exception-tools.hh b/include/sot/core/exception-tools.hh index af1948720..29600e17d 100644 --- a/include/sot/core/exception-tools.hh +++ b/include/sot/core/exception-tools.hh @@ -34,7 +34,8 @@ public: , CORBA, - KALMAN_SIZE + KALMAN_SIZE, + PARAMETER_SERVER }; static const std::string EXCEPTION_NAME; diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh index d648563ce..0a1b64aaa 100644 --- a/include/sot/core/parameter-server.hh +++ b/include/sot/core/parameter-server.hh @@ -67,10 +67,10 @@ public: const std::string &robotRef); /// Initialize - /// The dtparam is found from ros_param + /// @param dt: control interval provided by the device. /// The urdf model is found by reading /robot_description /// The robot name is found using the name inside robot_description - void init_simple(); + void init_simple(const double &dt); /* --- SIGNALS --- */ /* --- COMMANDS --- */ diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index 1ac612459..4d1d0d60a 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace dynamicgraph { @@ -61,8 +62,10 @@ ParameterServer::ParameterServer(const std::string &name) "URDF file path (string)", "Robot reference (string)"))); addCommand("init_simple", - makeCommandVoid0(*this, &ParameterServer::init_simple, - docCommandVoid0("Initialize the entity."))); + makeCommandVoid1(*this, &ParameterServer::init_simple, + docCommandVoid1("Initialize the entity.", + "Time period in seconds (double)" + ))); addCommand("setNameToId", makeCommandVoid2(*this, &ParameterServer::setNameToId, @@ -141,7 +144,12 @@ ParameterServer::ParameterServer(const std::string &name) "(string) ParameterName"))); } -void ParameterServer::init_simple() { +void ParameterServer::init_simple(const double & dt) { + + if (dt <= 0.0) + return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); + + m_dt = dt; m_emergency_stop_triggered = false; m_initSucceeded = true; @@ -150,16 +158,13 @@ void ParameterServer::init_simple() { std::shared_ptr< std::vector > listOfRobots = sot::getListOfRobots(); - std::cerr << "listOfRobots.size()=" - << listOfRobots->size() - << std::endl; - if (listOfRobots->size()==1) localName = (*listOfRobots)[0]; + else { + throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER, + "No robot registered in the parameter server list"); + } - std::cerr << "localName" << localName.c_str() - << std::endl; - if (!isNameInRobotUtil(localName)) { m_robot_util = createRobotUtil(localName); } else { From e5b7857f75fbe6fc17476ebc683d7bfdceea73f0 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Wed, 24 Jun 2020 16:28:31 +0200 Subject: [PATCH 13/43] Add virtual destructor and simplify the code. --- include/sot/core/parameter-server.hh | 2 ++ src/tools/parameter-server.cpp | 7 +------ 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh index 0a1b64aaa..c6e858a52 100644 --- a/include/sot/core/parameter-server.hh +++ b/include/sot/core/parameter-server.hh @@ -60,6 +60,8 @@ public: /* --- CONSTRUCTOR ---- */ ParameterServer(const std::string &name); + ~ParameterServer() {}; + /// Initialize /// @param dt: control interval /// @param urdfFile: path to the URDF model of the robot diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index 4d1d0d60a..11fed8d80 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -154,7 +154,7 @@ void ParameterServer::init_simple(const double & dt) { m_emergency_stop_triggered = false; m_initSucceeded = true; - std::string localName; + std::string localName("robot"); std::shared_ptr< std::vector > listOfRobots = sot::getListOfRobots(); @@ -196,11 +196,6 @@ void ParameterServer::init(const double &dt, const std::string &urdfFile, m_robot_util->m_urdf_filename = urdfFile; - addCommand( - "getJointsUrdfToSot", - makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot, - docDirectSetter("Display map Joints From URDF to SoT.", - "Vector of integer for mapping"))); } /* ------------------------------------------------------------------- */ From a9294b7e390d6cc79d015f21d71f4b257321b28e Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 09:03:00 +0200 Subject: [PATCH 14/43] Revert "Add virtual destructor and simplify the code." This reverts commit e5b7857f75fbe6fc17476ebc683d7bfdceea73f0. Do not merge directly in master... --- include/sot/core/parameter-server.hh | 2 -- src/tools/parameter-server.cpp | 7 ++++++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh index c6e858a52..0a1b64aaa 100644 --- a/include/sot/core/parameter-server.hh +++ b/include/sot/core/parameter-server.hh @@ -60,8 +60,6 @@ public: /* --- CONSTRUCTOR ---- */ ParameterServer(const std::string &name); - ~ParameterServer() {}; - /// Initialize /// @param dt: control interval /// @param urdfFile: path to the URDF model of the robot diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index 11fed8d80..4d1d0d60a 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -154,7 +154,7 @@ void ParameterServer::init_simple(const double & dt) { m_emergency_stop_triggered = false; m_initSucceeded = true; - std::string localName("robot"); + std::string localName; std::shared_ptr< std::vector > listOfRobots = sot::getListOfRobots(); @@ -196,6 +196,11 @@ void ParameterServer::init(const double &dt, const std::string &urdfFile, m_robot_util->m_urdf_filename = urdfFile; + addCommand( + "getJointsUrdfToSot", + makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot, + docDirectSetter("Display map Joints From URDF to SoT.", + "Vector of integer for mapping"))); } /* ------------------------------------------------------------------- */ From 61c35e7577c705dc8e2da0a08301c3f61d8ed02f Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 09:05:59 +0200 Subject: [PATCH 15/43] Revert "Clean up code, add exception-tools and take dt as an input for init_simple." This reverts commit 609349004bce6339d59baad64118633f31607b3f. --- include/sot/core/exception-tools.hh | 3 +-- include/sot/core/parameter-server.hh | 4 ++-- src/tools/parameter-server.cpp | 25 ++++++++++--------------- 3 files changed, 13 insertions(+), 19 deletions(-) diff --git a/include/sot/core/exception-tools.hh b/include/sot/core/exception-tools.hh index 29600e17d..af1948720 100644 --- a/include/sot/core/exception-tools.hh +++ b/include/sot/core/exception-tools.hh @@ -34,8 +34,7 @@ public: , CORBA, - KALMAN_SIZE, - PARAMETER_SERVER + KALMAN_SIZE }; static const std::string EXCEPTION_NAME; diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh index 0a1b64aaa..d648563ce 100644 --- a/include/sot/core/parameter-server.hh +++ b/include/sot/core/parameter-server.hh @@ -67,10 +67,10 @@ public: const std::string &robotRef); /// Initialize - /// @param dt: control interval provided by the device. + /// The dtparam is found from ros_param /// The urdf model is found by reading /robot_description /// The robot name is found using the name inside robot_description - void init_simple(const double &dt); + void init_simple(); /* --- SIGNALS --- */ /* --- COMMANDS --- */ diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index 4d1d0d60a..1ac612459 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include namespace dynamicgraph { @@ -62,10 +61,8 @@ ParameterServer::ParameterServer(const std::string &name) "URDF file path (string)", "Robot reference (string)"))); addCommand("init_simple", - makeCommandVoid1(*this, &ParameterServer::init_simple, - docCommandVoid1("Initialize the entity.", - "Time period in seconds (double)" - ))); + makeCommandVoid0(*this, &ParameterServer::init_simple, + docCommandVoid0("Initialize the entity."))); addCommand("setNameToId", makeCommandVoid2(*this, &ParameterServer::setNameToId, @@ -144,12 +141,7 @@ ParameterServer::ParameterServer(const std::string &name) "(string) ParameterName"))); } -void ParameterServer::init_simple(const double & dt) { - - if (dt <= 0.0) - return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); - - m_dt = dt; +void ParameterServer::init_simple() { m_emergency_stop_triggered = false; m_initSucceeded = true; @@ -158,13 +150,16 @@ void ParameterServer::init_simple(const double & dt) { std::shared_ptr< std::vector > listOfRobots = sot::getListOfRobots(); + std::cerr << "listOfRobots.size()=" + << listOfRobots->size() + << std::endl; + if (listOfRobots->size()==1) localName = (*listOfRobots)[0]; - else { - throw ExceptionTools(ExceptionTools::ErrorCodeEnum::PARAMETER_SERVER, - "No robot registered in the parameter server list"); - } + std::cerr << "localName" << localName.c_str() + << std::endl; + if (!isNameInRobotUtil(localName)) { m_robot_util = createRobotUtil(localName); } else { From 9109cd8c89130c22143b8ad54c5605b2c08b7b7c Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 09:06:12 +0200 Subject: [PATCH 16/43] Revert "At initialization read /robot_description model" This reverts commit c239120519dd40fb47b0c8b0c390d8795e4b936f. --- include/sot/core/parameter-server.hh | 5 ---- include/sot/core/robot-utils.hh | 1 - src/tools/parameter-server.cpp | 36 ---------------------------- src/tools/robot-utils.cpp | 15 ------------ 4 files changed, 57 deletions(-) diff --git a/include/sot/core/parameter-server.hh b/include/sot/core/parameter-server.hh index d648563ce..729f7c3af 100644 --- a/include/sot/core/parameter-server.hh +++ b/include/sot/core/parameter-server.hh @@ -66,11 +66,6 @@ public: void init(const double &dt, const std::string &urdfFile, const std::string &robotRef); - /// Initialize - /// The dtparam is found from ros_param - /// The urdf model is found by reading /robot_description - /// The robot name is found using the name inside robot_description - void init_simple(); /* --- SIGNALS --- */ /* --- COMMANDS --- */ diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh index a6da9a110..0cf7fcf28 100644 --- a/include/sot/core/robot-utils.hh +++ b/include/sot/core/robot-utils.hh @@ -246,7 +246,6 @@ RobotUtilShrPtr RefVoidRobotUtil(); RobotUtilShrPtr getRobotUtil(std::string &robotName); bool isNameInRobotUtil(std::string &robotName); RobotUtilShrPtr createRobotUtil(std::string &robotName); -std::shared_ptr< std::vector > getListOfRobots(); bool base_se3_to_sot(ConstRefVector pos, ConstRefMatrix R, RefVector q_sot); diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index 1ac612459..e87c0c2f5 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -60,9 +60,6 @@ ParameterServer::ParameterServer(const std::string &name) "Time period in seconds (double)", "URDF file path (string)", "Robot reference (string)"))); - addCommand("init_simple", - makeCommandVoid0(*this, &ParameterServer::init_simple, - docCommandVoid0("Initialize the entity."))); addCommand("setNameToId", makeCommandVoid2(*this, &ParameterServer::setNameToId, @@ -141,39 +138,6 @@ ParameterServer::ParameterServer(const std::string &name) "(string) ParameterName"))); } -void ParameterServer::init_simple() { - - m_emergency_stop_triggered = false; - m_initSucceeded = true; - - std::string localName; - std::shared_ptr< std::vector > - listOfRobots = sot::getListOfRobots(); - - std::cerr << "listOfRobots.size()=" - << listOfRobots->size() - << std::endl; - - if (listOfRobots->size()==1) - localName = (*listOfRobots)[0]; - - std::cerr << "localName" << localName.c_str() - << std::endl; - - if (!isNameInRobotUtil(localName)) { - m_robot_util = createRobotUtil(localName); - } else { - m_robot_util = getRobotUtil(localName); - } - - addCommand( - "getJointsUrdfToSot", - makeDirectGetter(*this, &m_robot_util->m_dgv_urdf_to_sot, - docDirectSetter("Display map Joints From URDF to SoT.", - "Vector of integer for mapping"))); - -} - void ParameterServer::init(const double &dt, const std::string &urdfFile, const std::string &robotRef) { if (dt <= 0.0) diff --git a/src/tools/robot-utils.cpp b/src/tools/robot-utils.cpp index fbc545825..899b385d7 100644 --- a/src/tools/robot-utils.cpp +++ b/src/tools/robot-utils.cpp @@ -438,21 +438,6 @@ bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf) { std::map sgl_map_name_to_robot_util; -std::shared_ptr > getListOfRobots() { - std::shared_ptr > res = - std::make_shared >(); - - std::map::iterator it = - sgl_map_name_to_robot_util.begin(); - while (it != sgl_map_name_to_robot_util.end()) - { - res->push_back(it->first); - it++; - } - - return res; -} - RobotUtilShrPtr getRobotUtil(std::string &robotName) { std::map::iterator it = sgl_map_name_to_robot_util.find(robotName); From beeb05d9683c433de603f50d6ed2923243918868 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Tue, 16 Jun 2020 10:22:08 +0200 Subject: [PATCH 17/43] [cmake] Add target_include_directories for Boost. This is missing when compiling with clang on Mac OS Catalina. Moreover it seems needed anyway. --- CMakeLists.txt | 2 ++ tests/CMakeLists.txt | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 642ad1725..1415ecdc4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -170,6 +170,8 @@ SET(${PROJECT_NAME}_SOURCES ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $) +TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} + SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES} dynamic-graph::dynamic-graph pinocchio::pinocchio) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5b6311d8a..e0666b881 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -106,6 +106,9 @@ IF(UNIX) ADD_EXECUTABLE(test_abstract_interface tools/test_abstract_interface.cpp) TARGET_LINK_LIBRARIES(test_abstract_interface pluginabstract ${CMAKE_DL_LIBS} ${Boost_LIBRARIES}) + TARGET_INCLUDE_DIRECTORIES(test_abstract_interface + SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) + ENDIF(UNIX) FOREACH(path ${tests}) @@ -116,6 +119,9 @@ FOREACH(path ${tests}) ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS} dynamic-graph::dynamic-graph) + TARGET_INCLUDE_DIRECTORIES(${test} + SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) + IF(UNIX) TARGET_LINK_LIBRARIES(${test} ${CMAKE_DL_LIBS}) ENDIF(UNIX) From b62848e588b76a9490f278764e14c41de50500ee Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Tue, 16 Jun 2020 12:50:27 +0200 Subject: [PATCH 18/43] [cmake] Fix target_link_libraries to fix the link dependencies and interface. This avoid unnecessary target_include_directories. --- tests/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e0666b881..86340da85 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -115,13 +115,13 @@ FOREACH(path ${tests}) GET_FILENAME_COMPONENT(test ${path} NAME) ADD_UNIT_TEST(${test} ${path}.cpp) - TARGET_LINK_LIBRARIES(${test} ${PROJECT_NAME} ${Boost_LIBRARIES} + TARGET_LINK_LIBRARIES(${test} + PUBLIC + ${PROJECT_NAME} + ${Boost_LIBRARIES} ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS} dynamic-graph::dynamic-graph) - TARGET_INCLUDE_DIRECTORIES(${test} - SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) - IF(UNIX) TARGET_LINK_LIBRARIES(${test} ${CMAKE_DL_LIBS}) ENDIF(UNIX) From 4a53404755a940c8c95e31b7164c4311aac10ae7 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Tue, 16 Jun 2020 14:32:30 +0200 Subject: [PATCH 19/43] [cmake] Remove useless dependency to Boost_LIBRARIES Pointed out by G. Saurel and J. Mirabel. --- tests/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 86340da85..26f0957c4 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -118,7 +118,6 @@ FOREACH(path ${tests}) TARGET_LINK_LIBRARIES(${test} PUBLIC ${PROJECT_NAME} - ${Boost_LIBRARIES} ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS} dynamic-graph::dynamic-graph) From d7ccb2e2cc27f5af9fa57a09783cfe2f3b355594 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Tue, 16 Jun 2020 17:24:36 +0200 Subject: [PATCH 20/43] [cmake] Link is made PUBLIC. --- tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 26f0957c4..d60266155 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -122,7 +122,7 @@ FOREACH(path ${tests}) dynamic-graph::dynamic-graph) IF(UNIX) - TARGET_LINK_LIBRARIES(${test} ${CMAKE_DL_LIBS}) + TARGET_LINK_LIBRARIES(${test} PUBLIC ${CMAKE_DL_LIBS}) ENDIF(UNIX) ENDFOREACH(path ${tests}) From 9dbda566c83f839be3f445341d5cb7c09d3250f6 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Fri, 26 Jun 2020 20:13:27 +0200 Subject: [PATCH 21/43] Put template feature-pose in hxx file. Instance of the template is created in feature-pose. extern tag added in feature-pose.hh Update of test_feature_generic. --- include/sot/core/feature-pose.hh | 9 +- include/sot/core/feature-pose.hxx | 363 ++++++++++++++++++++++++ src/feature/feature-pose.cpp | 296 +------------------ tests/features/test_feature_generic.cpp | 10 +- 4 files changed, 374 insertions(+), 304 deletions(-) create mode 100644 include/sot/core/feature-pose.hxx diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index eeefcaeda..25e194dd7 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -116,7 +116,7 @@ public: public: FeaturePose(const std::string &name); - virtual ~FeaturePose(void) {} + virtual ~FeaturePose(void); virtual unsigned int &getDimension(unsigned int &dim, int time); @@ -161,9 +161,16 @@ private: /// \todo Intermediate variables for internal computations }; +#if __cplusplus >= 201103L +extern template class SOT_CORE_DLLAPI FeaturePose; +extern template class SOT_CORE_DLLAPI FeaturePose; +#endif + } /* namespace sot */ } /* namespace dynamicgraph */ +#include + #endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__ /* diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx new file mode 100644 index 000000000..be7346b75 --- /dev/null +++ b/include/sot/core/feature-pose.hxx @@ -0,0 +1,363 @@ +/* + * Copyright 2019 + * Joseph Mirabel + * + * LAAS-CNRS + * + */ + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +/* --- SOT --- */ +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include + +using namespace std; +using namespace dynamicgraph; +using namespace dynamicgraph::sot; + +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + +namespace dynamicgraph { +namespace sot { +namespace dg = dynamicgraph; + +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; + +typedef FeaturePose FeaturePose_t; +typedef FeaturePose FeaturePoseSE3_t; + + +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); + +template +FeaturePose::FeaturePose(const string &pointName) + : FeatureAbstract(pointName), + oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"), + jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"), + oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"), + jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"), + jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"), + jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"), + faMfbDes(NULL, + CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"), + faNufafbDes(NULL, + CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"), + faMfb( + boost::bind(&FeaturePose::computefaMfb, this, _1, _2), + oMja << jaMfa << oMjb << jbMfb, + CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"), + q_faMfb(boost::bind(&FeaturePose::computeQfaMfb, this, _1, + _2), + faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"), + q_faMfbDes(boost::bind(&FeaturePose::computeQfaMfbDes, + this, _1, _2), + faMfbDes, + CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") { + oMja.setConstant(Id); + jaMfa.setConstant(Id); + jbMfb.setConstant(Id); + faMfbDes.setConstant(Id); + faNufafbDes.setConstant(Vector::Zero(6)); + + jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb); + + errorSOUT.addDependencies(q_faMfbDes << q_faMfb); + + signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb); + signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes); + + errordotSOUT.setFunction( + boost::bind(&FeaturePose::computeErrorDot, this, _1, _2)); + errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes); + + // Commands + // + { + using namespace dynamicgraph::command; + addCommand("keep", + makeCommandVoid1( + *this, &FeaturePose::servoCurrentPosition, + docCommandVoid1( + "modify the desired position to servo at current pos.", + "time"))); + } +} + +template +FeaturePose::~FeaturePose() +{ +} + + + +/* --------------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +template +static inline void check(const FeaturePose &ft) { + (void)ft; + assert(ft.oMja.isPlugged()); + assert(ft.jaMfa.isPlugged()); + assert(ft.oMjb.isPlugged()); + assert(ft.jbMfb.isPlugged()); + assert(ft.faMfbDes.isPlugged()); + assert(ft.faNufafbDes.isPlugged()); +} + +template +unsigned int &FeaturePose::getDimension(unsigned int &dim, + int time) { + sotDEBUG(25) << "# In {" << endl; + + const Flags &fl = selectionSIN.access(time); + + dim = 0; + for (int i = 0; i < 6; ++i) + if (fl(i)) + dim++; + + sotDEBUG(25) << "# Out }" << endl; + return dim; +} + +void toVector(const MatrixHomogeneous &M, Vector7 &v) { + v.head<3>() = M.translation(); + QuaternionMap(v.tail<4>().data()) = M.linear(); +} + +Vector7 toVector(const MatrixHomogeneous &M) { + Vector7 ret; + toVector(M, ret); + return ret; +} + +template +Matrix &FeaturePose::computeJacobian(Matrix &J, int time) { + typedef typename LG_t::type LieGroup_t; + + check(*this); + + q_faMfb.recompute(time); + q_faMfbDes.recompute(time); + + const unsigned int &dim = dimensionSOUT(time); + const Flags &fl = selectionSIN(time); + + const Matrix &_jbJjb = jbJjb(time); + + const MatrixHomogeneous &_jbMfb = + (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id); + + const Matrix::Index cJ = _jbJjb.cols(); + J.resize(dim, cJ); + + MatrixTwist X; + Eigen::Matrix Jminus; + + buildFrom(_jbMfb.inverse(Eigen::Affine), X); + MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() * + oMja.access(time).rotation().transpose() * + oMjb.access(time).rotation() * _jbMfb.rotation(); + if (boost::is_same::value) + X.topRows<3>().applyOnTheLeft(faRfb); + LieGroup_t().template dDifference( + q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); + + // Contribution of b: + // J = Jminus * X * jbJjb; + unsigned int rJ = 0; + for (unsigned int r = 0; r < 6; ++r) + if (fl((int)r)) + J.row(rJ++) = (Jminus * X).row(r) * _jbJjb; + + if (jaJja.isPlugged()) { + const Matrix &_jaJja = jaJja(time); + const MatrixHomogeneous &_jaMfa = + (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id), + _faMfb = faMfb.accessCopy(); + + buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X); + if (boost::is_same::value) + X.topRows<3>().applyOnTheLeft(faRfb); + + // J -= (Jminus * X) * jaJja(time); + rJ = 0; + for (unsigned int r = 0; r < 6; ++r) + if (fl((int)r)) + J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja; + } + + return J; +} + +template +MatrixHomogeneous & +FeaturePose::computefaMfb(MatrixHomogeneous &res, int time) { + check(*this); + + res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) * + jbMfb(time); + return res; +} + +template +Vector7 &FeaturePose::computeQfaMfb(Vector7 &res, int time) { + check(*this); + + toVector(faMfb(time), res); + return res; +} + +template +Vector7 &FeaturePose::computeQfaMfbDes(Vector7 &res, int time) { + check(*this); + + toVector(faMfbDes(time), res); + return res; +} + +template +Vector &FeaturePose::computeError(Vector &error, int time) { + typedef typename LG_t::type LieGroup_t; + check(*this); + + const Flags &fl = selectionSIN(time); + + Eigen::Matrix v; + LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v); + + error.resize(dimensionSOUT(time)); + unsigned int cursor = 0; + for (unsigned int i = 0; i < 6; ++i) + if (fl((int)i)) + error(cursor++) = v(i); + + return error; +} + +// This function is responsible of converting the input velocity expressed with +// SE(3) convention onto a velocity expressed with the convention of this +// feature (R^3xSO(3) or SE(3)), in the right frame. +template +Vector6d convertVelocity(const MatrixHomogeneous &M, + const MatrixHomogeneous &Mdes, + const Vector &faNufafbDes) { + (void)M; + MatrixTwist X; + buildFrom(Mdes.inverse(Eigen::Affine), X); + return X * faNufafbDes; +} +template <> +Vector6d convertVelocity(const MatrixHomogeneous &M, + const MatrixHomogeneous &Mdes, + const Vector &faNufafbDes) { + Vector6d nu; + nu.head<3>() = + faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>()); + nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>(); + return nu; +} + +template +Vector &FeaturePose::computeErrorDot(Vector &errordot, + int time) { + typedef typename LG_t::type LieGroup_t; + check(*this); + + errordot.resize(dimensionSOUT(time)); + const Flags &fl = selectionSIN(time); + if (!faNufafbDes.isPlugged()) { + errordot.setZero(); + return errordot; + } + + q_faMfb.recompute(time); + q_faMfbDes.recompute(time); + faNufafbDes.recompute(time); + + const MatrixHomogeneous &_faMfbDes = faMfbDes(time); + + Eigen::Matrix Jminus; + + LieGroup_t().template dDifference( + q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); + Vector6d nu = convertVelocity(faMfb(time), _faMfbDes, + faNufafbDes.accessCopy()); + unsigned int cursor = 0; + for (unsigned int i = 0; i < 6; ++i) + if (fl((int)i)) + errordot(cursor++) = Jminus.row(i) * nu; + + return errordot; +} + +/* Modify the value of the reference (sdes) so that it corresponds + * to the current position. The effect on the servo is to maintain the + * current position and correct any drift. */ +template +void FeaturePose::servoCurrentPosition(const int &time) { + check(*this); + + const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id), + _jaMfa = + (jaMfa.isPlugged() ? jaMfa.access(time) : Id), + _oMjb = oMjb.access(time), + _jbMfb = + (jbMfb.isPlugged() ? jbMfb.access(time) : Id); + faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb; +} + +static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"}; +template +void FeaturePose::display(std::ostream &os) const { + os << CLASS_NAME << "<" << name << ">: ("; + + try { + const Flags &fl = selectionSIN.accessCopy(); + bool first = true; + for (int i = 0; i < 6; ++i) + if (fl(i)) { + if (first) { + first = false; + } else { + os << ","; + } + os << featureNames[i]; + } + os << ") "; + } catch (ExceptionAbstract e) { + os << " selectSIN not set."; + } +} + + +} +} diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index b9e640546..71a72e51d 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -38,11 +38,13 @@ typedef pinocchio::CartesianProductOperation< pinocchio::SpecialOrthogonalOperationTpl<3, double> > R3xSO3_t; typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + template struct LG_t { typedef typename boost::mpl::if_c::type type; }; + typedef FeaturePose FeaturePose_t; typedef FeaturePose FeaturePoseSE3_t; template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); @@ -55,297 +57,3 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); -template -FeaturePose::FeaturePose(const string &pointName) - : FeatureAbstract(pointName), - oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"), - jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"), - oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"), - jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"), - jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"), - jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"), - faMfbDes(NULL, - CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"), - faNufafbDes(NULL, - CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"), - faMfb( - boost::bind(&FeaturePose::computefaMfb, this, _1, _2), - oMja << jaMfa << oMjb << jbMfb, - CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"), - q_faMfb(boost::bind(&FeaturePose::computeQfaMfb, this, _1, - _2), - faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"), - q_faMfbDes(boost::bind(&FeaturePose::computeQfaMfbDes, - this, _1, _2), - faMfbDes, - CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") { - oMja.setConstant(Id); - jaMfa.setConstant(Id); - jbMfb.setConstant(Id); - faMfbDes.setConstant(Id); - faNufafbDes.setConstant(Vector::Zero(6)); - - jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb); - - errorSOUT.addDependencies(q_faMfbDes << q_faMfb); - - signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb); - signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes); - - errordotSOUT.setFunction( - boost::bind(&FeaturePose::computeErrorDot, this, _1, _2)); - errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes); - - // Commands - // - { - using namespace dynamicgraph::command; - addCommand("keep", - makeCommandVoid1( - *this, &FeaturePose::servoCurrentPosition, - docCommandVoid1( - "modify the desired position to servo at current pos.", - "time"))); - } -} - -/* --------------------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -template -static inline void check(const FeaturePose &ft) { - (void)ft; - assert(ft.oMja.isPlugged()); - assert(ft.jaMfa.isPlugged()); - assert(ft.oMjb.isPlugged()); - assert(ft.jbMfb.isPlugged()); - assert(ft.faMfbDes.isPlugged()); - assert(ft.faNufafbDes.isPlugged()); -} - -template -unsigned int &FeaturePose::getDimension(unsigned int &dim, - int time) { - sotDEBUG(25) << "# In {" << endl; - - const Flags &fl = selectionSIN.access(time); - - dim = 0; - for (int i = 0; i < 6; ++i) - if (fl(i)) - dim++; - - sotDEBUG(25) << "# Out }" << endl; - return dim; -} - -void toVector(const MatrixHomogeneous &M, Vector7 &v) { - v.head<3>() = M.translation(); - QuaternionMap(v.tail<4>().data()) = M.linear(); -} - -Vector7 toVector(const MatrixHomogeneous &M) { - Vector7 ret; - toVector(M, ret); - return ret; -} - -template -Matrix &FeaturePose::computeJacobian(Matrix &J, int time) { - typedef typename LG_t::type LieGroup_t; - - check(*this); - - q_faMfb.recompute(time); - q_faMfbDes.recompute(time); - - const int &dim = dimensionSOUT(time); - const Flags &fl = selectionSIN(time); - - const Matrix &_jbJjb = jbJjb(time); - - const MatrixHomogeneous &_jbMfb = - (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id); - - const Matrix::Index cJ = _jbJjb.cols(); - J.resize(dim, cJ); - - MatrixTwist X; - Eigen::Matrix Jminus; - - buildFrom(_jbMfb.inverse(Eigen::Affine), X); - MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() * - oMja.access(time).rotation().transpose() * - oMjb.access(time).rotation() * _jbMfb.rotation(); - if (boost::is_same::value) - X.topRows<3>().applyOnTheLeft(faRfb); - LieGroup_t().template dDifference( - q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); - - // Contribution of b: - // J = Jminus * X * jbJjb; - unsigned int rJ = 0; - for (unsigned int r = 0; r < 6; ++r) - if (fl(r)) - J.row(rJ++) = (Jminus * X).row(r) * _jbJjb; - - if (jaJja.isPlugged()) { - const Matrix &_jaJja = jaJja(time); - const MatrixHomogeneous &_jaMfa = - (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id), - _faMfb = faMfb.accessCopy(); - - buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X); - if (boost::is_same::value) - X.topRows<3>().applyOnTheLeft(faRfb); - - // J -= (Jminus * X) * jaJja(time); - rJ = 0; - for (unsigned int r = 0; r < 6; ++r) - if (fl(r)) - J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja; - } - - return J; -} - -template -MatrixHomogeneous & -FeaturePose::computefaMfb(MatrixHomogeneous &res, int time) { - check(*this); - - res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) * - jbMfb(time); - return res; -} - -template -Vector7 &FeaturePose::computeQfaMfb(Vector7 &res, int time) { - check(*this); - - toVector(faMfb(time), res); - return res; -} - -template -Vector7 &FeaturePose::computeQfaMfbDes(Vector7 &res, int time) { - check(*this); - - toVector(faMfbDes(time), res); - return res; -} - -template -Vector &FeaturePose::computeError(Vector &error, int time) { - typedef typename LG_t::type LieGroup_t; - check(*this); - - const Flags &fl = selectionSIN(time); - - Eigen::Matrix v; - LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v); - - error.resize(dimensionSOUT(time)); - unsigned int cursor = 0; - for (unsigned int i = 0; i < 6; ++i) - if (fl(i)) - error(cursor++) = v(i); - - return error; -} - -// This function is responsible of converting the input velocity expressed with -// SE(3) convention onto a velocity expressed with the convention of this -// feature (R^3xSO(3) or SE(3)), in the right frame. -template -Vector6d convertVelocity(const MatrixHomogeneous &M, - const MatrixHomogeneous &Mdes, - const Vector &faNufafbDes) { - (void)M; - MatrixTwist X; - buildFrom(Mdes.inverse(Eigen::Affine), X); - return X * faNufafbDes; -} -template <> -Vector6d convertVelocity(const MatrixHomogeneous &M, - const MatrixHomogeneous &Mdes, - const Vector &faNufafbDes) { - Vector6d nu; - nu.head<3>() = - faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>()); - nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>(); - return nu; -} - -template -Vector &FeaturePose::computeErrorDot(Vector &errordot, - int time) { - typedef typename LG_t::type LieGroup_t; - check(*this); - - errordot.resize(dimensionSOUT(time)); - const Flags &fl = selectionSIN(time); - if (!faNufafbDes.isPlugged()) { - errordot.setZero(); - return errordot; - } - - q_faMfb.recompute(time); - q_faMfbDes.recompute(time); - faNufafbDes.recompute(time); - - const MatrixHomogeneous &_faMfbDes = faMfbDes(time); - - Eigen::Matrix Jminus; - - LieGroup_t().template dDifference( - q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); - Vector6d nu = convertVelocity(faMfb(time), _faMfbDes, - faNufafbDes.accessCopy()); - unsigned int cursor = 0; - for (unsigned int i = 0; i < 6; ++i) - if (fl(i)) - errordot(cursor++) = Jminus.row(i) * nu; - - return errordot; -} - -/* Modify the value of the reference (sdes) so that it corresponds - * to the current position. The effect on the servo is to maintain the - * current position and correct any drift. */ -template -void FeaturePose::servoCurrentPosition(const int &time) { - check(*this); - - const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id), - _jaMfa = - (jaMfa.isPlugged() ? jaMfa.access(time) : Id), - _oMjb = oMjb.access(time), - _jbMfb = - (jbMfb.isPlugged() ? jbMfb.access(time) : Id); - faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb; -} - -static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"}; -template -void FeaturePose::display(std::ostream &os) const { - os << CLASS_NAME << "<" << name << ">: ("; - - try { - const Flags &fl = selectionSIN.accessCopy(); - bool first = true; - for (int i = 0; i < 6; ++i) - if (fl(i)) { - if (first) { - first = false; - } else { - os << ","; - } - os << featureNames[i]; - } - os << ") "; - } catch (ExceptionAbstract e) { - os << " selectSIN not set."; - } -} diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index ca36505cc..435f4514c 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -270,15 +270,6 @@ MatrixHomogeneous randomM() { } typedef pinocchio::SE3 SE3; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; Vector7 toVector(const pinocchio::SE3 &M) { Vector7 v; @@ -334,6 +325,7 @@ class TestFeaturePose : public FeatureTestBase { setJointFrame(); } + void _setFrame() { setSignal( feature_.jaMfa, From 9d9004a468d5dce2e0db9c2e9a50aacbd5ea5497 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 07:20:45 +0200 Subject: [PATCH 22/43] [feature-pose] Reorganize such that feature-pose.hxx is not in feature-pose.hh --- include/sot/core/feature-pose.hh | 26 ++++++++++++++++++++++++- include/sot/core/feature-pose.hxx | 19 ------------------ src/feature/feature-pose.cpp | 1 + tests/features/test_feature_generic.cpp | 3 +++ 4 files changed, 29 insertions(+), 20 deletions(-) diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index 25e194dd7..b842de518 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -169,7 +169,31 @@ extern template class SOT_CORE_DLLAPI FeaturePose; } /* namespace sot */ } /* namespace dynamicgraph */ -#include + +using namespace std; +using namespace dynamicgraph; +using namespace dynamicgraph::sot; + +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + +namespace dynamicgraph { +namespace sot { +namespace dg = dynamicgraph; + +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; + +typedef FeaturePose FeaturePose_t; +typedef FeaturePose FeaturePoseSE3_t; +} +} + #endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__ diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx index be7346b75..15292e895 100644 --- a/include/sot/core/feature-pose.hxx +++ b/include/sot/core/feature-pose.hxx @@ -27,29 +27,10 @@ #include #include -using namespace std; -using namespace dynamicgraph; -using namespace dynamicgraph::sot; - -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - namespace dynamicgraph { namespace sot { namespace dg = dynamicgraph; -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; - -typedef FeaturePose FeaturePose_t; -typedef FeaturePose FeaturePoseSE3_t; - - /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index 71a72e51d..62a56a737 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; using namespace dynamicgraph; diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index 435f4514c..867a05317 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -27,10 +27,12 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -38,6 +40,7 @@ using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; +namespace dg = dynamicgraph; #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ From 362af0335a2a7457ef7be99722586de2288ee09d Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 2 Jul 2020 08:07:55 +0200 Subject: [PATCH 23/43] Fix usage of explicit instantiation --- include/sot/core/feature-pose.hh | 33 +++++-------------------------- include/sot/core/feature-pose.hxx | 22 ++++++++++++++++----- src/feature/feature-pose.cpp | 23 ++------------------- 3 files changed, 24 insertions(+), 54 deletions(-) diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index b842de518..ed506c885 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -6,8 +6,8 @@ * */ -#ifndef __SOT_FEATURE_TRANSFORMATION_HH__ -#define __SOT_FEATURE_TRANSFORMATION_HH__ +#ifndef __SOT_FEATURE_POSE_HH__ +#define __SOT_FEATURE_POSE_HH__ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ @@ -166,36 +166,13 @@ extern template class SOT_CORE_DLLAPI FeaturePose; extern template class SOT_CORE_DLLAPI FeaturePose; #endif -} /* namespace sot */ -} /* namespace dynamicgraph */ - - -using namespace std; -using namespace dynamicgraph; -using namespace dynamicgraph::sot; - -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - -namespace dynamicgraph { -namespace sot { -namespace dg = dynamicgraph; - -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; - typedef FeaturePose FeaturePose_t; typedef FeaturePose FeaturePoseSE3_t; -} -} +} /* namespace sot */ +} /* namespace dynamicgraph */ -#endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__ +#endif // #ifndef __SOT_FEATURE_POSE_HH__ /* * Local variables: diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx index 15292e895..e8779ee5e 100644 --- a/include/sot/core/feature-pose.hxx +++ b/include/sot/core/feature-pose.hxx @@ -29,7 +29,19 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; + +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + +namespace internal { +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; +} /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ @@ -142,7 +154,7 @@ Vector7 toVector(const MatrixHomogeneous &M) { template Matrix &FeaturePose::computeJacobian(Matrix &J, int time) { - typedef typename LG_t::type LieGroup_t; + typedef typename internal::LG_t::type LieGroup_t; check(*this); @@ -227,7 +239,7 @@ Vector7 &FeaturePose::computeQfaMfbDes(Vector7 &res, int time) { template Vector &FeaturePose::computeError(Vector &error, int time) { - typedef typename LG_t::type LieGroup_t; + typedef typename internal::LG_t::type LieGroup_t; check(*this); const Flags &fl = selectionSIN(time); @@ -247,7 +259,7 @@ Vector &FeaturePose::computeError(Vector &error, int time) { // This function is responsible of converting the input velocity expressed with // SE(3) convention onto a velocity expressed with the convention of this // feature (R^3xSO(3) or SE(3)), in the right frame. -template +template Vector6d convertVelocity(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes) { @@ -270,7 +282,7 @@ Vector6d convertVelocity(const MatrixHomogeneous &M, template Vector &FeaturePose::computeErrorDot(Vector &errordot, int time) { - typedef typename LG_t::type LieGroup_t; + typedef typename internal::LG_t::type LieGroup_t; check(*this); errordot.resize(dimensionSOUT(time)); diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index 62a56a737..7d8906803 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -25,36 +25,17 @@ #include -#include #include #include #include -using namespace std; -using namespace dynamicgraph; using namespace dynamicgraph::sot; -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; - - typedef FeaturePose FeaturePose_t; typedef FeaturePose FeaturePoseSE3_t; template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); -/* --------------------------------------------------------------------- */ -/* --- CLASS ----------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); - +template class FeaturePose; +template class FeaturePose; From fbbf38c4d5d27176101ad36e055b38d5abf94904 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 13:28:47 +0200 Subject: [PATCH 24/43] [feature-pose] Make it work on clang. --- include/sot/core/feature-pose.hh | 4 ++++ include/sot/core/feature-pose.hxx | 10 +++++----- tests/features/test_feature_generic.cpp | 21 +++++++++++++++++++-- 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index ed506c885..6ca40ac71 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -161,6 +161,10 @@ private: /// \todo Intermediate variables for internal computations }; +template < typename T > +Vector6d convertVelocity(const MatrixHomogeneous &M, + const MatrixHomogeneous &Mdes, + const Vector &faNufafbDes); #if __cplusplus >= 201103L extern template class SOT_CORE_DLLAPI FeaturePose; extern template class SOT_CORE_DLLAPI FeaturePose; diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx index e8779ee5e..500dd4ecd 100644 --- a/include/sot/core/feature-pose.hxx +++ b/include/sot/core/feature-pose.hxx @@ -50,7 +50,7 @@ template struct LG_t { static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); template -FeaturePose::FeaturePose(const string &pointName) +FeaturePose::FeaturePose(const std::string &pointName) : FeatureAbstract(pointName), oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"), jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"), @@ -128,7 +128,7 @@ static inline void check(const FeaturePose &ft) { template unsigned int &FeaturePose::getDimension(unsigned int &dim, int time) { - sotDEBUG(25) << "# In {" << endl; + sotDEBUG(25) << "# In {" << std::endl; const Flags &fl = selectionSIN.access(time); @@ -137,7 +137,7 @@ unsigned int &FeaturePose::getDimension(unsigned int &dim, if (fl(i)) dim++; - sotDEBUG(25) << "# Out }" << endl; + sotDEBUG(25) << "# Out }" << std::endl; return dim; } @@ -259,8 +259,8 @@ Vector &FeaturePose::computeError(Vector &error, int time) { // This function is responsible of converting the input velocity expressed with // SE(3) convention onto a velocity expressed with the convention of this // feature (R^3xSO(3) or SE(3)), in the right frame. -template -Vector6d convertVelocity(const MatrixHomogeneous &M, +template <> +Vector6d convertVelocity(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes) { (void)M; diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index 867a05317..c082b7e7b 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -32,11 +32,28 @@ #include #include #include -#include #include #include #include +namespace dynamicgraph { +namespace sot { + +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + +namespace internal { +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; +} +} +} + using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; @@ -291,7 +308,7 @@ Vector toVector(const std::vector &in) { template class TestFeaturePose : public FeatureTestBase { public: - typedef typename LG_t::type LieGroup_t; + typedef typename dg::sot::internal::LG_t::type LieGroup_t; FeaturePose feature_; bool relative_; pinocchio::Model model_; From b052916a66653c9959ef6ce649ff2c5837b87b1b Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 16:05:42 +0200 Subject: [PATCH 25/43] [feature-pose] Fix the namespace problem. --- src/feature/feature-pose.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index 7d8906803..f328281c4 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -37,5 +37,9 @@ template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); +namespace dynamicgraph { +namespace sot { template class FeaturePose; template class FeaturePose; +} +} From f7cbede9bf4f9ff1bbb64673aa717c034449cf67 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Thu, 2 Jul 2020 18:45:36 +0200 Subject: [PATCH 26/43] [tests] Fix namespace problem for test_feature_generic --- tests/features/test_feature_generic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index c082b7e7b..18ad6645b 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -38,6 +38,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; typedef pinocchio::CartesianProductOperation< pinocchio::VectorSpaceOperationTpl<3, double>, @@ -57,7 +58,6 @@ template struct LG_t { using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; -namespace dg = dynamicgraph; #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ From 468cac4453560c0145094c4a6e161e135b318159 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Mon, 6 Jul 2020 07:19:15 +0200 Subject: [PATCH 27/43] Remove definition of dg across various class. Make it internal to the implementation. --- include/sot/core/feature-abstract.hh | 17 ++++++++--------- include/sot/core/feature-generic.hh | 13 ++++++------- include/sot/core/feature-posture.hh | 10 +++++----- 3 files changed, 19 insertions(+), 21 deletions(-) diff --git a/include/sot/core/feature-abstract.hh b/include/sot/core/feature-abstract.hh index 5ee14cfbc..282ae07fb 100644 --- a/include/sot/core/feature-abstract.hh +++ b/include/sot/core/feature-abstract.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include "sot/core/api.hh" @@ -137,20 +136,20 @@ public: \par[in] time: The time at which the error is computed. \return The vector res with the appropriate value. */ - virtual dg::Vector &computeError(dg::Vector &res, int time) = 0; + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time) = 0; /*! \brief Compute the Jacobian of the error according the robot state. \par[out] res: The matrix in which the error will be written. \return The matrix res with the appropriate values. */ - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time) = 0; + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time) = 0; /// Callback for signal errordotSOUT /// /// Copy components of the input signal errordotSIN defined by selection /// flag selectionSIN. - virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); + virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); /*! @} */ @@ -170,7 +169,7 @@ public: SignalPtr selectionSIN; /// Derivative of the reference value. - SignalPtr errordotSIN; + SignalPtr errordotSIN; /*! @} */ @@ -179,15 +178,15 @@ public: /*! \brief This signal returns the error between the desired value and the current value : \f$ E(t) = {\bf s}(t) - {\bf s}^*(t)\f$ */ - SignalTimeDependent errorSOUT; + SignalTimeDependent errorSOUT; /*! \brief Derivative of the error with respect to time: * \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt} \f$ */ - SignalTimeDependent errordotSOUT; + SignalTimeDependent errordotSOUT; /*! \brief Jacobian of the error wrt the robot state: * \f$ J = \frac{\partial {\bf s}}{\partial {\bf q}}\f$ */ - SignalTimeDependent jacobianSOUT; + SignalTimeDependent jacobianSOUT; /*! \brief Returns the dimension of the feature as an output signal. */ SignalTimeDependent dimensionSOUT; @@ -196,7 +195,7 @@ public: FileName. */ virtual std::ostream &writeGraph(std::ostream &os) const; - virtual SignalTimeDependent &getErrorDot() { + virtual SignalTimeDependent &getErrorDot() { return errordotSOUT; } diff --git a/include/sot/core/feature-generic.hh b/include/sot/core/feature-generic.hh index 21bfe832f..2d0e9e189 100644 --- a/include/sot/core/feature-generic.hh +++ b/include/sot/core/feature-generic.hh @@ -38,7 +38,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeatureGeneric @@ -65,21 +64,21 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - dg::Vector::Index dimensionDefault; + dynamicgraph::Vector::Index dimensionDefault; /* --- SIGNALS ------------------------------------------------------------ */ public: - /*! \name dg::Signals + /*! \name dynamicgraph::Signals @{ */ /*! \name Input signals @{ */ /*! \brief Input for the error. */ - dg::SignalPtr errorSIN; + dynamicgraph::SignalPtr errorSIN; /*! \brief Input for the Jacobian. */ - dg::SignalPtr jacobianSIN; + dynamicgraph::SignalPtr jacobianSIN; /*! @} */ @@ -110,10 +109,10 @@ public: /*! \brief Compute the error between the desired value and the value itself. */ - virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); /*! \brief Compute the Jacobian of the value according to the robot state.. */ - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); /*! @} */ diff --git a/include/sot/core/feature-posture.hh b/include/sot/core/feature-posture.hh index 3ff6b6287..dede94104 100644 --- a/include/sot/core/feature-posture.hh +++ b/include/sot/core/feature-posture.hh @@ -52,8 +52,8 @@ class SOTFEATUREPOSTURE_EXPORT FeaturePosture : public FeatureAbstract { DYNAMIC_GRAPH_ENTITY_DECL(); public: - typedef dynamicgraph::SignalPtr signalIn_t; - typedef dynamicgraph::SignalTimeDependent signalOut_t; + typedef dynamicgraph::SignalPtr signalIn_t; + typedef dynamicgraph::SignalTimeDependent signalOut_t; DECLARE_NO_REFERENCE; @@ -63,9 +63,9 @@ public: void selectDof(unsigned dofId, bool control); protected: - virtual dg::Vector &computeError(dg::Vector &res, int); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int); - virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int); + virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); signalIn_t state_; signalIn_t posture_; From d59eb72c20205034e95b6634cd2bf9598bb5c27c Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Mon, 6 Jul 2020 10:12:31 +0200 Subject: [PATCH 28/43] Fix dg namespace pb. --- cmake | 2 +- include/sot/core/gain-adaptive.hh | 8 +++----- include/sot/core/robot-utils.hh | 8 +++----- include/sot/core/seq-play.hh | 10 ++++----- include/sot/core/seqplay.hh | 10 ++++----- include/sot/core/sequencer.hh | 1 - include/sot/core/smooth-reach.hh | 17 +++++++-------- include/sot/core/sot.hh | 9 ++++---- include/sot/core/task-abstract.hh | 7 +++---- include/sot/core/task-conti.hh | 6 ++---- include/sot/core/task-pd.hh | 9 ++++---- include/sot/core/task-unilateral.hh | 10 ++++----- include/sot/core/task.hh | 18 ++++++++-------- include/sot/core/time-stamp.hh | 16 +++++++-------- include/sot/core/timer.hh | 13 ++++++------ include/sot/core/trajectory.hh | 4 +--- include/sot/core/vector-constant.hh | 5 ++--- include/sot/core/vector-to-rotation.hh | 10 ++++----- include/sot/core/visual-point-projecter.hh | 8 ++++---- src/tools/parameter-server.cpp | 2 +- src/tools/sequencer.cpp | 2 +- tests/tools/test_robot_utils.cpp | 24 +++++++++++----------- 22 files changed, 85 insertions(+), 114 deletions(-) diff --git a/cmake b/cmake index 9e21ae222..c333a88de 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 9e21ae2222fdb51dccd1320bb7208f73259b0c73 +Subproject commit c333a88decb3e4c0a86947bc6c7f072dc5c5df20 diff --git a/include/sot/core/gain-adaptive.hh b/include/sot/core/gain-adaptive.hh index e778242d8..9cd40538c 100644 --- a/include/sot/core/gain-adaptive.hh +++ b/include/sot/core/gain-adaptive.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -42,7 +41,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /** Exponentially decreasing gain. * It follows the law \f[ g(e) = a \exp (-b ||e||) + c \f]. @@ -52,7 +50,7 @@ namespace dg = dynamicgraph; * - \f$ b = 0 \f$, * - \f$ c = 0.1 \f$. */ -class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dg::Entity { +class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dynamicgraph::Entity { public: /* --- CONSTANTS --- */ /* Default values. */ @@ -116,8 +114,8 @@ public: /* --- INIT --- */ void forceConstant(void); public: /* --- SIGNALS --- */ - dg::SignalPtr errorSIN; - dg::SignalTimeDependent gainSOUT; + dynamicgraph::SignalPtr errorSIN; + dynamicgraph::SignalTimeDependent gainSOUT; protected: double &computeGain(double &res, int t); diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh index 0cf7fcf28..bcb11f79e 100644 --- a/include/sot/core/robot-utils.hh +++ b/include/sot/core/robot-utils.hh @@ -18,8 +18,6 @@ #include #include -namespace dg = ::dynamicgraph; -using namespace dg; namespace dynamicgraph { namespace sot { @@ -57,8 +55,8 @@ struct SOT_CORE_EXPORT ForceUtil { void set_name_to_force_id(const std::string &name, const Index &force_id); - void set_force_id_to_limits(const Index &force_id, const dg::Vector &lf, - const dg::Vector &uf); + void set_force_id_to_limits(const Index &force_id, const dynamicgraph::Vector &lf, + const dynamicgraph::Vector &uf); void create_force_id_to_name_map(); @@ -169,7 +167,7 @@ public: /// Set the map between urdf index and sot index void set_urdf_to_sot(const std::vector &urdf_to_sot); - void set_urdf_to_sot(const dg::Vector &urdf_to_sot); + void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot); /// Set the limits (lq,uq) for joint idx void set_joint_limits_for_id(const Index &idx, const double &lq, diff --git a/include/sot/core/seq-play.hh b/include/sot/core/seq-play.hh index dd791ac73..6be8ca4e3 100644 --- a/include/sot/core/seq-play.hh +++ b/include/sot/core/seq-play.hh @@ -16,7 +16,7 @@ /* -- MaaL --- */ #include -namespace dg = dynamicgraph; + /* SOT */ #include #include @@ -50,7 +50,7 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - typedef std::list StateList; + typedef std::list StateList; StateList stateList; StateList::iterator currPos; unsigned int currRank; @@ -64,7 +64,7 @@ public: void loadFile(const std::string &name); - dg::Vector &getNextPosition(dg::Vector &pos, const int &time); + dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos, const int &time); public: /* --- DISPLAY --- */ virtual void display(std::ostream &os) const; @@ -75,10 +75,8 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ - // dynamicgraph::SignalPtr positionSIN; - // dynamicgraph::SignalTimeDependant velocitySOUT; dynamicgraph::SignalTimeDependent refresherSINTERN; - dynamicgraph::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/seqplay.hh b/include/sot/core/seqplay.hh index dd791ac73..6be8ca4e3 100644 --- a/include/sot/core/seqplay.hh +++ b/include/sot/core/seqplay.hh @@ -16,7 +16,7 @@ /* -- MaaL --- */ #include -namespace dg = dynamicgraph; + /* SOT */ #include #include @@ -50,7 +50,7 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - typedef std::list StateList; + typedef std::list StateList; StateList stateList; StateList::iterator currPos; unsigned int currRank; @@ -64,7 +64,7 @@ public: void loadFile(const std::string &name); - dg::Vector &getNextPosition(dg::Vector &pos, const int &time); + dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos, const int &time); public: /* --- DISPLAY --- */ virtual void display(std::ostream &os) const; @@ -75,10 +75,8 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ - // dynamicgraph::SignalPtr positionSIN; - // dynamicgraph::SignalTimeDependant velocitySOUT; dynamicgraph::SignalTimeDependent refresherSINTERN; - dynamicgraph::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/sequencer.hh b/include/sot/core/sequencer.hh index 0c0584b88..0a57cc3cd 100644 --- a/include/sot/core/sequencer.hh +++ b/include/sot/core/sequencer.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include diff --git a/include/sot/core/smooth-reach.hh b/include/sot/core/smooth-reach.hh index 1240e5b9d..5848bb190 100644 --- a/include/sot/core/smooth-reach.hh +++ b/include/sot/core/smooth-reach.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -39,19 +38,18 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTSMOOTHREACH_EXPORT SmoothReach : public dg::Entity { +class SOTSMOOTHREACH_EXPORT SmoothReach : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName() const { return CLASS_NAME; } private: - dg::Vector start, goal; + dynamicgraph::Vector start, goal; int startTime, lengthTime; bool isStarted, isParam; int smoothMode; @@ -64,15 +62,14 @@ public: /* --- CONSTRUCTION --- */ virtual ~SmoothReach(void){}; public: /* --- SIGNAL --- */ - dg::SignalPtr startSIN; - dg::SignalTimeDependent goalSOUT; - // dg::SignalTimeDependent percentSOUT; + dynamicgraph::SignalPtr startSIN; + dynamicgraph::SignalTimeDependent goalSOUT; public: /* --- FUNCTION --- */ - dg::Vector &goalSOUT_function(dg::Vector &goal, const int &time); + dynamicgraph::Vector &goalSOUT_function(dynamicgraph::Vector &goal, const int &time); - void set(const dg::Vector &goal, const int &length); - const dg::Vector &getGoal(void); + void set(const dynamicgraph::Vector &goal, const int &length); + const dynamicgraph::Vector &getGoal(void); const int &getLength(void); const int &getStart(void); diff --git a/include/sot/core/sot.hh b/include/sot/core/sot.hh index 54ab52eef..067573571 100644 --- a/include/sot/core/sot.hh +++ b/include/sot/core/sot.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* Classes standards. */ #include /* Classe std::list */ @@ -159,7 +158,7 @@ public: /* --- CONTROL --- */ */ /*! \brief Compute the control law. */ - virtual dg::Vector &computeControlLaw(dg::Vector &control, const int &time); + virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control, const int &time); /*! @} */ @@ -181,18 +180,18 @@ public: /* --- SIGNALS --- */ * the recurence of the SOT (e.g. velocity coming from the other * OpenHRP plugins). */ - SignalPtr q0SIN; + SignalPtr q0SIN; /*! \brief A matrix K whose columns are a base of the desired velocity. * In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free * parameter to be computed. * \note K should be an orthonormal matrix. */ - SignalPtr proj0SIN; + SignalPtr proj0SIN; /*! \brief This signal allow to change the threshold for the damped pseudo-inverse on-line */ SignalPtr inversionThresholdSIN; /*! \brief Allow to get the result of the computed control law. */ - SignalTimeDependent controlSOUT; + SignalTimeDependent controlSOUT; /*! @} */ /*! \brief This method write the priority between tasks in the output stream diff --git a/include/sot/core/task-abstract.hh b/include/sot/core/task-abstract.hh index cb9fe4dd5..248f1ec5c 100644 --- a/include/sot/core/task-abstract.hh +++ b/include/sot/core/task-abstract.hh @@ -17,7 +17,6 @@ /* Matrix */ #include #include -namespace dg = dynamicgraph; /* STD */ #include @@ -46,7 +45,7 @@ namespace sot { /// TaskAbstract. The value and Jacobian of a Task are computed from the /// features that are stored in the task. -class SOT_CORE_EXPORT TaskAbstract : public dg::Entity { +class SOT_CORE_EXPORT TaskAbstract : public dynamicgraph::Entity { public: /* Use a derivative of this class to store computational memory. */ class MemoryTaskAbstract { @@ -76,8 +75,8 @@ public: TaskAbstract(const std::string &n); public: /* --- SIGNALS --- */ - dg::SignalTimeDependent taskSOUT; - dg::SignalTimeDependent jacobianSOUT; + dynamicgraph::SignalTimeDependent taskSOUT; + dynamicgraph::SignalTimeDependent jacobianSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/task-conti.hh b/include/sot/core/task-conti.hh index 01b71002a..4098ea325 100644 --- a/include/sot/core/task-conti.hh +++ b/include/sot/core/task-conti.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* STD */ #include @@ -48,7 +47,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; class SOTTASKCONTI_EXPORT TaskConti : public Task { protected: @@ -56,7 +54,7 @@ protected: int timeRef; double mu; - dg::Vector q0; + dynamicgraph::Vector q0; public: static const std::string CLASS_NAME; @@ -74,7 +72,7 @@ public: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr controlPrevSIN; + dynamicgraph::SignalPtr controlPrevSIN; /* --- DISPLAY ------------------------------------------------------------ */ void display(std::ostream &os) const; diff --git a/include/sot/core/task-pd.hh b/include/sot/core/task-pd.hh index 2778afb40..8c674924c 100644 --- a/include/sot/core/task-pd.hh +++ b/include/sot/core/task-pd.hh @@ -37,27 +37,26 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; class SOTTASKPD_EXPORT TaskPD : public Task { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } - dg::Vector previousError; + dynamicgraph::Vector previousError; double beta; public: TaskPD(const std::string &n); /* --- COMPUTATION --- */ - dg::Vector &computeErrorDot(dg::Vector &error, int time); + dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &error, int time); VectorMultiBound &computeTaskModif(VectorMultiBound &error, int time); /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalTimeDependent errorDotSOUT; - dg::SignalPtr errorDotSIN; + dynamicgraph::SignalTimeDependent errorDotSOUT; + dynamicgraph::SignalPtr errorDotSIN; /* --- PARAMS --- */ void initCommand(void); diff --git a/include/sot/core/task-unilateral.hh b/include/sot/core/task-unilateral.hh index 071669d6b..3a704a524 100644 --- a/include/sot/core/task-unilateral.hh +++ b/include/sot/core/task-unilateral.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* STD */ #include @@ -48,7 +47,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; class SOTTASKUNILATERAL_EXPORT TaskUnilateral : public Task { protected: @@ -66,10 +64,10 @@ public: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr positionSIN; - dg::SignalPtr referenceInfSIN; - dg::SignalPtr referenceSupSIN; - dg::SignalPtr dtSIN; + dynamicgraph::SignalPtr positionSIN; + dynamicgraph::SignalPtr referenceInfSIN; + dynamicgraph::SignalPtr referenceSupSIN; + dynamicgraph::SignalPtr dtSIN; /* --- DISPLAY ------------------------------------------------------------ */ void display(std::ostream &os) const; diff --git a/include/sot/core/task.hh b/include/sot/core/task.hh index 24e6e1a78..7417a0342 100644 --- a/include/sot/core/task.hh +++ b/include/sot/core/task.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* STD */ #include @@ -70,7 +69,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; class SOTTASK_EXPORT Task : public TaskAbstract { public: @@ -99,19 +97,19 @@ public: bool getWithDerivative(void); /* --- COMPUTATION --- */ - dg::Vector &computeError(dg::Vector &error, int time); + dynamicgraph::Vector &computeError(dynamicgraph::Vector &error, int time); VectorMultiBound &computeTaskExponentialDecrease(VectorMultiBound &errorRef, int time); - dg::Matrix &computeJacobian(dg::Matrix &J, int time); - dg::Vector &computeErrorTimeDerivative(dg::Vector &res, int time); + dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &J, int time); + dynamicgraph::Vector &computeErrorTimeDerivative(dynamicgraph::Vector &res, int time); /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr controlGainSIN; - dg::SignalPtr dampingGainSINOUT; - dg::SignalPtr controlSelectionSIN; - dg::SignalTimeDependent errorSOUT; - dg::SignalTimeDependent errorTimeDerivativeSOUT; + dynamicgraph::SignalPtr controlGainSIN; + dynamicgraph::SignalPtr dampingGainSINOUT; + dynamicgraph::SignalPtr controlSelectionSIN; + dynamicgraph::SignalTimeDependent errorSOUT; + dynamicgraph::SignalTimeDependent errorTimeDerivativeSOUT; /* --- DISPLAY ------------------------------------------------------------ */ void display(std::ostream &os) const; diff --git a/include/sot/core/time-stamp.hh b/include/sot/core/time-stamp.hh index 6c1a9d03b..93b3d9918 100644 --- a/include/sot/core/time-stamp.hh +++ b/include/sot/core/time-stamp.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* Classes standards. */ #ifndef WIN32 @@ -50,9 +49,8 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; -class TimeStamp_EXPORT TimeStamp : public dg::Entity { +class TimeStamp_EXPORT TimeStamp : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -72,17 +70,17 @@ public: /* --- DISPLAY --- */ public: /* --- SIGNALS --- */ /* These signals can be called several time per period, given * each time a different results. Useful for chronos. */ - dg::Signal timeSOUT; - dg::Signal timeDoubleSOUT; + dynamicgraph::Signal timeSOUT; + dynamicgraph::Signal timeDoubleSOUT; /* These signals can be called several time per period, but give * always the same results different results. Useful for synchro. */ - dg::SignalTimeDependent timeOnceSOUT; - dg::SignalTimeDependent timeOnceDoubleSOUT; + dynamicgraph::SignalTimeDependent timeOnceSOUT; + dynamicgraph::SignalTimeDependent timeOnceDoubleSOUT; protected: /* --- SIGNAL FUNCTIONS --- */ - dg::Vector &getTimeStamp(dg::Vector &res, const int &time); - double &getTimeStampDouble(const dg::Vector &vect, double &res); + dynamicgraph::Vector &getTimeStamp(dynamicgraph::Vector &res, const int &time); + double &getTimeStampDouble(const dynamicgraph::Vector &vect, double &res); }; } /* namespace sot */ diff --git a/include/sot/core/timer.hh b/include/sot/core/timer.hh index 7651c81af..0913968eb 100644 --- a/include/sot/core/timer.hh +++ b/include/sot/core/timer.hh @@ -47,9 +47,8 @@ /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -namespace dg = dynamicgraph; -template class Timer_EXPORT Timer : public dg::Entity { +template class Timer_EXPORT Timer : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -72,13 +71,13 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ - dg::SignalPtr sigSIN; - dg::SignalTimeDependent sigSOUT; - dg::SignalTimeDependent sigClockSOUT; - dg::Signal timerSOUT; + dynamicgraph::SignalPtr sigSIN; + dynamicgraph::SignalTimeDependent sigSOUT; + dynamicgraph::SignalTimeDependent sigClockSOUT; + dynamicgraph::Signal timerSOUT; protected: /* --- SIGNAL FUNCTIONS --- */ - void plug(dg::Signal &sig) { + void plug(dynamicgraph::Signal &sig) { sigSIN = &sig; dt = 0.; } diff --git a/include/sot/core/trajectory.hh b/include/sot/core/trajectory.hh index 7c25d02f7..b6957f8e1 100644 --- a/include/sot/core/trajectory.hh +++ b/include/sot/core/trajectory.hh @@ -17,8 +17,6 @@ #include #include -namespace dg = dynamicgraph; -namespace ba = boost::assign; namespace dynamicgraph { namespace sot { @@ -124,7 +122,7 @@ public: void display(std::ostream &os) const { boost::array names = - ba::list_of("Positions")("Velocities")("Accelerations")("Effort"); + boost::assign::list_of("Positions")("Velocities")("Accelerations")("Effort"); const std::vector *points = 0; diff --git a/include/sot/core/vector-constant.hh b/include/sot/core/vector-constant.hh index ba197a9b3..245c00e5e 100644 --- a/include/sot/core/vector-constant.hh +++ b/include/sot/core/vector-constant.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- VECTOR ---------------------------------------------------------- */ @@ -42,10 +41,10 @@ public: virtual ~VectorConstant(void) {} - SignalTimeDependent SOUT; + SignalTimeDependent SOUT; /// \brief Set value of vector (and therefore of output signal) - void setValue(const dg::Vector &inValue); + void setValue(const dynamicgraph::Vector &inValue); }; } // namespace sot diff --git a/include/sot/core/vector-to-rotation.hh b/include/sot/core/vector-to-rotation.hh index 53e79bf54..9a4bb2aeb 100644 --- a/include/sot/core/vector-to-rotation.hh +++ b/include/sot/core/vector-to-rotation.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* STD */ #include @@ -40,9 +39,8 @@ namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; -class SOTVECTORTOROTATION_EXPORT VectorToRotation : public dg::Entity { +class SOTVECTORTOROTATION_EXPORT VectorToRotation : public dynamicgraph::Entity { static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -56,10 +54,10 @@ public: virtual ~VectorToRotation(void) {} - dg::SignalPtr SIN; - dg::SignalTimeDependent SOUT; + dynamicgraph::SignalPtr SIN; + dynamicgraph::SignalTimeDependent SOUT; - MatrixRotation &computeRotation(const dg::Vector &angles, + MatrixRotation &computeRotation(const dynamicgraph::Vector &angles, MatrixRotation &res); }; diff --git a/include/sot/core/visual-point-projecter.hh b/include/sot/core/visual-point-projecter.hh index 186aa2035..98659a736 100644 --- a/include/sot/core/visual-point-projecter.hh +++ b/include/sot/core/visual-point-projecter.hh @@ -25,7 +25,7 @@ /* Matrix */ #include -namespace dg = dynamicgraph; + #include /* SOT */ @@ -52,12 +52,12 @@ public: /* --- ENTITY INHERITANCE --- */ virtual const std::string &getClassName(void) const { return CLASS_NAME; } public: /* --- SIGNALS --- */ - DECLARE_SIGNAL_IN(point3D, dg::Vector); + DECLARE_SIGNAL_IN(point3D, dynamicgraph::Vector); DECLARE_SIGNAL_IN(transfo, MatrixHomogeneous); - DECLARE_SIGNAL_OUT(point3Dgaze, dg::Vector); + DECLARE_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector); DECLARE_SIGNAL_OUT(depth, double); - DECLARE_SIGNAL_OUT(point2D, dg::Vector); + DECLARE_SIGNAL_OUT(point2D, dynamicgraph::Vector); }; // class VisualPointProjecter diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index e87c0c2f5..99c17c571 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -213,7 +213,7 @@ void ParameterServer::setForceNameToForceId(const std::string &forceName, static_cast(forceId)); } -void ParameterServer::setJoints(const dg::Vector &urdf_to_sot) { +void ParameterServer::setJoints(const dynamicgraph::Vector &urdf_to_sot) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!"); return; diff --git a/src/tools/sequencer.cpp b/src/tools/sequencer.cpp index 2cd18134a..ececc599e 100644 --- a/src/tools/sequencer.cpp +++ b/src/tools/sequencer.cpp @@ -59,7 +59,7 @@ class sotEventTaskBased : public Sequencer::sotEventAbstract { cmdArgs >> taskname; sotDEBUG(15) << "Add task " << taskname << std::endl; taskPtr = dynamic_cast( - &dg::PoolStorage::getInstance()->getEntity(taskname)); + &dynamicgraph::PoolStorage::getInstance()->getEntity(taskname)); } } virtual void display(std::ostream &os) const { diff --git a/tests/tools/test_robot_utils.cpp b/tests/tools/test_robot_utils.cpp index 6f1955101..47169e8fb 100644 --- a/tests/tools/test_robot_utils.cpp +++ b/tests/tools/test_robot_utils.cpp @@ -56,7 +56,7 @@ int main(void) { /*Test set urdf_to_sot */ - dg::Vector urdf_to_sot(3); + dynamicgraph::Vector urdf_to_sot(3); urdf_to_sot << 0, 2, 1; robot_util->set_urdf_to_sot(urdf_to_sot); if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot) { @@ -65,10 +65,10 @@ int main(void) { std::cout << "ERROR: urdf_to_sot does not work !" << std::endl; } /*Test joints_urdf_to_sot and joints_sot_to_urdf */ - dg::Vector q_urdf(3); + dynamicgraph::Vector q_urdf(3); q_urdf << 10, 20, 30; - dg::Vector q_sot(3); - dg::Vector q_test_urdf(3); + dynamicgraph::Vector q_sot(3); + dynamicgraph::Vector q_test_urdf(3); robot_util->joints_urdf_to_sot(q_urdf, q_sot); robot_util->joints_sot_to_urdf(q_sot, q_test_urdf); if (q_urdf == q_test_urdf) { @@ -81,23 +81,23 @@ int main(void) { } /*Test velocity_sot_to_urdf and velocity_urdf_to_sot */ - dg::Vector q2_urdf(10); - dg::Vector v_urdf(9); - dg::Vector v_sot(9); + dynamicgraph::Vector q2_urdf(10); + dynamicgraph::Vector v_urdf(9); + dynamicgraph::Vector v_sot(9); robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot); robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf); std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !" << std::endl; /*Test base_urdf_to_sot and base_sot_to_urdf */ - dg::Vector base_q_urdf(7); - dg::Vector base_q_sot(6); + dynamicgraph::Vector base_q_urdf(7); + dynamicgraph::Vector base_q_sot(6); robot_util->base_urdf_to_sot(base_q_urdf, base_q_sot); robot_util->base_sot_to_urdf(base_q_sot, base_q_urdf); std::cout << "base_urdf_to_sot and base_sot_to_urdf work !" << std::endl; /*Test config_urdf_to_sot and config_sot_to_urdf */ - dg::Vector q2_sot(9); + dynamicgraph::Vector q2_sot(9); robot_util->config_urdf_to_sot(q2_urdf, q2_sot); robot_util->config_sot_to_urdf(q2_sot, q2_urdf); std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl; @@ -116,9 +116,9 @@ int main(void) { robot_util->m_force_util.set_name_to_force_id(lh, 2); robot_util->m_force_util.set_name_to_force_id(rh, 3); - dg::Vector lf_lim(6); + dynamicgraph::Vector lf_lim(6); lf_lim << 1, 2, 3, 4, 5, 6; - dg::Vector uf_lim(6); + dynamicgraph::Vector uf_lim(6); uf_lim << 10, 20, 30, 40, 50, 60; robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim); if (robot_util->m_force_util.get_id_from_name(rf) == 0 && From b54f6da33ebba07870493cc3ca1f8081571b5415 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Mon, 6 Jul 2020 11:05:53 +0200 Subject: [PATCH 29/43] Fix dg namespace pb. --- include/sot/core/binary-int-to-uint.hh | 7 ++- include/sot/core/clamp-workspace.hh | 24 +++++----- include/sot/core/com-freezer.hh | 15 +++--- include/sot/core/control-gr.hh | 12 ++--- include/sot/core/control-pd.hh | 29 ++++++------ include/sot/core/derivator-impl.hh | 5 +- include/sot/core/derivator.hh | 11 ++--- include/sot/core/device.hh | 42 ++++++++-------- include/sot/core/exp-moving-avg.hh | 9 ++-- include/sot/core/feature-1d.hh | 9 ++-- include/sot/core/feature-joint-limits.hh | 15 +++--- include/sot/core/feature-line-distance.hh | 17 ++++--- include/sot/core/feature-point6d-relative.hh | 21 ++++---- include/sot/core/feature-point6d.hh | 13 +++-- include/sot/core/feature-pose.hh | 23 +++++---- include/sot/core/feature-task.hh | 2 +- include/sot/core/feature-vector3.hh | 13 +++-- include/sot/core/feature-visual-point.hh | 13 +++-- include/sot/core/fir-filter.hh | 1 - include/sot/core/gain-hyperbolic.hh | 8 ++-- include/sot/core/gradient-ascent.hh | 9 ++-- include/sot/core/gripper-control.hh | 50 ++++++++++---------- include/sot/core/integrator-abstract-impl.hh | 2 +- include/sot/core/integrator-abstract.hh | 13 +++-- include/sot/core/integrator-euler.hh | 11 ++--- include/sot/core/joint-limitator.hh | 20 ++++---- include/sot/core/joint-trajectory-entity.hh | 22 ++++----- include/sot/core/mailbox.hh | 9 ++-- include/sot/core/mailbox.hxx | 2 - include/sot/core/matrix-constant.hh | 5 +- include/sot/core/memory-task-sot.hh | 7 ++- include/sot/core/motion-period.hh | 8 ++-- include/sot/core/neck-limitation.hh | 10 ++-- include/sot/core/op-point-modifier.hh | 14 +++--- include/sot/core/reader.hh | 10 ++-- include/sot/core/robot-simu.hh | 2 +- src/feature/feature-point6d-relative.cpp | 1 + tests/tools/test_device.cpp | 1 + 38 files changed, 223 insertions(+), 262 deletions(-) diff --git a/include/sot/core/binary-int-to-uint.hh b/include/sot/core/binary-int-to-uint.hh index e61879f1c..a073117f0 100644 --- a/include/sot/core/binary-int-to-uint.hh +++ b/include/sot/core/binary-int-to-uint.hh @@ -35,17 +35,16 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; -class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dg::Entity { +class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr binaryIntSIN; - dg::SignalTimeDependent binaryUintSOUT; + dynamicgraph::SignalPtr binaryIntSIN; + dynamicgraph::SignalTimeDependent binaryUintSOUT; public: BinaryIntToUint(const std::string &name); diff --git a/include/sot/core/clamp-workspace.hh b/include/sot/core/clamp-workspace.hh index d82905ed6..7b53a11d7 100644 --- a/include/sot/core/clamp-workspace.hh +++ b/include/sot/core/clamp-workspace.hh @@ -15,7 +15,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -39,24 +38,23 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dg::Entity { +class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr positionrefSIN; - dg::SignalPtr positionSIN; - dg::SignalTimeDependent alphaSOUT; - dg::SignalTimeDependent alphabarSOUT; - dg::SignalTimeDependent handrefSOUT; + dynamicgraph::SignalPtr positionrefSIN; + dynamicgraph::SignalPtr positionSIN; + dynamicgraph::SignalTimeDependent alphaSOUT; + dynamicgraph::SignalTimeDependent alphabarSOUT; + dynamicgraph::SignalTimeDependent handrefSOUT; public: ClampWorkspace(const std::string &name); @@ -64,8 +62,8 @@ public: void update(int time); - virtual dg::Matrix &computeOutput(dg::Matrix &res, int time); - virtual dg::Matrix &computeOutputBar(dg::Matrix &res, int time); + virtual dynamicgraph::Matrix &computeOutput(dynamicgraph::Matrix &res, int time); + virtual dynamicgraph::Matrix &computeOutputBar(dynamicgraph::Matrix &res, int time); virtual MatrixHomogeneous &computeRef(MatrixHomogeneous &res, int time); virtual void display(std::ostream &) const; @@ -73,10 +71,10 @@ public: private: int timeUpdate; - dg::Matrix alpha; - dg::Matrix alphabar; + dynamicgraph::Matrix alpha; + dynamicgraph::Matrix alphabar; MatrixHomogeneous prefMp; - dg::Vector pd; + dynamicgraph::Vector pd; MatrixRotation Rd; MatrixHomogeneous handref; diff --git a/include/sot/core/com-freezer.hh b/include/sot/core/com-freezer.hh index 6e4a7aee4..4621f6e25 100644 --- a/include/sot/core/com-freezer.hh +++ b/include/sot/core/com-freezer.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -39,19 +38,17 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; - /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTCOMFREEZER_EXPORT CoMFreezer : public dg::Entity { +class SOTCOMFREEZER_EXPORT CoMFreezer : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName() const { return CLASS_NAME; } private: - dg::Vector m_lastCoM; + dynamicgraph::Vector m_lastCoM; bool m_previousPGInProcess; int m_lastStopTime; @@ -60,12 +57,12 @@ public: /* --- CONSTRUCTION --- */ virtual ~CoMFreezer(void); public: /* --- SIGNAL --- */ - dg::SignalPtr CoMRefSIN; - dg::SignalPtr PGInProcessSIN; - dg::SignalTimeDependent freezedCoMSOUT; + dynamicgraph::SignalPtr CoMRefSIN; + dynamicgraph::SignalPtr PGInProcessSIN; + dynamicgraph::SignalTimeDependent freezedCoMSOUT; public: /* --- FUNCTION --- */ - dg::Vector &computeFreezedCoM(dg::Vector &freezedCoM, const int &time); + dynamicgraph::Vector &computeFreezedCoM(dynamicgraph::Vector &freezedCoM, const int &time); public: /* --- PARAMS --- */ virtual void display(std::ostream &os) const; diff --git a/include/sot/core/control-gr.hh b/include/sot/core/control-gr.hh index 0ba768e92..c0ac928f6 100644 --- a/include/sot/core/control-gr.hh +++ b/include/sot/core/control-gr.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -40,7 +39,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ @@ -70,14 +68,14 @@ protected: double _dimension; public: /* --- SIGNALS --- */ - SignalPtr matrixASIN; - SignalPtr accelerationSIN; - SignalPtr gravitySIN; - SignalTimeDependent controlSOUT; + SignalPtr matrixASIN; + SignalPtr accelerationSIN; + SignalPtr gravitySIN; + SignalTimeDependent controlSOUT; protected: double &setsize(int dimension); - dg::Vector &computeControl(dg::Vector &tau, int t); + dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t); }; } // namespace sot diff --git a/include/sot/core/control-pd.hh b/include/sot/core/control-pd.hh index a9bc35762..293300f51 100644 --- a/include/sot/core/control-pd.hh +++ b/include/sot/core/control-pd.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -67,22 +66,22 @@ protected: double TimeStep; public: /* --- SIGNALS --- */ - SignalPtr KpSIN; - SignalPtr KdSIN; - SignalPtr positionSIN; - SignalPtr desiredpositionSIN; - SignalPtr velocitySIN; - SignalPtr desiredvelocitySIN; - SignalTimeDependent controlSOUT; - SignalTimeDependent positionErrorSOUT; - SignalTimeDependent velocityErrorSOUT; + SignalPtr KpSIN; + SignalPtr KdSIN; + SignalPtr positionSIN; + SignalPtr desiredpositionSIN; + SignalPtr velocitySIN; + SignalPtr desiredvelocitySIN; + SignalTimeDependent controlSOUT; + SignalTimeDependent positionErrorSOUT; + SignalTimeDependent velocityErrorSOUT; protected: - dg::Vector &computeControl(dg::Vector &tau, int t); - dg::Vector position_error_; - dg::Vector velocity_error_; - dg::Vector &getPositionError(dg::Vector &position_error, int t); - dg::Vector &getVelocityError(dg::Vector &velocity_error, int t); + dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t); + dynamicgraph::Vector position_error_; + dynamicgraph::Vector velocity_error_; + dynamicgraph::Vector &getPositionError(dynamicgraph::Vector &position_error, int t); + dynamicgraph::Vector &getVelocityError(dynamicgraph::Vector &velocity_error, int t); }; } // namespace sot diff --git a/include/sot/core/derivator-impl.hh b/include/sot/core/derivator-impl.hh index 616b4cd74..ba354eefa 100644 --- a/include/sot/core/derivator-impl.hh +++ b/include/sot/core/derivator-impl.hh @@ -33,7 +33,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; #ifdef WIN32 #define DECLARE_SPECIFICATION(className, sotSigType) \ @@ -47,8 +46,8 @@ namespace dg = dynamicgraph; #endif DECLARE_SPECIFICATION(DerivatorDouble, double) -DECLARE_SPECIFICATION(DerivatorVector, dg::Vector) -DECLARE_SPECIFICATION(DerivatorMatrix, dg::Matrix) +DECLARE_SPECIFICATION(DerivatorVector, dynamicgraph::Vector) +DECLARE_SPECIFICATION(DerivatorMatrix, dynamicgraph::Matrix) DECLARE_SPECIFICATION(DerivatorVectorQuaternion, VectorQuaternion) } /* namespace sot */ } /* namespace dynamicgraph */ diff --git a/include/sot/core/derivator.hh b/include/sot/core/derivator.hh index 883c141a1..82a7413ad 100644 --- a/include/sot/core/derivator.hh +++ b/include/sot/core/derivator.hh @@ -29,13 +29,12 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -template class Derivator : public dg::Entity { +template class Derivator : public dynamicgraph::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); protected: @@ -48,7 +47,7 @@ public: /* --- CONSTRUCTION --- */ static std::string getTypeName(void) { return "Unknown"; } Derivator(const std::string &name) - : dg::Entity(name), memory(), initialized(false), + : dynamicgraph::Entity(name), memory(), initialized(false), timestep(TIMESTEP_DEFAULT), SIN(NULL, "sotDerivator<" + getTypeName() + ">(" + name + ")::input(" + getTypeName() + ")::sin"), @@ -65,9 +64,9 @@ public: /* --- CONSTRUCTION --- */ virtual ~Derivator(void){}; public: /* --- SIGNAL --- */ - dg::SignalPtr SIN; - dg::SignalTimeDependent SOUT; - dg::Signal timestepSIN; + dynamicgraph::SignalPtr SIN; + dynamicgraph::SignalTimeDependent SOUT; + dynamicgraph::Signal timestepSIN; protected: T &computeDerivation(T &res, int time) { diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 4ff081192..9cb5c266c 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -17,7 +17,7 @@ /* -- MaaL --- */ #include -namespace dg = dynamicgraph; + /* SOT */ #include "sot/core/api.hh" #include "sot/core/periodic-call.hh" @@ -55,10 +55,10 @@ public: }; protected: - dg::Vector state_; - dg::Vector velocity_; + dynamicgraph::Vector state_; + dynamicgraph::Vector velocity_; bool sanityCheck_; - dg::Vector vel_control_; + dynamicgraph::Vector vel_control_; ControlInput controlInputType_; bool withForceSignals[4]; PeriodicCall periodicCallBefore_; @@ -81,9 +81,9 @@ public: virtual ~Device(); virtual void setStateSize(const unsigned int &size); - virtual void setState(const dg::Vector &st); + virtual void setState(const dynamicgraph::Vector &st); void setVelocitySize(const unsigned int &size); - virtual void setVelocity(const dg::Vector &vel); + virtual void setVelocity(const dynamicgraph::Vector &vel); virtual void setSecondOrderIntegration(); virtual void setNoIntegration(); virtual void setControlInputType(const std::string &cit); @@ -107,20 +107,20 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ - dynamicgraph::SignalPtr controlSIN; - dynamicgraph::SignalPtr attitudeSIN; - dynamicgraph::SignalPtr zmpSIN; + dynamicgraph::SignalPtr controlSIN; + dynamicgraph::SignalPtr attitudeSIN; + dynamicgraph::SignalPtr zmpSIN; /// \name Device current state. /// \{ - dynamicgraph::Signal stateSOUT; - dynamicgraph::Signal velocitySOUT; + dynamicgraph::Signal stateSOUT; + dynamicgraph::Signal velocitySOUT; dynamicgraph::Signal attitudeSOUT; /*! \brief The current state of the robot from the command viewpoint. */ - dynamicgraph::Signal motorcontrolSOUT; - dynamicgraph::Signal previousControlSOUT; + dynamicgraph::Signal motorcontrolSOUT; + dynamicgraph::Signal previousControlSOUT; /*! \brief The ZMP reference send by the previous controller. */ - dynamicgraph::Signal ZMPPreviousControllerSOUT; + dynamicgraph::Signal ZMPPreviousControllerSOUT; /// \} /// \name Real robot current state @@ -129,19 +129,19 @@ public: /* --- SIGNALS --- */ /// does *not* match the state control input signal. /// \{ /// Motor positions - dynamicgraph::Signal robotState_; + dynamicgraph::Signal robotState_; /// Motor velocities - dynamicgraph::Signal robotVelocity_; + dynamicgraph::Signal robotVelocity_; /// The force torque sensors - dynamicgraph::Signal *forcesSOUT[4]; + dynamicgraph::Signal *forcesSOUT[4]; /// Motor torques /// \todo why pseudo ? - dynamicgraph::Signal pseudoTorqueSOUT; + dynamicgraph::Signal pseudoTorqueSOUT; /// \} protected: /// Compute roll pitch yaw angles of freeflyer joint. - void integrateRollPitchYaw(dg::Vector &state, const dg::Vector &control, + void integrateRollPitchYaw(dynamicgraph::Vector &state, const dynamicgraph::Vector &control, double dt); /// Store Position of free flyer joint MatrixHomogeneous ffPose_; @@ -164,13 +164,13 @@ protected: const MatrixHomogeneous &freeFlyerPose() const; public: - virtual void setRoot(const dg::Matrix &root); + virtual void setRoot(const dynamicgraph::Matrix &root); virtual void setRoot(const MatrixHomogeneous &worldMwaist); private: // Intermediate variable to avoid dynamic allocation - dg::Vector forceZero6; + dynamicgraph::Vector forceZero6; }; } // namespace sot } // namespace dynamicgraph diff --git a/include/sot/core/exp-moving-avg.hh b/include/sot/core/exp-moving-avg.hh index 6857cc119..2ed1d4252 100644 --- a/include/sot/core/exp-moving-avg.hh +++ b/include/sot/core/exp-moving-avg.hh @@ -18,7 +18,6 @@ #include #include -namespace dg = ::dynamicgraph; namespace dynamicgraph { namespace sot { @@ -35,9 +34,9 @@ class SOT_CORE_DLLAPI ExpMovingAvg : public Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: - SignalPtr updateSIN; + SignalPtr updateSIN; SignalTimeDependent refresherSINTERN; - SignalTimeDependent averageSOUT; + SignalTimeDependent averageSOUT; public: ExpMovingAvg(const std::string &n); @@ -46,9 +45,9 @@ public: void setAlpha(const double &alpha_); protected: - dg::Vector &update(dg::Vector &res, const int &inTime); + dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime); - dg::Vector average; + dynamicgraph::Vector average; double alpha; bool init; diff --git a/include/sot/core/feature-1d.hh b/include/sot/core/feature-1d.hh index 9eef46551..23705f190 100644 --- a/include/sot/core/feature-1d.hh +++ b/include/sot/core/feature-1d.hh @@ -38,7 +38,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class Feature1D \brief Simple test: the task is defined to be e_2 = .5 . e'.e, with @@ -65,10 +64,10 @@ public: @{ */ /*! \brief Input for the error. */ - dg::SignalPtr errorSIN; + dynamicgraph::SignalPtr errorSIN; /*! \brief Input for the Jacobian. */ - dg::SignalPtr jacobianSIN; + dynamicgraph::SignalPtr jacobianSIN; /*! @} */ @@ -99,10 +98,10 @@ public: /*! \brief Compute the error between the desired value and the value itself. */ - virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); /*! \brief Compute the Jacobian of the value according to the robot state.. */ - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); /*! @} */ diff --git a/include/sot/core/feature-joint-limits.hh b/include/sot/core/feature-joint-limits.hh index fbb67c1a5..8a11cb1ed 100644 --- a/include/sot/core/feature-joint-limits.hh +++ b/include/sot/core/feature-joint-limits.hh @@ -38,7 +38,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeatureJointLimits @@ -62,10 +61,10 @@ protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr jointSIN; - dg::SignalPtr upperJlSIN; - dg::SignalPtr lowerJlSIN; - dg::SignalTimeDependent widthJlSINTERN; + dynamicgraph::SignalPtr jointSIN; + dynamicgraph::SignalPtr upperJlSIN; + dynamicgraph::SignalPtr lowerJlSIN; + dynamicgraph::SignalTimeDependent widthJlSINTERN; using FeatureAbstract::selectionSIN; @@ -84,9 +83,9 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dg::Vector &computeError(dg::Vector &res, int time); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); - dg::Vector &computeWidthJl(dg::Vector &res, const int &time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res, const int &time); /** Static Feature selection. */ inline static Flags selectActuated(void); diff --git a/include/sot/core/feature-line-distance.hh b/include/sot/core/feature-line-distance.hh index 37034c4a5..c618cd9e9 100644 --- a/include/sot/core/feature-line-distance.hh +++ b/include/sot/core/feature-line-distance.hh @@ -38,7 +38,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeatureLineDistance @@ -54,11 +53,11 @@ public: protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr positionSIN; - dg::SignalPtr articularJacobianSIN; - dg::SignalPtr positionRefSIN; - dg::SignalPtr vectorSIN; - dg::SignalTimeDependent lineSOUT; + dynamicgraph::SignalPtr positionSIN; + dynamicgraph::SignalPtr articularJacobianSIN; + dynamicgraph::SignalPtr positionRefSIN; + dynamicgraph::SignalPtr vectorSIN; + dynamicgraph::SignalTimeDependent lineSOUT; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -76,9 +75,9 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dg::Vector &computeError(dg::Vector &res, int time); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); - dg::Vector &computeLineCoordinates(dg::Vector &cood, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + dynamicgraph::Vector &computeLineCoordinates(dynamicgraph::Vector &cood, int time); virtual void display(std::ostream &os) const; }; diff --git a/include/sot/core/feature-point6d-relative.hh b/include/sot/core/feature-point6d-relative.hh index 9cf4afa83..e1d073070 100644 --- a/include/sot/core/feature-point6d-relative.hh +++ b/include/sot/core/feature-point6d-relative.hh @@ -40,7 +40,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeaturePoint6dRelative @@ -56,23 +55,23 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - dg::Matrix L; + dynamicgraph::Matrix L; /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr positionReferenceSIN; - dg::SignalPtr articularJacobianReferenceSIN; + dynamicgraph::SignalPtr positionReferenceSIN; + dynamicgraph::SignalPtr articularJacobianReferenceSIN; - /*! dg::Signals related to the computation of the derivative of + /*! dynamicgraph::Signals related to the computation of the derivative of the error @{ */ - /*! dg::Signals giving the derivative of the input signals. + /*! dynamicgraph::Signals giving the derivative of the input signals. @{*/ /*! Derivative of the relative position. */ - dg::SignalPtr dotpositionSIN; + dynamicgraph::SignalPtr dotpositionSIN; /*! Derivative of the reference position. */ - dg::SignalPtr dotpositionReferenceSIN; + dynamicgraph::SignalPtr dotpositionReferenceSIN; /*! @} */ using FeaturePoint6d::getReference; @@ -81,9 +80,9 @@ public: FeaturePoint6dRelative(const std::string &name); virtual ~FeaturePoint6dRelative(void) {} - virtual dg::Vector &computeError(dg::Vector &res, int time); - virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); virtual void display(std::ostream &os) const; diff --git a/include/sot/core/feature-point6d.hh b/include/sot/core/feature-point6d.hh index 97b39443e..e2624f071 100644 --- a/include/sot/core/feature-point6d.hh +++ b/include/sot/core/feature-point6d.hh @@ -40,7 +40,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeaturePoint6d @@ -71,9 +70,9 @@ private: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr positionSIN; - dg::SignalPtr velocitySIN; - dg::SignalPtr articularJacobianSIN; + dynamicgraph::SignalPtr positionSIN; + dynamicgraph::SignalPtr velocitySIN; + dynamicgraph::SignalPtr articularJacobianSIN; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -90,9 +89,9 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dg::Vector &computeError(dg::Vector &res, int time); - virtual dg::Vector &computeErrordot(dg::Vector &res, int time); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); /** Static Feature selection. */ inline static Flags selectX(void) { return FLAG_LINE_1; } diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index 6ca40ac71..a55d84208 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -24,7 +24,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /// Enum used to specify what difference operation is used in FeaturePose. enum Representation_t { SE3Representation, R3xSO3Representation }; @@ -71,23 +70,23 @@ public: @{ */ /// Input pose of Joint A wrt to world frame. - dg::SignalPtr oMja; + dynamicgraph::SignalPtr oMja; /// Input pose of Frame A wrt to Joint A. - dg::SignalPtr jaMfa; + dynamicgraph::SignalPtr jaMfa; /// Input pose of Joint B wrt to world frame. - dg::SignalPtr oMjb; + dynamicgraph::SignalPtr oMjb; /// Input pose of Frame B wrt to Joint B. - dg::SignalPtr jbMfb; + dynamicgraph::SignalPtr jbMfb; /// Jacobian of the input Joint A, expressed in Joint A - dg::SignalPtr jaJja; + dynamicgraph::SignalPtr jaJja; /// Jacobian of the input Joint B, expressed in Joint B - dg::SignalPtr jbJjb; + dynamicgraph::SignalPtr jbJjb; /// The desired pose of Frame B wrt to Frame A. - dg::SignalPtr faMfbDes; + dynamicgraph::SignalPtr faMfbDes; /// The desired velocity of Frame B wrt to Frame A. The /// value is expressed in Frame A. - dg::SignalPtr faNufafbDes; + dynamicgraph::SignalPtr faNufafbDes; /*! @} */ /*! \name Output signals @@ -121,21 +120,21 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); /// Computes \f$ {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \f$ - virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); /// Computes \f$ \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \f$. /// There are two different cases, depending on the representation: /// - R3xSO3Representation: \f$ X = \left( \begin{array}{cc} I_3 & [ /// {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \f$ /// - SE3Representation: \f$ X = {{}^{fa}X^*_{fb}}^{-1} \f$ (see /// pinocchio::SE3Base::toActionMatrix) - virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); + virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); /// Computes \f$ \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}} /// {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \f$. There are two /// different cases, depending on the representation: /// - R3xSO3Representation: \f$ Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} & /// 0_3 \\ 0_3 & I_3 \end{array} \right) \f$ /// - SE3Representation: \f$ Y = I_6 \f$ - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); /** Static Feature selection. */ inline static Flags selectX(void) { return FLAG_LINE_1; } diff --git a/include/sot/core/feature-task.hh b/include/sot/core/feature-task.hh index 66df85ac3..d1f33f40a 100644 --- a/include/sot/core/feature-task.hh +++ b/include/sot/core/feature-task.hh @@ -39,7 +39,7 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; + class SOTFEATURETASK_EXPORT FeatureTask : public FeatureGeneric { diff --git a/include/sot/core/feature-vector3.hh b/include/sot/core/feature-vector3.hh index d5d42ebae..43402f531 100644 --- a/include/sot/core/feature-vector3.hh +++ b/include/sot/core/feature-vector3.hh @@ -39,7 +39,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeatureVector3 @@ -56,10 +55,10 @@ public: protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr vectorSIN; - dg::SignalPtr positionSIN; - dg::SignalPtr articularJacobianSIN; - dg::SignalPtr positionRefSIN; + dynamicgraph::SignalPtr vectorSIN; + dynamicgraph::SignalPtr positionSIN; + dynamicgraph::SignalPtr articularJacobianSIN; + dynamicgraph::SignalPtr positionRefSIN; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -71,8 +70,8 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dg::Vector &computeError(dg::Vector &res, int time); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); virtual void display(std::ostream &os) const; }; diff --git a/include/sot/core/feature-visual-point.hh b/include/sot/core/feature-visual-point.hh index 7bebd3d2e..ffdc37c5f 100644 --- a/include/sot/core/feature-visual-point.hh +++ b/include/sot/core/feature-visual-point.hh @@ -38,7 +38,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \class FeatureVisualPoint @@ -53,15 +52,15 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - dg::Matrix L; + dynamicgraph::Matrix L; /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalPtr xySIN; + dynamicgraph::SignalPtr xySIN; /** FeatureVisualPoint depth (required to compute the interaction matrix) * default Z = 1m. */ - dg::SignalPtr ZSIN; - dg::SignalPtr articularJacobianSIN; + dynamicgraph::SignalPtr ZSIN; + dynamicgraph::SignalPtr articularJacobianSIN; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -75,8 +74,8 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dg::Vector &computeError(dg::Vector &res, int time); - virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); /** Static Feature selection. */ inline static Flags selectX(void) { return FLAG_LINE_1; } diff --git a/include/sot/core/fir-filter.hh b/include/sot/core/fir-filter.hh index b543dbdd8..3e563b36e 100644 --- a/include/sot/core/fir-filter.hh +++ b/include/sot/core/fir-filter.hh @@ -21,7 +21,6 @@ #include #include -namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { diff --git a/include/sot/core/gain-hyperbolic.hh b/include/sot/core/gain-hyperbolic.hh index 3694eaef8..abba25c40 100644 --- a/include/sot/core/gain-hyperbolic.hh +++ b/include/sot/core/gain-hyperbolic.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -42,7 +41,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /** \brief Hyperbolic gain. * It follows the law \f[ g(e) = a \frac{\tanh(-b(||e|| - d)) + 1}{2} + c \f] @@ -52,7 +50,7 @@ namespace dg = dynamicgraph; * - \f$ c = 0.1 \f$, * - \f$ d = 0 \f$. */ -class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dg::Entity { +class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dynamicgraph::Entity { public: /* --- CONSTANTS --- */ /* Default values. */ @@ -94,8 +92,8 @@ public: /* --- INIT --- */ void forceConstant(void); public: /* --- SIGNALS --- */ - dg::SignalPtr errorSIN; - dg::SignalTimeDependent gainSOUT; + dynamicgraph::SignalPtr errorSIN; + dynamicgraph::SignalTimeDependent gainSOUT; protected: double &computeGain(double &res, int t); diff --git a/include/sot/core/gradient-ascent.hh b/include/sot/core/gradient-ascent.hh index fba98c76d..8eff01ba5 100644 --- a/include/sot/core/gradient-ascent.hh +++ b/include/sot/core/gradient-ascent.hh @@ -18,7 +18,6 @@ #include #include -namespace dg = ::dynamicgraph; namespace dynamicgraph { namespace sot { @@ -35,19 +34,19 @@ class SOT_CORE_DLLAPI GradientAscent : public Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: - SignalPtr gradientSIN; + SignalPtr gradientSIN; SignalPtr learningRateSIN; SignalTimeDependent refresherSINTERN; - SignalTimeDependent valueSOUT; + SignalTimeDependent valueSOUT; public: GradientAscent(const std::string &n); virtual ~GradientAscent(void); protected: - dg::Vector &update(dg::Vector &res, const int &inTime); + dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime); - dg::Vector value; + dynamicgraph::Vector value; double alpha; bool init; diff --git a/include/sot/core/gripper-control.hh b/include/sot/core/gripper-control.hh index 367396415..61189295a 100644 --- a/include/sot/core/gripper-control.hh +++ b/include/sot/core/gripper-control.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -47,7 +46,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \brief The goal of this entity is to ensure that the maximal torque will not * be exceeded during a grasping task. @@ -60,7 +58,7 @@ protected: double offset; static const double OFFSET_DEFAULT; //! \brief The multiplication - dg::Vector factor; + dynamicgraph::Vector factor; public: GripperControl(void); @@ -68,30 +66,30 @@ public: //! \brief Computes the // if the torque limit is reached, the normalized position is reduced by // (offset) - void computeIncrement(const dg::Vector &torques, - const dg::Vector &torqueLimits, - const dg::Vector ¤tNormVel); + void computeIncrement(const dynamicgraph::Vector &torques, + const dynamicgraph::Vector &torqueLimits, + const dynamicgraph::Vector ¤tNormVel); //! \brief - dg::Vector &computeDesiredPosition(const dg::Vector ¤tPos, - const dg::Vector &desiredPos, - const dg::Vector &torques, - const dg::Vector &torqueLimits, - dg::Vector &referencePos); + dynamicgraph::Vector &computeDesiredPosition(const dynamicgraph::Vector ¤tPos, + const dynamicgraph::Vector &desiredPos, + const dynamicgraph::Vector &torques, + const dynamicgraph::Vector &torqueLimits, + dynamicgraph::Vector &referencePos); /*! \brief select only some of the values of the vector fullsize, * based on the Flags vector. */ - static dg::Vector &selector(const dg::Vector &fullsize, const Flags &selec, - dg::Vector &desPos); + static dynamicgraph::Vector &selector(const dynamicgraph::Vector &fullsize, const Flags &selec, + dynamicgraph::Vector &desPos); }; /* --------------------------------------------------------------------- */ /* --- PLUGIN ---------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin : public dg::Entity, +class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin : public dynamicgraph::Entity, public GripperControl { DYNAMIC_GRAPH_ENTITY_DECL(); @@ -107,22 +105,22 @@ public: /* --- CONSTRUCTION --- */ public: /* --- SIGNAL --- */ /* --- INPUTS --- */ - dg::SignalPtr positionSIN; - dg::SignalPtr positionDesSIN; - dg::SignalPtr torqueSIN; - dg::SignalPtr torqueLimitSIN; - dg::SignalPtr selectionSIN; + dynamicgraph::SignalPtr positionSIN; + dynamicgraph::SignalPtr positionDesSIN; + dynamicgraph::SignalPtr torqueSIN; + dynamicgraph::SignalPtr torqueLimitSIN; + dynamicgraph::SignalPtr selectionSIN; /* --- INTERMEDIARY --- */ - dg::SignalPtr positionFullSizeSIN; - dg::SignalTimeDependent positionReduceSOUT; - dg::SignalPtr torqueFullSizeSIN; - dg::SignalTimeDependent torqueReduceSOUT; - dg::SignalPtr torqueLimitFullSizeSIN; - dg::SignalTimeDependent torqueLimitReduceSOUT; + dynamicgraph::SignalPtr positionFullSizeSIN; + dynamicgraph::SignalTimeDependent positionReduceSOUT; + dynamicgraph::SignalPtr torqueFullSizeSIN; + dynamicgraph::SignalTimeDependent torqueReduceSOUT; + dynamicgraph::SignalPtr torqueLimitFullSizeSIN; + dynamicgraph::SignalTimeDependent torqueLimitReduceSOUT; /* --- OUTPUTS --- */ - dg::SignalTimeDependent desiredPositionSOUT; + dynamicgraph::SignalTimeDependent desiredPositionSOUT; public: /* --- COMMANDLINE --- */ void initCommands(); diff --git a/include/sot/core/integrator-abstract-impl.hh b/include/sot/core/integrator-abstract-impl.hh index 2f4b15bbd..25df9ee43 100644 --- a/include/sot/core/integrator-abstract-impl.hh +++ b/include/sot/core/integrator-abstract-impl.hh @@ -46,7 +46,7 @@ namespace dynamicgraph { namespace sot { DECLARE_SPECIFICATION(IntegratorAbstractDouble, double, double) -DECLARE_SPECIFICATION(IntegratorAbstractVector, dg::Vector, dg::Matrix) +DECLARE_SPECIFICATION(IntegratorAbstractVector, dynamicgraph::Vector, dynamicgraph::Matrix) } // namespace sot } // namespace dynamicgraph #endif // #ifndef __SOT_MAILBOX_HH diff --git a/include/sot/core/integrator-abstract.hh b/include/sot/core/integrator-abstract.hh index b62a63211..9dc9e35c1 100644 --- a/include/sot/core/integrator-abstract.hh +++ b/include/sot/core/integrator-abstract.hh @@ -34,7 +34,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /*! \brief integrates an ODE. If Y is the output and X the input, the * following equation is integrated: @@ -43,20 +42,20 @@ namespace dg = dynamicgraph; * function between X and Y, while the b_i are those of the numerator. */ template -class IntegratorAbstract : public dg::Entity { +class IntegratorAbstract : public dynamicgraph::Entity { public: IntegratorAbstract(const std::string &name) - : dg::Entity(name), + : dynamicgraph::Entity(name), SIN(NULL, "sotIntegratorAbstract(" + name + ")::input(vector)::sin"), SOUT(boost::bind(&IntegratorAbstract::integrate, this, _1, _2), SIN, "sotIntegratorAbstract(" + name + ")::output(vector)::sout") { signalRegistration(SIN << SOUT); - using namespace dg::command; + using namespace dynamicgraph::command; const std::string typeName = - Value::typeName(dg::command::ValueHelper::TypeID); + Value::typeName(dynamicgraph::command::ValueHelper::TypeID); addCommand( "pushNumCoef", @@ -92,9 +91,9 @@ public: void popDenomCoef() { denominator.pop_back(); } public: - dg::SignalPtr SIN; + dynamicgraph::SignalPtr SIN; - dg::SignalTimeDependent SOUT; + dynamicgraph::SignalTimeDependent SOUT; protected: std::vector numerator; diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index 97458eaa6..e808c9b31 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -25,7 +25,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; namespace internal { template bool integratorEulerCoeffIsIdentity(const coefT c) { @@ -70,7 +69,7 @@ public: ")::output(vector)::derivativesout") { this->signalRegistration(derivativeSOUT); - using namespace dg::command; + using namespace dynamicgraph::command; setSamplingPeriod(0.005); @@ -97,7 +96,7 @@ protected: std::vector inputMemory; std::vector outputMemory; - dg::SignalTimeDependent derivativeSOUT; + dynamicgraph::SignalTimeDependent derivativeSOUT; double dt; double invdt; @@ -152,7 +151,7 @@ public: sigT &derivative(sigT &res, int time) { if (outputMemory.size() < 2) - throw dg::ExceptionSignal(dg::ExceptionSignal::GENERIC, + throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC, "Integrator does not compute the derivative."); SOUT.recompute(time); @@ -184,8 +183,8 @@ public: // Check that denominator.back is the identity if (!internal::integratorEulerCoeffIsIdentity(denominator.back())) - throw dg::ExceptionSignal( - dg::ExceptionSignal::GENERIC, + throw dynamicgraph::ExceptionSignal( + dynamicgraph::ExceptionSignal::GENERIC, "The coefficient of the highest order derivative of denominator " "should be 1 (the last pushDenomCoef should be the identity)."); } diff --git a/include/sot/core/joint-limitator.hh b/include/sot/core/joint-limitator.hh index af4d2617c..e3cce3244 100644 --- a/include/sot/core/joint-limitator.hh +++ b/include/sot/core/joint-limitator.hh @@ -11,7 +11,6 @@ #define SOT_FEATURE_JOINTLIMITS_HH // Matrix #include -namespace dg = dynamicgraph; // SOT #include @@ -30,32 +29,31 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /// \brief Filter control vector to avoid exceeding joint maximum values. /// /// This must be plugged between the entity producing the command /// (i.e. usually the sot) and the entity executing it (the device). -class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dg::Entity { +class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dynamicgraph::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: JointLimitator(const std::string &name); virtual ~JointLimitator() {} - virtual dg::Vector &computeControl(dg::Vector &res, int time); - dg::Vector &computeWidthJl(dg::Vector &res, const int &time); + virtual dynamicgraph::Vector &computeControl(dynamicgraph::Vector &res, int time); + dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res, const int &time); virtual void display(std::ostream &os) const; /// \name Signals /// \{ - dg::SignalPtr jointSIN; - dg::SignalPtr upperJlSIN; - dg::SignalPtr lowerJlSIN; - dg::SignalPtr controlSIN; - dg::SignalTimeDependent controlSOUT; - dg::SignalTimeDependent widthJlSINTERN; + dynamicgraph::SignalPtr jointSIN; + dynamicgraph::SignalPtr upperJlSIN; + dynamicgraph::SignalPtr lowerJlSIN; + dynamicgraph::SignalPtr controlSIN; + dynamicgraph::SignalTimeDependent controlSOUT; + dynamicgraph::SignalTimeDependent widthJlSINTERN; /// \} }; } // end of namespace sot. diff --git a/include/sot/core/joint-trajectory-entity.hh b/include/sot/core/joint-trajectory-entity.hh index e435e495e..91a7aa755 100644 --- a/include/sot/core/joint-trajectory-entity.hh +++ b/include/sot/core/joint-trajectory-entity.hh @@ -15,7 +15,7 @@ // Maal #include -namespace dg = dynamicgraph; + // SOT #include #include @@ -57,13 +57,13 @@ public: void loadFile(const std::string &name); /// \brief Return the next pose for the legs. - dg::Vector &getNextPosition(dg::Vector &pos, const int &time); + dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos, const int &time); /// \brief Return the next com. - dg::Vector &getNextCoM(dg::Vector &com, const int &time); + dynamicgraph::Vector &getNextCoM(dynamicgraph::Vector &com, const int &time); /// \brief Return the next cop. - dg::Vector &getNextCoP(dg::Vector &cop, const int &time); + dynamicgraph::Vector &getNextCoP(dynamicgraph::Vector &cop, const int &time); /// \brief Return the next waist. sot::MatrixHomogeneous &getNextWaist(sot::MatrixHomogeneous &waist, @@ -74,7 +74,7 @@ public: /// \brief Convert a xyztheta vector into an homogeneous matrix sot::MatrixHomogeneous - XYZThetaToMatrixHomogeneous(const dg::Vector &xyztheta); + XYZThetaToMatrixHomogeneous(const dynamicgraph::Vector &xyztheta); /// \brief Perform one update of the signals. int &OneStepOfUpdate(int &dummy, const int &time); @@ -102,13 +102,13 @@ public: SignalTimeDependent OneStepOfUpdateS; /// \brief Publish pose for each evaluation of the graph. - dynamicgraph::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; /// \brief Publish com for each evaluation of the graph. - dynamicgraph::SignalTimeDependent comSOUT; + dynamicgraph::SignalTimeDependent comSOUT; /// \brief Publish zmp for each evaluation of the graph. - dynamicgraph::SignalTimeDependent zmpSOUT; + dynamicgraph::SignalTimeDependent zmpSOUT; /// \brief Publish waist for each evaluation of the graph. dynamicgraph::SignalTimeDependent waistSOUT; @@ -128,13 +128,13 @@ protected: timestamp traj_timestamp_; /// \brief Store the pos; - dg::Vector pose_; + dynamicgraph::Vector pose_; /// \brief Store the center of mass. - dg::Vector com_; + dynamicgraph::Vector com_; /// \brief Store the center of pressure ZMP. - dg::Vector cop_; + dynamicgraph::Vector cop_; /// \brief Store the waist position. sot::MatrixHomogeneous waist_; diff --git a/include/sot/core/mailbox.hh b/include/sot/core/mailbox.hh index d28b4b2b8..0aab925af 100644 --- a/include/sot/core/mailbox.hh +++ b/include/sot/core/mailbox.hh @@ -31,9 +31,8 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; -template class Mailbox : public dg::Entity { +template class Mailbox : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -63,9 +62,9 @@ protected: bool update; public: /* --- SIGNALS --- */ - dg::SignalTimeDependent SOUT; - dg::SignalTimeDependent objSOUT; - dg::SignalTimeDependent timeSOUT; + dynamicgraph::SignalTimeDependent SOUT; + dynamicgraph::SignalTimeDependent objSOUT; + dynamicgraph::SignalTimeDependent timeSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/mailbox.hxx b/include/sot/core/mailbox.hxx index 747575852..a34b4b77d 100644 --- a/include/sot/core/mailbox.hxx +++ b/include/sot/core/mailbox.hxx @@ -15,8 +15,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; - /* -------------------------------------------------------------------------- */ /* --- CONSTRUCTION --------------------------------------------------------- */ /* -------------------------------------------------------------------------- */ diff --git a/include/sot/core/matrix-constant.hh b/include/sot/core/matrix-constant.hh index 29ba4eaab..f8fc26be3 100644 --- a/include/sot/core/matrix-constant.hh +++ b/include/sot/core/matrix-constant.hh @@ -12,7 +12,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- MATRIX ---------------------------------------------------------- */ @@ -36,14 +35,14 @@ public: int rows, cols; double color; - void setValue(const dg::Matrix &inValue); + void setValue(const dynamicgraph::Matrix &inValue); public: MatrixConstant(const std::string &name); virtual ~MatrixConstant(void) {} - SignalTimeDependent SOUT; + SignalTimeDependent SOUT; }; } // namespace sot diff --git a/include/sot/core/memory-task-sot.hh b/include/sot/core/memory-task-sot.hh index f88b5a39a..3fcb0f22e 100644 --- a/include/sot/core/memory-task-sot.hh +++ b/include/sot/core/memory-task-sot.hh @@ -20,7 +20,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract { public: // protected: @@ -30,10 +29,10 @@ public: // protected: KernelConst_t; /* Internal memory to reduce the dynamic allocation at task resolution. */ - dg::Vector err, tmpTask, tmpVar, tmpControl; - dg::Matrix Jt; //( nJ,mJ ); + dynamicgraph::Vector err, tmpTask, tmpVar, tmpControl; + dynamicgraph::Matrix Jt; //( nJ,mJ ); - dg::Matrix JK; //(nJ,mJ); + dynamicgraph::Matrix JK; //(nJ,mJ); SVD_t svd; Kernel_t kernel; diff --git a/include/sot/core/motion-period.hh b/include/sot/core/motion-period.hh index b2fc894de..25ff16cd2 100644 --- a/include/sot/core/motion-period.hh +++ b/include/sot/core/motion-period.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -47,9 +46,8 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; -class SOTMOTIONPERIOD_EXPORT MotionPeriod : public dg::Entity { +class SOTMOTIONPERIOD_EXPORT MotionPeriod : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; @@ -73,13 +71,13 @@ protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dg::SignalTimeDependent motionSOUT; + dynamicgraph::SignalTimeDependent motionSOUT; public: MotionPeriod(const std::string &name); virtual ~MotionPeriod(void) {} - dg::Vector &computeMotion(dg::Vector &res, const int &time); + dynamicgraph::Vector &computeMotion(dynamicgraph::Vector &res, const int &time); virtual void display(std::ostream &os) const; }; diff --git a/include/sot/core/neck-limitation.hh b/include/sot/core/neck-limitation.hh index 0a3b80eb1..23c593229 100644 --- a/include/sot/core/neck-limitation.hh +++ b/include/sot/core/neck-limitation.hh @@ -16,7 +16,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* SOT */ #include @@ -49,9 +48,8 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; -class NeckLimitation_EXPORT NeckLimitation : public dg::Entity { +class NeckLimitation_EXPORT NeckLimitation : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -74,11 +72,11 @@ public: /* --- CONSTRUCTION --- */ virtual ~NeckLimitation(void); public: /* --- SIGNAL --- */ - dg::SignalPtr jointSIN; - dg::SignalTimeDependent jointSOUT; + dynamicgraph::SignalPtr jointSIN; + dynamicgraph::SignalTimeDependent jointSOUT; public: /* --- FUNCTIONS --- */ - dg::Vector &computeJointLimitation(dg::Vector &jointLimited, + dynamicgraph::Vector &computeJointLimitation(dynamicgraph::Vector &jointLimited, const int &timeSpec); public: /* --- PARAMS --- */ diff --git a/include/sot/core/op-point-modifier.hh b/include/sot/core/op-point-modifier.hh index 4b442acf6..809c0dfd3 100644 --- a/include/sot/core/op-point-modifier.hh +++ b/include/sot/core/op-point-modifier.hh @@ -17,7 +17,6 @@ /* Matrix */ #include -namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- API ------------------------------------------------------------- */ @@ -39,7 +38,6 @@ namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; /// /// \brief Compute position and jacobian of a local frame attached to a joint. @@ -47,23 +45,23 @@ namespace dg = dynamicgraph; /// The position of the local frame in the frame of the joint is represented by /// transformation. /// -class SOTOPPOINTMODIFIER_EXPORT OpPointModifier : public dg::Entity { +class SOTOPPOINTMODIFIER_EXPORT OpPointModifier : public dynamicgraph::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } public: - dg::SignalPtr jacobianSIN; - dg::SignalPtr positionSIN; + dynamicgraph::SignalPtr jacobianSIN; + dynamicgraph::SignalPtr positionSIN; - dg::SignalTimeDependent jacobianSOUT; - dg::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent jacobianSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; public: OpPointModifier(const std::string &name); virtual ~OpPointModifier(void) {} - dg::Matrix &jacobianSOUT_function(dg::Matrix &res, const int &time); + dynamicgraph::Matrix &jacobianSOUT_function(dynamicgraph::Matrix &res, const int &time); MatrixHomogeneous &positionSOUT_function(MatrixHomogeneous &res, const int &time); void setTransformation(const Eigen::Matrix4d &tr); diff --git a/include/sot/core/reader.hh b/include/sot/core/reader.hh index 36f2c4d5a..7c216aea5 100644 --- a/include/sot/core/reader.hh +++ b/include/sot/core/reader.hh @@ -16,7 +16,7 @@ /* Matrix */ #include -namespace dg = dynamicgraph; + /* STD */ #include @@ -61,8 +61,8 @@ class SOTREADER_EXPORT sotReader : public Entity { public: SignalPtr selectionSIN; - SignalTimeDependent vectorSOUT; - SignalTimeDependent matrixSOUT; + SignalTimeDependent vectorSOUT; + SignalTimeDependent matrixSOUT; public: sotReader(const std::string n); @@ -80,8 +80,8 @@ protected: int rows, cols; - dg::Vector &getNextData(dg::Vector &res, const unsigned int time); - dg::Matrix &getNextMatrix(dg::Matrix &res, const unsigned int time); + dynamicgraph::Vector &getNextData(dynamicgraph::Vector &res, const unsigned int time); + dynamicgraph::Matrix &getNextMatrix(dynamicgraph::Matrix &res, const unsigned int time); void resize(const int &nbRow, const int &nbCol); public: diff --git a/include/sot/core/robot-simu.hh b/include/sot/core/robot-simu.hh index e37c83826..50d232d0d 100644 --- a/include/sot/core/robot-simu.hh +++ b/include/sot/core/robot-simu.hh @@ -17,7 +17,7 @@ /* -- MaaL --- */ #include -namespace dg = dynamicgraph; + /* SOT */ #include "sot/core/api.hh" #include "sot/core/device.hh" diff --git a/src/feature/feature-point6d-relative.cpp b/src/feature/feature-point6d-relative.cpp index e6ab8179b..950eb7bc2 100644 --- a/src/feature/feature-point6d-relative.cpp +++ b/src/feature/feature-point6d-relative.cpp @@ -23,6 +23,7 @@ using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; +namespace dg = dynamicgraph; #include DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative, diff --git a/tests/tools/test_device.cpp b/tests/tools/test_device.cpp index ebe3437a6..a2edb35b1 100644 --- a/tests/tools/test_device.cpp +++ b/tests/tools/test_device.cpp @@ -25,6 +25,7 @@ using namespace std; using namespace dynamicgraph; using namespace dynamicgraph::sot; +namespace dg = dynamicgraph; class TestDevice : public dg::sot::Device { public: From 83e88f59490cf2116ca9434dc8a536169f490496 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:33:11 +0200 Subject: [PATCH 30/43] Revert "Fix dg namespace pb." This reverts commit b54f6da33ebba07870493cc3ca1f8081571b5415. --- include/sot/core/binary-int-to-uint.hh | 7 +-- include/sot/core/clamp-workspace.hh | 24 +++++----- include/sot/core/com-freezer.hh | 15 +++--- include/sot/core/control-gr.hh | 12 +++-- include/sot/core/control-pd.hh | 29 ++++++------ include/sot/core/derivator-impl.hh | 5 +- include/sot/core/derivator.hh | 11 +++-- include/sot/core/device.hh | 42 ++++++++-------- include/sot/core/exp-moving-avg.hh | 9 ++-- include/sot/core/feature-1d.hh | 9 ++-- include/sot/core/feature-joint-limits.hh | 15 +++--- include/sot/core/feature-line-distance.hh | 17 +++---- include/sot/core/feature-point6d-relative.hh | 21 ++++---- include/sot/core/feature-point6d.hh | 13 ++--- include/sot/core/feature-pose.hh | 23 ++++----- include/sot/core/feature-task.hh | 2 +- include/sot/core/feature-vector3.hh | 13 ++--- include/sot/core/feature-visual-point.hh | 13 ++--- include/sot/core/fir-filter.hh | 1 + include/sot/core/gain-hyperbolic.hh | 8 ++-- include/sot/core/gradient-ascent.hh | 9 ++-- include/sot/core/gripper-control.hh | 50 ++++++++++---------- include/sot/core/integrator-abstract-impl.hh | 2 +- include/sot/core/integrator-abstract.hh | 13 ++--- include/sot/core/integrator-euler.hh | 11 +++-- include/sot/core/joint-limitator.hh | 20 ++++---- include/sot/core/joint-trajectory-entity.hh | 22 ++++----- include/sot/core/mailbox.hh | 9 ++-- include/sot/core/mailbox.hxx | 2 + include/sot/core/matrix-constant.hh | 5 +- include/sot/core/memory-task-sot.hh | 7 +-- include/sot/core/motion-period.hh | 8 ++-- include/sot/core/neck-limitation.hh | 10 ++-- include/sot/core/op-point-modifier.hh | 14 +++--- include/sot/core/reader.hh | 10 ++-- include/sot/core/robot-simu.hh | 2 +- src/feature/feature-point6d-relative.cpp | 1 - tests/tools/test_device.cpp | 1 - 38 files changed, 262 insertions(+), 223 deletions(-) diff --git a/include/sot/core/binary-int-to-uint.hh b/include/sot/core/binary-int-to-uint.hh index a073117f0..e61879f1c 100644 --- a/include/sot/core/binary-int-to-uint.hh +++ b/include/sot/core/binary-int-to-uint.hh @@ -35,16 +35,17 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; -class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dynamicgraph::Entity { +class SOTBINARYINTTOUINT_EXPORT BinaryIntToUint : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr binaryIntSIN; - dynamicgraph::SignalTimeDependent binaryUintSOUT; + dg::SignalPtr binaryIntSIN; + dg::SignalTimeDependent binaryUintSOUT; public: BinaryIntToUint(const std::string &name); diff --git a/include/sot/core/clamp-workspace.hh b/include/sot/core/clamp-workspace.hh index 7b53a11d7..d82905ed6 100644 --- a/include/sot/core/clamp-workspace.hh +++ b/include/sot/core/clamp-workspace.hh @@ -15,6 +15,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -38,23 +39,24 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dynamicgraph::Entity { +class SOTCLAMPWORKSPACE_EXPORT ClampWorkspace : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr positionrefSIN; - dynamicgraph::SignalPtr positionSIN; - dynamicgraph::SignalTimeDependent alphaSOUT; - dynamicgraph::SignalTimeDependent alphabarSOUT; - dynamicgraph::SignalTimeDependent handrefSOUT; + dg::SignalPtr positionrefSIN; + dg::SignalPtr positionSIN; + dg::SignalTimeDependent alphaSOUT; + dg::SignalTimeDependent alphabarSOUT; + dg::SignalTimeDependent handrefSOUT; public: ClampWorkspace(const std::string &name); @@ -62,8 +64,8 @@ public: void update(int time); - virtual dynamicgraph::Matrix &computeOutput(dynamicgraph::Matrix &res, int time); - virtual dynamicgraph::Matrix &computeOutputBar(dynamicgraph::Matrix &res, int time); + virtual dg::Matrix &computeOutput(dg::Matrix &res, int time); + virtual dg::Matrix &computeOutputBar(dg::Matrix &res, int time); virtual MatrixHomogeneous &computeRef(MatrixHomogeneous &res, int time); virtual void display(std::ostream &) const; @@ -71,10 +73,10 @@ public: private: int timeUpdate; - dynamicgraph::Matrix alpha; - dynamicgraph::Matrix alphabar; + dg::Matrix alpha; + dg::Matrix alphabar; MatrixHomogeneous prefMp; - dynamicgraph::Vector pd; + dg::Vector pd; MatrixRotation Rd; MatrixHomogeneous handref; diff --git a/include/sot/core/com-freezer.hh b/include/sot/core/com-freezer.hh index 4621f6e25..6e4a7aee4 100644 --- a/include/sot/core/com-freezer.hh +++ b/include/sot/core/com-freezer.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -38,17 +39,19 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; + /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTCOMFREEZER_EXPORT CoMFreezer : public dynamicgraph::Entity { +class SOTCOMFREEZER_EXPORT CoMFreezer : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName() const { return CLASS_NAME; } private: - dynamicgraph::Vector m_lastCoM; + dg::Vector m_lastCoM; bool m_previousPGInProcess; int m_lastStopTime; @@ -57,12 +60,12 @@ public: /* --- CONSTRUCTION --- */ virtual ~CoMFreezer(void); public: /* --- SIGNAL --- */ - dynamicgraph::SignalPtr CoMRefSIN; - dynamicgraph::SignalPtr PGInProcessSIN; - dynamicgraph::SignalTimeDependent freezedCoMSOUT; + dg::SignalPtr CoMRefSIN; + dg::SignalPtr PGInProcessSIN; + dg::SignalTimeDependent freezedCoMSOUT; public: /* --- FUNCTION --- */ - dynamicgraph::Vector &computeFreezedCoM(dynamicgraph::Vector &freezedCoM, const int &time); + dg::Vector &computeFreezedCoM(dg::Vector &freezedCoM, const int &time); public: /* --- PARAMS --- */ virtual void display(std::ostream &os) const; diff --git a/include/sot/core/control-gr.hh b/include/sot/core/control-gr.hh index c0ac928f6..0ba768e92 100644 --- a/include/sot/core/control-gr.hh +++ b/include/sot/core/control-gr.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -39,6 +40,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ @@ -68,14 +70,14 @@ protected: double _dimension; public: /* --- SIGNALS --- */ - SignalPtr matrixASIN; - SignalPtr accelerationSIN; - SignalPtr gravitySIN; - SignalTimeDependent controlSOUT; + SignalPtr matrixASIN; + SignalPtr accelerationSIN; + SignalPtr gravitySIN; + SignalTimeDependent controlSOUT; protected: double &setsize(int dimension); - dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t); + dg::Vector &computeControl(dg::Vector &tau, int t); }; } // namespace sot diff --git a/include/sot/core/control-pd.hh b/include/sot/core/control-pd.hh index 293300f51..a9bc35762 100644 --- a/include/sot/core/control-pd.hh +++ b/include/sot/core/control-pd.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -66,22 +67,22 @@ protected: double TimeStep; public: /* --- SIGNALS --- */ - SignalPtr KpSIN; - SignalPtr KdSIN; - SignalPtr positionSIN; - SignalPtr desiredpositionSIN; - SignalPtr velocitySIN; - SignalPtr desiredvelocitySIN; - SignalTimeDependent controlSOUT; - SignalTimeDependent positionErrorSOUT; - SignalTimeDependent velocityErrorSOUT; + SignalPtr KpSIN; + SignalPtr KdSIN; + SignalPtr positionSIN; + SignalPtr desiredpositionSIN; + SignalPtr velocitySIN; + SignalPtr desiredvelocitySIN; + SignalTimeDependent controlSOUT; + SignalTimeDependent positionErrorSOUT; + SignalTimeDependent velocityErrorSOUT; protected: - dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t); - dynamicgraph::Vector position_error_; - dynamicgraph::Vector velocity_error_; - dynamicgraph::Vector &getPositionError(dynamicgraph::Vector &position_error, int t); - dynamicgraph::Vector &getVelocityError(dynamicgraph::Vector &velocity_error, int t); + dg::Vector &computeControl(dg::Vector &tau, int t); + dg::Vector position_error_; + dg::Vector velocity_error_; + dg::Vector &getPositionError(dg::Vector &position_error, int t); + dg::Vector &getVelocityError(dg::Vector &velocity_error, int t); }; } // namespace sot diff --git a/include/sot/core/derivator-impl.hh b/include/sot/core/derivator-impl.hh index ba354eefa..616b4cd74 100644 --- a/include/sot/core/derivator-impl.hh +++ b/include/sot/core/derivator-impl.hh @@ -33,6 +33,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; #ifdef WIN32 #define DECLARE_SPECIFICATION(className, sotSigType) \ @@ -46,8 +47,8 @@ namespace sot { #endif DECLARE_SPECIFICATION(DerivatorDouble, double) -DECLARE_SPECIFICATION(DerivatorVector, dynamicgraph::Vector) -DECLARE_SPECIFICATION(DerivatorMatrix, dynamicgraph::Matrix) +DECLARE_SPECIFICATION(DerivatorVector, dg::Vector) +DECLARE_SPECIFICATION(DerivatorMatrix, dg::Matrix) DECLARE_SPECIFICATION(DerivatorVectorQuaternion, VectorQuaternion) } /* namespace sot */ } /* namespace dynamicgraph */ diff --git a/include/sot/core/derivator.hh b/include/sot/core/derivator.hh index 82a7413ad..883c141a1 100644 --- a/include/sot/core/derivator.hh +++ b/include/sot/core/derivator.hh @@ -29,12 +29,13 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -template class Derivator : public dynamicgraph::Entity { +template class Derivator : public dg::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); protected: @@ -47,7 +48,7 @@ public: /* --- CONSTRUCTION --- */ static std::string getTypeName(void) { return "Unknown"; } Derivator(const std::string &name) - : dynamicgraph::Entity(name), memory(), initialized(false), + : dg::Entity(name), memory(), initialized(false), timestep(TIMESTEP_DEFAULT), SIN(NULL, "sotDerivator<" + getTypeName() + ">(" + name + ")::input(" + getTypeName() + ")::sin"), @@ -64,9 +65,9 @@ public: /* --- CONSTRUCTION --- */ virtual ~Derivator(void){}; public: /* --- SIGNAL --- */ - dynamicgraph::SignalPtr SIN; - dynamicgraph::SignalTimeDependent SOUT; - dynamicgraph::Signal timestepSIN; + dg::SignalPtr SIN; + dg::SignalTimeDependent SOUT; + dg::Signal timestepSIN; protected: T &computeDerivation(T &res, int time) { diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 9cb5c266c..4ff081192 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -17,7 +17,7 @@ /* -- MaaL --- */ #include - +namespace dg = dynamicgraph; /* SOT */ #include "sot/core/api.hh" #include "sot/core/periodic-call.hh" @@ -55,10 +55,10 @@ public: }; protected: - dynamicgraph::Vector state_; - dynamicgraph::Vector velocity_; + dg::Vector state_; + dg::Vector velocity_; bool sanityCheck_; - dynamicgraph::Vector vel_control_; + dg::Vector vel_control_; ControlInput controlInputType_; bool withForceSignals[4]; PeriodicCall periodicCallBefore_; @@ -81,9 +81,9 @@ public: virtual ~Device(); virtual void setStateSize(const unsigned int &size); - virtual void setState(const dynamicgraph::Vector &st); + virtual void setState(const dg::Vector &st); void setVelocitySize(const unsigned int &size); - virtual void setVelocity(const dynamicgraph::Vector &vel); + virtual void setVelocity(const dg::Vector &vel); virtual void setSecondOrderIntegration(); virtual void setNoIntegration(); virtual void setControlInputType(const std::string &cit); @@ -107,20 +107,20 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ - dynamicgraph::SignalPtr controlSIN; - dynamicgraph::SignalPtr attitudeSIN; - dynamicgraph::SignalPtr zmpSIN; + dynamicgraph::SignalPtr controlSIN; + dynamicgraph::SignalPtr attitudeSIN; + dynamicgraph::SignalPtr zmpSIN; /// \name Device current state. /// \{ - dynamicgraph::Signal stateSOUT; - dynamicgraph::Signal velocitySOUT; + dynamicgraph::Signal stateSOUT; + dynamicgraph::Signal velocitySOUT; dynamicgraph::Signal attitudeSOUT; /*! \brief The current state of the robot from the command viewpoint. */ - dynamicgraph::Signal motorcontrolSOUT; - dynamicgraph::Signal previousControlSOUT; + dynamicgraph::Signal motorcontrolSOUT; + dynamicgraph::Signal previousControlSOUT; /*! \brief The ZMP reference send by the previous controller. */ - dynamicgraph::Signal ZMPPreviousControllerSOUT; + dynamicgraph::Signal ZMPPreviousControllerSOUT; /// \} /// \name Real robot current state @@ -129,19 +129,19 @@ public: /* --- SIGNALS --- */ /// does *not* match the state control input signal. /// \{ /// Motor positions - dynamicgraph::Signal robotState_; + dynamicgraph::Signal robotState_; /// Motor velocities - dynamicgraph::Signal robotVelocity_; + dynamicgraph::Signal robotVelocity_; /// The force torque sensors - dynamicgraph::Signal *forcesSOUT[4]; + dynamicgraph::Signal *forcesSOUT[4]; /// Motor torques /// \todo why pseudo ? - dynamicgraph::Signal pseudoTorqueSOUT; + dynamicgraph::Signal pseudoTorqueSOUT; /// \} protected: /// Compute roll pitch yaw angles of freeflyer joint. - void integrateRollPitchYaw(dynamicgraph::Vector &state, const dynamicgraph::Vector &control, + void integrateRollPitchYaw(dg::Vector &state, const dg::Vector &control, double dt); /// Store Position of free flyer joint MatrixHomogeneous ffPose_; @@ -164,13 +164,13 @@ protected: const MatrixHomogeneous &freeFlyerPose() const; public: - virtual void setRoot(const dynamicgraph::Matrix &root); + virtual void setRoot(const dg::Matrix &root); virtual void setRoot(const MatrixHomogeneous &worldMwaist); private: // Intermediate variable to avoid dynamic allocation - dynamicgraph::Vector forceZero6; + dg::Vector forceZero6; }; } // namespace sot } // namespace dynamicgraph diff --git a/include/sot/core/exp-moving-avg.hh b/include/sot/core/exp-moving-avg.hh index 2ed1d4252..6857cc119 100644 --- a/include/sot/core/exp-moving-avg.hh +++ b/include/sot/core/exp-moving-avg.hh @@ -18,6 +18,7 @@ #include #include +namespace dg = ::dynamicgraph; namespace dynamicgraph { namespace sot { @@ -34,9 +35,9 @@ class SOT_CORE_DLLAPI ExpMovingAvg : public Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: - SignalPtr updateSIN; + SignalPtr updateSIN; SignalTimeDependent refresherSINTERN; - SignalTimeDependent averageSOUT; + SignalTimeDependent averageSOUT; public: ExpMovingAvg(const std::string &n); @@ -45,9 +46,9 @@ public: void setAlpha(const double &alpha_); protected: - dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime); + dg::Vector &update(dg::Vector &res, const int &inTime); - dynamicgraph::Vector average; + dg::Vector average; double alpha; bool init; diff --git a/include/sot/core/feature-1d.hh b/include/sot/core/feature-1d.hh index 23705f190..9eef46551 100644 --- a/include/sot/core/feature-1d.hh +++ b/include/sot/core/feature-1d.hh @@ -38,6 +38,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class Feature1D \brief Simple test: the task is defined to be e_2 = .5 . e'.e, with @@ -64,10 +65,10 @@ public: @{ */ /*! \brief Input for the error. */ - dynamicgraph::SignalPtr errorSIN; + dg::SignalPtr errorSIN; /*! \brief Input for the Jacobian. */ - dynamicgraph::SignalPtr jacobianSIN; + dg::SignalPtr jacobianSIN; /*! @} */ @@ -98,10 +99,10 @@ public: /*! \brief Compute the error between the desired value and the value itself. */ - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); /*! \brief Compute the Jacobian of the value according to the robot state.. */ - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); /*! @} */ diff --git a/include/sot/core/feature-joint-limits.hh b/include/sot/core/feature-joint-limits.hh index 8a11cb1ed..fbb67c1a5 100644 --- a/include/sot/core/feature-joint-limits.hh +++ b/include/sot/core/feature-joint-limits.hh @@ -38,6 +38,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeatureJointLimits @@ -61,10 +62,10 @@ protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr jointSIN; - dynamicgraph::SignalPtr upperJlSIN; - dynamicgraph::SignalPtr lowerJlSIN; - dynamicgraph::SignalTimeDependent widthJlSINTERN; + dg::SignalPtr jointSIN; + dg::SignalPtr upperJlSIN; + dg::SignalPtr lowerJlSIN; + dg::SignalTimeDependent widthJlSINTERN; using FeatureAbstract::selectionSIN; @@ -83,9 +84,9 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); - dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res, const int &time); + virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + dg::Vector &computeWidthJl(dg::Vector &res, const int &time); /** Static Feature selection. */ inline static Flags selectActuated(void); diff --git a/include/sot/core/feature-line-distance.hh b/include/sot/core/feature-line-distance.hh index c618cd9e9..37034c4a5 100644 --- a/include/sot/core/feature-line-distance.hh +++ b/include/sot/core/feature-line-distance.hh @@ -38,6 +38,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeatureLineDistance @@ -53,11 +54,11 @@ public: protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr positionSIN; - dynamicgraph::SignalPtr articularJacobianSIN; - dynamicgraph::SignalPtr positionRefSIN; - dynamicgraph::SignalPtr vectorSIN; - dynamicgraph::SignalTimeDependent lineSOUT; + dg::SignalPtr positionSIN; + dg::SignalPtr articularJacobianSIN; + dg::SignalPtr positionRefSIN; + dg::SignalPtr vectorSIN; + dg::SignalTimeDependent lineSOUT; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -75,9 +76,9 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); - dynamicgraph::Vector &computeLineCoordinates(dynamicgraph::Vector &cood, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); + dg::Vector &computeLineCoordinates(dg::Vector &cood, int time); virtual void display(std::ostream &os) const; }; diff --git a/include/sot/core/feature-point6d-relative.hh b/include/sot/core/feature-point6d-relative.hh index e1d073070..9cf4afa83 100644 --- a/include/sot/core/feature-point6d-relative.hh +++ b/include/sot/core/feature-point6d-relative.hh @@ -40,6 +40,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeaturePoint6dRelative @@ -55,23 +56,23 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - dynamicgraph::Matrix L; + dg::Matrix L; /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr positionReferenceSIN; - dynamicgraph::SignalPtr articularJacobianReferenceSIN; + dg::SignalPtr positionReferenceSIN; + dg::SignalPtr articularJacobianReferenceSIN; - /*! dynamicgraph::Signals related to the computation of the derivative of + /*! dg::Signals related to the computation of the derivative of the error @{ */ - /*! dynamicgraph::Signals giving the derivative of the input signals. + /*! dg::Signals giving the derivative of the input signals. @{*/ /*! Derivative of the relative position. */ - dynamicgraph::SignalPtr dotpositionSIN; + dg::SignalPtr dotpositionSIN; /*! Derivative of the reference position. */ - dynamicgraph::SignalPtr dotpositionReferenceSIN; + dg::SignalPtr dotpositionReferenceSIN; /*! @} */ using FeaturePoint6d::getReference; @@ -80,9 +81,9 @@ public: FeaturePoint6dRelative(const std::string &name); virtual ~FeaturePoint6dRelative(void) {} - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); virtual void display(std::ostream &os) const; diff --git a/include/sot/core/feature-point6d.hh b/include/sot/core/feature-point6d.hh index e2624f071..97b39443e 100644 --- a/include/sot/core/feature-point6d.hh +++ b/include/sot/core/feature-point6d.hh @@ -40,6 +40,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeaturePoint6d @@ -70,9 +71,9 @@ private: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr positionSIN; - dynamicgraph::SignalPtr velocitySIN; - dynamicgraph::SignalPtr articularJacobianSIN; + dg::SignalPtr positionSIN; + dg::SignalPtr velocitySIN; + dg::SignalPtr articularJacobianSIN; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -89,9 +90,9 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dg::Vector &computeErrordot(dg::Vector &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); /** Static Feature selection. */ inline static Flags selectX(void) { return FLAG_LINE_1; } diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index a55d84208..6ca40ac71 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -24,6 +24,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /// Enum used to specify what difference operation is used in FeaturePose. enum Representation_t { SE3Representation, R3xSO3Representation }; @@ -70,23 +71,23 @@ public: @{ */ /// Input pose of Joint A wrt to world frame. - dynamicgraph::SignalPtr oMja; + dg::SignalPtr oMja; /// Input pose of Frame A wrt to Joint A. - dynamicgraph::SignalPtr jaMfa; + dg::SignalPtr jaMfa; /// Input pose of Joint B wrt to world frame. - dynamicgraph::SignalPtr oMjb; + dg::SignalPtr oMjb; /// Input pose of Frame B wrt to Joint B. - dynamicgraph::SignalPtr jbMfb; + dg::SignalPtr jbMfb; /// Jacobian of the input Joint A, expressed in Joint A - dynamicgraph::SignalPtr jaJja; + dg::SignalPtr jaJja; /// Jacobian of the input Joint B, expressed in Joint B - dynamicgraph::SignalPtr jbJjb; + dg::SignalPtr jbJjb; /// The desired pose of Frame B wrt to Frame A. - dynamicgraph::SignalPtr faMfbDes; + dg::SignalPtr faMfbDes; /// The desired velocity of Frame B wrt to Frame A. The /// value is expressed in Frame A. - dynamicgraph::SignalPtr faNufafbDes; + dg::SignalPtr faNufafbDes; /*! @} */ /*! \name Output signals @@ -120,21 +121,21 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); /// Computes \f$ {}^oM^{-1}_{fa} {}^oM_{fb} \ominus {}^{fa}M^*_{fb} \f$ - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); /// Computes \f$ \frac{\partial\ominus}{\partial b} X {}^{fa}\nu^*_{fafb} \f$. /// There are two different cases, depending on the representation: /// - R3xSO3Representation: \f$ X = \left( \begin{array}{cc} I_3 & [ /// {}^{fa}t_{fb} ] \\ 0_3 & {{}^{fa}R^*_{fb}}^T \end{array} \right) \f$ /// - SE3Representation: \f$ X = {{}^{fa}X^*_{fb}}^{-1} \f$ (see /// pinocchio::SE3Base::toActionMatrix) - virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); + virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); /// Computes \f$ \frac{\partial\ominus}{\partial b} Y \left( {{}^{fb}X_{jb}} /// {}^{jb}J_{jb} - {{}^{fb}X_{ja}} {}^{ja}J_{ja} \right) \f$. There are two /// different cases, depending on the representation: /// - R3xSO3Representation: \f$ Y = \left( \begin{array}{cc} {{}^{fa}R_{fb}} & /// 0_3 \\ 0_3 & I_3 \end{array} \right) \f$ /// - SE3Representation: \f$ Y = I_6 \f$ - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); /** Static Feature selection. */ inline static Flags selectX(void) { return FLAG_LINE_1; } diff --git a/include/sot/core/feature-task.hh b/include/sot/core/feature-task.hh index d1f33f40a..66df85ac3 100644 --- a/include/sot/core/feature-task.hh +++ b/include/sot/core/feature-task.hh @@ -39,7 +39,7 @@ namespace dynamicgraph { namespace sot { - +namespace dg = dynamicgraph; class SOTFEATURETASK_EXPORT FeatureTask : public FeatureGeneric { diff --git a/include/sot/core/feature-vector3.hh b/include/sot/core/feature-vector3.hh index 43402f531..d5d42ebae 100644 --- a/include/sot/core/feature-vector3.hh +++ b/include/sot/core/feature-vector3.hh @@ -39,6 +39,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeatureVector3 @@ -55,10 +56,10 @@ public: protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr vectorSIN; - dynamicgraph::SignalPtr positionSIN; - dynamicgraph::SignalPtr articularJacobianSIN; - dynamicgraph::SignalPtr positionRefSIN; + dg::SignalPtr vectorSIN; + dg::SignalPtr positionSIN; + dg::SignalPtr articularJacobianSIN; + dg::SignalPtr positionRefSIN; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -70,8 +71,8 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); virtual void display(std::ostream &os) const; }; diff --git a/include/sot/core/feature-visual-point.hh b/include/sot/core/feature-visual-point.hh index ffdc37c5f..7bebd3d2e 100644 --- a/include/sot/core/feature-visual-point.hh +++ b/include/sot/core/feature-visual-point.hh @@ -38,6 +38,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeatureVisualPoint @@ -52,15 +53,15 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - dynamicgraph::Matrix L; + dg::Matrix L; /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr xySIN; + dg::SignalPtr xySIN; /** FeatureVisualPoint depth (required to compute the interaction matrix) * default Z = 1m. */ - dynamicgraph::SignalPtr ZSIN; - dynamicgraph::SignalPtr articularJacobianSIN; + dg::SignalPtr ZSIN; + dg::SignalPtr articularJacobianSIN; using FeatureAbstract::errorSOUT; using FeatureAbstract::jacobianSOUT; @@ -74,8 +75,8 @@ public: virtual unsigned int &getDimension(unsigned int &dim, int time); - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); /** Static Feature selection. */ inline static Flags selectX(void) { return FLAG_LINE_1; } diff --git a/include/sot/core/fir-filter.hh b/include/sot/core/fir-filter.hh index 3e563b36e..b543dbdd8 100644 --- a/include/sot/core/fir-filter.hh +++ b/include/sot/core/fir-filter.hh @@ -21,6 +21,7 @@ #include #include +namespace dg = dynamicgraph; namespace dynamicgraph { namespace sot { diff --git a/include/sot/core/gain-hyperbolic.hh b/include/sot/core/gain-hyperbolic.hh index abba25c40..3694eaef8 100644 --- a/include/sot/core/gain-hyperbolic.hh +++ b/include/sot/core/gain-hyperbolic.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -41,6 +42,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /** \brief Hyperbolic gain. * It follows the law \f[ g(e) = a \frac{\tanh(-b(||e|| - d)) + 1}{2} + c \f] @@ -50,7 +52,7 @@ namespace sot { * - \f$ c = 0.1 \f$, * - \f$ d = 0 \f$. */ -class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dynamicgraph::Entity { +class SOTGAINHYPERBOLIC_EXPORT GainHyperbolic : public dg::Entity { public: /* --- CONSTANTS --- */ /* Default values. */ @@ -92,8 +94,8 @@ public: /* --- INIT --- */ void forceConstant(void); public: /* --- SIGNALS --- */ - dynamicgraph::SignalPtr errorSIN; - dynamicgraph::SignalTimeDependent gainSOUT; + dg::SignalPtr errorSIN; + dg::SignalTimeDependent gainSOUT; protected: double &computeGain(double &res, int t); diff --git a/include/sot/core/gradient-ascent.hh b/include/sot/core/gradient-ascent.hh index 8eff01ba5..fba98c76d 100644 --- a/include/sot/core/gradient-ascent.hh +++ b/include/sot/core/gradient-ascent.hh @@ -18,6 +18,7 @@ #include #include +namespace dg = ::dynamicgraph; namespace dynamicgraph { namespace sot { @@ -34,19 +35,19 @@ class SOT_CORE_DLLAPI GradientAscent : public Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: - SignalPtr gradientSIN; + SignalPtr gradientSIN; SignalPtr learningRateSIN; SignalTimeDependent refresherSINTERN; - SignalTimeDependent valueSOUT; + SignalTimeDependent valueSOUT; public: GradientAscent(const std::string &n); virtual ~GradientAscent(void); protected: - dynamicgraph::Vector &update(dynamicgraph::Vector &res, const int &inTime); + dg::Vector &update(dg::Vector &res, const int &inTime); - dynamicgraph::Vector value; + dg::Vector value; double alpha; bool init; diff --git a/include/sot/core/gripper-control.hh b/include/sot/core/gripper-control.hh index 61189295a..367396415 100644 --- a/include/sot/core/gripper-control.hh +++ b/include/sot/core/gripper-control.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -46,6 +47,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \brief The goal of this entity is to ensure that the maximal torque will not * be exceeded during a grasping task. @@ -58,7 +60,7 @@ protected: double offset; static const double OFFSET_DEFAULT; //! \brief The multiplication - dynamicgraph::Vector factor; + dg::Vector factor; public: GripperControl(void); @@ -66,30 +68,30 @@ public: //! \brief Computes the // if the torque limit is reached, the normalized position is reduced by // (offset) - void computeIncrement(const dynamicgraph::Vector &torques, - const dynamicgraph::Vector &torqueLimits, - const dynamicgraph::Vector ¤tNormVel); + void computeIncrement(const dg::Vector &torques, + const dg::Vector &torqueLimits, + const dg::Vector ¤tNormVel); //! \brief - dynamicgraph::Vector &computeDesiredPosition(const dynamicgraph::Vector ¤tPos, - const dynamicgraph::Vector &desiredPos, - const dynamicgraph::Vector &torques, - const dynamicgraph::Vector &torqueLimits, - dynamicgraph::Vector &referencePos); + dg::Vector &computeDesiredPosition(const dg::Vector ¤tPos, + const dg::Vector &desiredPos, + const dg::Vector &torques, + const dg::Vector &torqueLimits, + dg::Vector &referencePos); /*! \brief select only some of the values of the vector fullsize, * based on the Flags vector. */ - static dynamicgraph::Vector &selector(const dynamicgraph::Vector &fullsize, const Flags &selec, - dynamicgraph::Vector &desPos); + static dg::Vector &selector(const dg::Vector &fullsize, const Flags &selec, + dg::Vector &desPos); }; /* --------------------------------------------------------------------- */ /* --- PLUGIN ---------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin : public dynamicgraph::Entity, +class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin : public dg::Entity, public GripperControl { DYNAMIC_GRAPH_ENTITY_DECL(); @@ -105,22 +107,22 @@ public: /* --- CONSTRUCTION --- */ public: /* --- SIGNAL --- */ /* --- INPUTS --- */ - dynamicgraph::SignalPtr positionSIN; - dynamicgraph::SignalPtr positionDesSIN; - dynamicgraph::SignalPtr torqueSIN; - dynamicgraph::SignalPtr torqueLimitSIN; - dynamicgraph::SignalPtr selectionSIN; + dg::SignalPtr positionSIN; + dg::SignalPtr positionDesSIN; + dg::SignalPtr torqueSIN; + dg::SignalPtr torqueLimitSIN; + dg::SignalPtr selectionSIN; /* --- INTERMEDIARY --- */ - dynamicgraph::SignalPtr positionFullSizeSIN; - dynamicgraph::SignalTimeDependent positionReduceSOUT; - dynamicgraph::SignalPtr torqueFullSizeSIN; - dynamicgraph::SignalTimeDependent torqueReduceSOUT; - dynamicgraph::SignalPtr torqueLimitFullSizeSIN; - dynamicgraph::SignalTimeDependent torqueLimitReduceSOUT; + dg::SignalPtr positionFullSizeSIN; + dg::SignalTimeDependent positionReduceSOUT; + dg::SignalPtr torqueFullSizeSIN; + dg::SignalTimeDependent torqueReduceSOUT; + dg::SignalPtr torqueLimitFullSizeSIN; + dg::SignalTimeDependent torqueLimitReduceSOUT; /* --- OUTPUTS --- */ - dynamicgraph::SignalTimeDependent desiredPositionSOUT; + dg::SignalTimeDependent desiredPositionSOUT; public: /* --- COMMANDLINE --- */ void initCommands(); diff --git a/include/sot/core/integrator-abstract-impl.hh b/include/sot/core/integrator-abstract-impl.hh index 25df9ee43..2f4b15bbd 100644 --- a/include/sot/core/integrator-abstract-impl.hh +++ b/include/sot/core/integrator-abstract-impl.hh @@ -46,7 +46,7 @@ namespace dynamicgraph { namespace sot { DECLARE_SPECIFICATION(IntegratorAbstractDouble, double, double) -DECLARE_SPECIFICATION(IntegratorAbstractVector, dynamicgraph::Vector, dynamicgraph::Matrix) +DECLARE_SPECIFICATION(IntegratorAbstractVector, dg::Vector, dg::Matrix) } // namespace sot } // namespace dynamicgraph #endif // #ifndef __SOT_MAILBOX_HH diff --git a/include/sot/core/integrator-abstract.hh b/include/sot/core/integrator-abstract.hh index 9dc9e35c1..b62a63211 100644 --- a/include/sot/core/integrator-abstract.hh +++ b/include/sot/core/integrator-abstract.hh @@ -34,6 +34,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \brief integrates an ODE. If Y is the output and X the input, the * following equation is integrated: @@ -42,20 +43,20 @@ namespace sot { * function between X and Y, while the b_i are those of the numerator. */ template -class IntegratorAbstract : public dynamicgraph::Entity { +class IntegratorAbstract : public dg::Entity { public: IntegratorAbstract(const std::string &name) - : dynamicgraph::Entity(name), + : dg::Entity(name), SIN(NULL, "sotIntegratorAbstract(" + name + ")::input(vector)::sin"), SOUT(boost::bind(&IntegratorAbstract::integrate, this, _1, _2), SIN, "sotIntegratorAbstract(" + name + ")::output(vector)::sout") { signalRegistration(SIN << SOUT); - using namespace dynamicgraph::command; + using namespace dg::command; const std::string typeName = - Value::typeName(dynamicgraph::command::ValueHelper::TypeID); + Value::typeName(dg::command::ValueHelper::TypeID); addCommand( "pushNumCoef", @@ -91,9 +92,9 @@ public: void popDenomCoef() { denominator.pop_back(); } public: - dynamicgraph::SignalPtr SIN; + dg::SignalPtr SIN; - dynamicgraph::SignalTimeDependent SOUT; + dg::SignalTimeDependent SOUT; protected: std::vector numerator; diff --git a/include/sot/core/integrator-euler.hh b/include/sot/core/integrator-euler.hh index e808c9b31..97458eaa6 100644 --- a/include/sot/core/integrator-euler.hh +++ b/include/sot/core/integrator-euler.hh @@ -25,6 +25,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; namespace internal { template bool integratorEulerCoeffIsIdentity(const coefT c) { @@ -69,7 +70,7 @@ public: ")::output(vector)::derivativesout") { this->signalRegistration(derivativeSOUT); - using namespace dynamicgraph::command; + using namespace dg::command; setSamplingPeriod(0.005); @@ -96,7 +97,7 @@ protected: std::vector inputMemory; std::vector outputMemory; - dynamicgraph::SignalTimeDependent derivativeSOUT; + dg::SignalTimeDependent derivativeSOUT; double dt; double invdt; @@ -151,7 +152,7 @@ public: sigT &derivative(sigT &res, int time) { if (outputMemory.size() < 2) - throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC, + throw dg::ExceptionSignal(dg::ExceptionSignal::GENERIC, "Integrator does not compute the derivative."); SOUT.recompute(time); @@ -183,8 +184,8 @@ public: // Check that denominator.back is the identity if (!internal::integratorEulerCoeffIsIdentity(denominator.back())) - throw dynamicgraph::ExceptionSignal( - dynamicgraph::ExceptionSignal::GENERIC, + throw dg::ExceptionSignal( + dg::ExceptionSignal::GENERIC, "The coefficient of the highest order derivative of denominator " "should be 1 (the last pushDenomCoef should be the identity)."); } diff --git a/include/sot/core/joint-limitator.hh b/include/sot/core/joint-limitator.hh index e3cce3244..af4d2617c 100644 --- a/include/sot/core/joint-limitator.hh +++ b/include/sot/core/joint-limitator.hh @@ -11,6 +11,7 @@ #define SOT_FEATURE_JOINTLIMITS_HH // Matrix #include +namespace dg = dynamicgraph; // SOT #include @@ -29,31 +30,32 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /// \brief Filter control vector to avoid exceeding joint maximum values. /// /// This must be plugged between the entity producing the command /// (i.e. usually the sot) and the entity executing it (the device). -class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dynamicgraph::Entity { +class SOTJOINTLIMITATOR_EXPORT JointLimitator : public dg::Entity { DYNAMIC_GRAPH_ENTITY_DECL(); public: JointLimitator(const std::string &name); virtual ~JointLimitator() {} - virtual dynamicgraph::Vector &computeControl(dynamicgraph::Vector &res, int time); - dynamicgraph::Vector &computeWidthJl(dynamicgraph::Vector &res, const int &time); + virtual dg::Vector &computeControl(dg::Vector &res, int time); + dg::Vector &computeWidthJl(dg::Vector &res, const int &time); virtual void display(std::ostream &os) const; /// \name Signals /// \{ - dynamicgraph::SignalPtr jointSIN; - dynamicgraph::SignalPtr upperJlSIN; - dynamicgraph::SignalPtr lowerJlSIN; - dynamicgraph::SignalPtr controlSIN; - dynamicgraph::SignalTimeDependent controlSOUT; - dynamicgraph::SignalTimeDependent widthJlSINTERN; + dg::SignalPtr jointSIN; + dg::SignalPtr upperJlSIN; + dg::SignalPtr lowerJlSIN; + dg::SignalPtr controlSIN; + dg::SignalTimeDependent controlSOUT; + dg::SignalTimeDependent widthJlSINTERN; /// \} }; } // end of namespace sot. diff --git a/include/sot/core/joint-trajectory-entity.hh b/include/sot/core/joint-trajectory-entity.hh index 91a7aa755..e435e495e 100644 --- a/include/sot/core/joint-trajectory-entity.hh +++ b/include/sot/core/joint-trajectory-entity.hh @@ -15,7 +15,7 @@ // Maal #include - +namespace dg = dynamicgraph; // SOT #include #include @@ -57,13 +57,13 @@ public: void loadFile(const std::string &name); /// \brief Return the next pose for the legs. - dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos, const int &time); + dg::Vector &getNextPosition(dg::Vector &pos, const int &time); /// \brief Return the next com. - dynamicgraph::Vector &getNextCoM(dynamicgraph::Vector &com, const int &time); + dg::Vector &getNextCoM(dg::Vector &com, const int &time); /// \brief Return the next cop. - dynamicgraph::Vector &getNextCoP(dynamicgraph::Vector &cop, const int &time); + dg::Vector &getNextCoP(dg::Vector &cop, const int &time); /// \brief Return the next waist. sot::MatrixHomogeneous &getNextWaist(sot::MatrixHomogeneous &waist, @@ -74,7 +74,7 @@ public: /// \brief Convert a xyztheta vector into an homogeneous matrix sot::MatrixHomogeneous - XYZThetaToMatrixHomogeneous(const dynamicgraph::Vector &xyztheta); + XYZThetaToMatrixHomogeneous(const dg::Vector &xyztheta); /// \brief Perform one update of the signals. int &OneStepOfUpdate(int &dummy, const int &time); @@ -102,13 +102,13 @@ public: SignalTimeDependent OneStepOfUpdateS; /// \brief Publish pose for each evaluation of the graph. - dynamicgraph::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; /// \brief Publish com for each evaluation of the graph. - dynamicgraph::SignalTimeDependent comSOUT; + dynamicgraph::SignalTimeDependent comSOUT; /// \brief Publish zmp for each evaluation of the graph. - dynamicgraph::SignalTimeDependent zmpSOUT; + dynamicgraph::SignalTimeDependent zmpSOUT; /// \brief Publish waist for each evaluation of the graph. dynamicgraph::SignalTimeDependent waistSOUT; @@ -128,13 +128,13 @@ protected: timestamp traj_timestamp_; /// \brief Store the pos; - dynamicgraph::Vector pose_; + dg::Vector pose_; /// \brief Store the center of mass. - dynamicgraph::Vector com_; + dg::Vector com_; /// \brief Store the center of pressure ZMP. - dynamicgraph::Vector cop_; + dg::Vector cop_; /// \brief Store the waist position. sot::MatrixHomogeneous waist_; diff --git a/include/sot/core/mailbox.hh b/include/sot/core/mailbox.hh index 0aab925af..d28b4b2b8 100644 --- a/include/sot/core/mailbox.hh +++ b/include/sot/core/mailbox.hh @@ -31,8 +31,9 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; -template class Mailbox : public dynamicgraph::Entity { +template class Mailbox : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -62,9 +63,9 @@ protected: bool update; public: /* --- SIGNALS --- */ - dynamicgraph::SignalTimeDependent SOUT; - dynamicgraph::SignalTimeDependent objSOUT; - dynamicgraph::SignalTimeDependent timeSOUT; + dg::SignalTimeDependent SOUT; + dg::SignalTimeDependent objSOUT; + dg::SignalTimeDependent timeSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/mailbox.hxx b/include/sot/core/mailbox.hxx index a34b4b77d..747575852 100644 --- a/include/sot/core/mailbox.hxx +++ b/include/sot/core/mailbox.hxx @@ -15,6 +15,8 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; + /* -------------------------------------------------------------------------- */ /* --- CONSTRUCTION --------------------------------------------------------- */ /* -------------------------------------------------------------------------- */ diff --git a/include/sot/core/matrix-constant.hh b/include/sot/core/matrix-constant.hh index f8fc26be3..29ba4eaab 100644 --- a/include/sot/core/matrix-constant.hh +++ b/include/sot/core/matrix-constant.hh @@ -12,6 +12,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- MATRIX ---------------------------------------------------------- */ @@ -35,14 +36,14 @@ public: int rows, cols; double color; - void setValue(const dynamicgraph::Matrix &inValue); + void setValue(const dg::Matrix &inValue); public: MatrixConstant(const std::string &name); virtual ~MatrixConstant(void) {} - SignalTimeDependent SOUT; + SignalTimeDependent SOUT; }; } // namespace sot diff --git a/include/sot/core/memory-task-sot.hh b/include/sot/core/memory-task-sot.hh index 3fcb0f22e..f88b5a39a 100644 --- a/include/sot/core/memory-task-sot.hh +++ b/include/sot/core/memory-task-sot.hh @@ -20,6 +20,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; class SOT_CORE_EXPORT MemoryTaskSOT : public TaskAbstract::MemoryTaskAbstract { public: // protected: @@ -29,10 +30,10 @@ public: // protected: KernelConst_t; /* Internal memory to reduce the dynamic allocation at task resolution. */ - dynamicgraph::Vector err, tmpTask, tmpVar, tmpControl; - dynamicgraph::Matrix Jt; //( nJ,mJ ); + dg::Vector err, tmpTask, tmpVar, tmpControl; + dg::Matrix Jt; //( nJ,mJ ); - dynamicgraph::Matrix JK; //(nJ,mJ); + dg::Matrix JK; //(nJ,mJ); SVD_t svd; Kernel_t kernel; diff --git a/include/sot/core/motion-period.hh b/include/sot/core/motion-period.hh index 25ff16cd2..b2fc894de 100644 --- a/include/sot/core/motion-period.hh +++ b/include/sot/core/motion-period.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -46,8 +47,9 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; -class SOTMOTIONPERIOD_EXPORT MotionPeriod : public dynamicgraph::Entity { +class SOTMOTIONPERIOD_EXPORT MotionPeriod : public dg::Entity { public: static const std::string CLASS_NAME; @@ -71,13 +73,13 @@ protected: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalTimeDependent motionSOUT; + dg::SignalTimeDependent motionSOUT; public: MotionPeriod(const std::string &name); virtual ~MotionPeriod(void) {} - dynamicgraph::Vector &computeMotion(dynamicgraph::Vector &res, const int &time); + dg::Vector &computeMotion(dg::Vector &res, const int &time); virtual void display(std::ostream &os) const; }; diff --git a/include/sot/core/neck-limitation.hh b/include/sot/core/neck-limitation.hh index 23c593229..0a3b80eb1 100644 --- a/include/sot/core/neck-limitation.hh +++ b/include/sot/core/neck-limitation.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -48,8 +49,9 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; -class NeckLimitation_EXPORT NeckLimitation : public dynamicgraph::Entity { +class NeckLimitation_EXPORT NeckLimitation : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -72,11 +74,11 @@ public: /* --- CONSTRUCTION --- */ virtual ~NeckLimitation(void); public: /* --- SIGNAL --- */ - dynamicgraph::SignalPtr jointSIN; - dynamicgraph::SignalTimeDependent jointSOUT; + dg::SignalPtr jointSIN; + dg::SignalTimeDependent jointSOUT; public: /* --- FUNCTIONS --- */ - dynamicgraph::Vector &computeJointLimitation(dynamicgraph::Vector &jointLimited, + dg::Vector &computeJointLimitation(dg::Vector &jointLimited, const int &timeSpec); public: /* --- PARAMS --- */ diff --git a/include/sot/core/op-point-modifier.hh b/include/sot/core/op-point-modifier.hh index 809c0dfd3..4b442acf6 100644 --- a/include/sot/core/op-point-modifier.hh +++ b/include/sot/core/op-point-modifier.hh @@ -17,6 +17,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- API ------------------------------------------------------------- */ @@ -38,6 +39,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /// /// \brief Compute position and jacobian of a local frame attached to a joint. @@ -45,23 +47,23 @@ namespace sot { /// The position of the local frame in the frame of the joint is represented by /// transformation. /// -class SOTOPPOINTMODIFIER_EXPORT OpPointModifier : public dynamicgraph::Entity { +class SOTOPPOINTMODIFIER_EXPORT OpPointModifier : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } public: - dynamicgraph::SignalPtr jacobianSIN; - dynamicgraph::SignalPtr positionSIN; + dg::SignalPtr jacobianSIN; + dg::SignalPtr positionSIN; - dynamicgraph::SignalTimeDependent jacobianSOUT; - dynamicgraph::SignalTimeDependent positionSOUT; + dg::SignalTimeDependent jacobianSOUT; + dg::SignalTimeDependent positionSOUT; public: OpPointModifier(const std::string &name); virtual ~OpPointModifier(void) {} - dynamicgraph::Matrix &jacobianSOUT_function(dynamicgraph::Matrix &res, const int &time); + dg::Matrix &jacobianSOUT_function(dg::Matrix &res, const int &time); MatrixHomogeneous &positionSOUT_function(MatrixHomogeneous &res, const int &time); void setTransformation(const Eigen::Matrix4d &tr); diff --git a/include/sot/core/reader.hh b/include/sot/core/reader.hh index 7c216aea5..36f2c4d5a 100644 --- a/include/sot/core/reader.hh +++ b/include/sot/core/reader.hh @@ -16,7 +16,7 @@ /* Matrix */ #include - +namespace dg = dynamicgraph; /* STD */ #include @@ -61,8 +61,8 @@ class SOTREADER_EXPORT sotReader : public Entity { public: SignalPtr selectionSIN; - SignalTimeDependent vectorSOUT; - SignalTimeDependent matrixSOUT; + SignalTimeDependent vectorSOUT; + SignalTimeDependent matrixSOUT; public: sotReader(const std::string n); @@ -80,8 +80,8 @@ protected: int rows, cols; - dynamicgraph::Vector &getNextData(dynamicgraph::Vector &res, const unsigned int time); - dynamicgraph::Matrix &getNextMatrix(dynamicgraph::Matrix &res, const unsigned int time); + dg::Vector &getNextData(dg::Vector &res, const unsigned int time); + dg::Matrix &getNextMatrix(dg::Matrix &res, const unsigned int time); void resize(const int &nbRow, const int &nbCol); public: diff --git a/include/sot/core/robot-simu.hh b/include/sot/core/robot-simu.hh index 50d232d0d..e37c83826 100644 --- a/include/sot/core/robot-simu.hh +++ b/include/sot/core/robot-simu.hh @@ -17,7 +17,7 @@ /* -- MaaL --- */ #include - +namespace dg = dynamicgraph; /* SOT */ #include "sot/core/api.hh" #include "sot/core/device.hh" diff --git a/src/feature/feature-point6d-relative.cpp b/src/feature/feature-point6d-relative.cpp index 950eb7bc2..e6ab8179b 100644 --- a/src/feature/feature-point6d-relative.cpp +++ b/src/feature/feature-point6d-relative.cpp @@ -23,7 +23,6 @@ using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; -namespace dg = dynamicgraph; #include DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6dRelative, diff --git a/tests/tools/test_device.cpp b/tests/tools/test_device.cpp index a2edb35b1..ebe3437a6 100644 --- a/tests/tools/test_device.cpp +++ b/tests/tools/test_device.cpp @@ -25,7 +25,6 @@ using namespace std; using namespace dynamicgraph; using namespace dynamicgraph::sot; -namespace dg = dynamicgraph; class TestDevice : public dg::sot::Device { public: From 49a57bb4e9ddf20f874546234cc0845a17a1b4c7 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:33:51 +0200 Subject: [PATCH 31/43] Revert "Fix dg namespace pb." This reverts commit d59eb72c20205034e95b6634cd2bf9598bb5c27c. --- cmake | 2 +- include/sot/core/gain-adaptive.hh | 8 +++++--- include/sot/core/robot-utils.hh | 8 +++++--- include/sot/core/seq-play.hh | 10 +++++---- include/sot/core/seqplay.hh | 10 +++++---- include/sot/core/sequencer.hh | 1 + include/sot/core/smooth-reach.hh | 17 ++++++++------- include/sot/core/sot.hh | 9 ++++---- include/sot/core/task-abstract.hh | 7 ++++--- include/sot/core/task-conti.hh | 6 ++++-- include/sot/core/task-pd.hh | 9 ++++---- include/sot/core/task-unilateral.hh | 10 +++++---- include/sot/core/task.hh | 18 ++++++++-------- include/sot/core/time-stamp.hh | 16 ++++++++------- include/sot/core/timer.hh | 13 ++++++------ include/sot/core/trajectory.hh | 4 +++- include/sot/core/vector-constant.hh | 5 +++-- include/sot/core/vector-to-rotation.hh | 10 +++++---- include/sot/core/visual-point-projecter.hh | 8 ++++---- src/tools/parameter-server.cpp | 2 +- src/tools/sequencer.cpp | 2 +- tests/tools/test_robot_utils.cpp | 24 +++++++++++----------- 22 files changed, 114 insertions(+), 85 deletions(-) diff --git a/cmake b/cmake index c333a88de..9e21ae222 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit c333a88decb3e4c0a86947bc6c7f072dc5c5df20 +Subproject commit 9e21ae2222fdb51dccd1320bb7208f73259b0c73 diff --git a/include/sot/core/gain-adaptive.hh b/include/sot/core/gain-adaptive.hh index 9cd40538c..e778242d8 100644 --- a/include/sot/core/gain-adaptive.hh +++ b/include/sot/core/gain-adaptive.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -41,6 +42,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /** Exponentially decreasing gain. * It follows the law \f[ g(e) = a \exp (-b ||e||) + c \f]. @@ -50,7 +52,7 @@ namespace sot { * - \f$ b = 0 \f$, * - \f$ c = 0.1 \f$. */ -class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dynamicgraph::Entity { +class SOTGAINADAPTATIVE_EXPORT GainAdaptive : public dg::Entity { public: /* --- CONSTANTS --- */ /* Default values. */ @@ -114,8 +116,8 @@ public: /* --- INIT --- */ void forceConstant(void); public: /* --- SIGNALS --- */ - dynamicgraph::SignalPtr errorSIN; - dynamicgraph::SignalTimeDependent gainSOUT; + dg::SignalPtr errorSIN; + dg::SignalTimeDependent gainSOUT; protected: double &computeGain(double &res, int t); diff --git a/include/sot/core/robot-utils.hh b/include/sot/core/robot-utils.hh index bcb11f79e..0cf7fcf28 100644 --- a/include/sot/core/robot-utils.hh +++ b/include/sot/core/robot-utils.hh @@ -18,6 +18,8 @@ #include #include +namespace dg = ::dynamicgraph; +using namespace dg; namespace dynamicgraph { namespace sot { @@ -55,8 +57,8 @@ struct SOT_CORE_EXPORT ForceUtil { void set_name_to_force_id(const std::string &name, const Index &force_id); - void set_force_id_to_limits(const Index &force_id, const dynamicgraph::Vector &lf, - const dynamicgraph::Vector &uf); + void set_force_id_to_limits(const Index &force_id, const dg::Vector &lf, + const dg::Vector &uf); void create_force_id_to_name_map(); @@ -167,7 +169,7 @@ public: /// Set the map between urdf index and sot index void set_urdf_to_sot(const std::vector &urdf_to_sot); - void set_urdf_to_sot(const dynamicgraph::Vector &urdf_to_sot); + void set_urdf_to_sot(const dg::Vector &urdf_to_sot); /// Set the limits (lq,uq) for joint idx void set_joint_limits_for_id(const Index &idx, const double &lq, diff --git a/include/sot/core/seq-play.hh b/include/sot/core/seq-play.hh index 6be8ca4e3..dd791ac73 100644 --- a/include/sot/core/seq-play.hh +++ b/include/sot/core/seq-play.hh @@ -16,7 +16,7 @@ /* -- MaaL --- */ #include - +namespace dg = dynamicgraph; /* SOT */ #include #include @@ -50,7 +50,7 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - typedef std::list StateList; + typedef std::list StateList; StateList stateList; StateList::iterator currPos; unsigned int currRank; @@ -64,7 +64,7 @@ public: void loadFile(const std::string &name); - dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos, const int &time); + dg::Vector &getNextPosition(dg::Vector &pos, const int &time); public: /* --- DISPLAY --- */ virtual void display(std::ostream &os) const; @@ -75,8 +75,10 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ + // dynamicgraph::SignalPtr positionSIN; + // dynamicgraph::SignalTimeDependant velocitySOUT; dynamicgraph::SignalTimeDependent refresherSINTERN; - dynamicgraph::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/seqplay.hh b/include/sot/core/seqplay.hh index 6be8ca4e3..dd791ac73 100644 --- a/include/sot/core/seqplay.hh +++ b/include/sot/core/seqplay.hh @@ -16,7 +16,7 @@ /* -- MaaL --- */ #include - +namespace dg = dynamicgraph; /* SOT */ #include #include @@ -50,7 +50,7 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - typedef std::list StateList; + typedef std::list StateList; StateList stateList; StateList::iterator currPos; unsigned int currRank; @@ -64,7 +64,7 @@ public: void loadFile(const std::string &name); - dynamicgraph::Vector &getNextPosition(dynamicgraph::Vector &pos, const int &time); + dg::Vector &getNextPosition(dg::Vector &pos, const int &time); public: /* --- DISPLAY --- */ virtual void display(std::ostream &os) const; @@ -75,8 +75,10 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ + // dynamicgraph::SignalPtr positionSIN; + // dynamicgraph::SignalTimeDependant velocitySOUT; dynamicgraph::SignalTimeDependent refresherSINTERN; - dynamicgraph::SignalTimeDependent positionSOUT; + dynamicgraph::SignalTimeDependent positionSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/sequencer.hh b/include/sot/core/sequencer.hh index 0a57cc3cd..0c0584b88 100644 --- a/include/sot/core/sequencer.hh +++ b/include/sot/core/sequencer.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include diff --git a/include/sot/core/smooth-reach.hh b/include/sot/core/smooth-reach.hh index 5848bb190..1240e5b9d 100644 --- a/include/sot/core/smooth-reach.hh +++ b/include/sot/core/smooth-reach.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include @@ -38,18 +39,19 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ -class SOTSMOOTHREACH_EXPORT SmoothReach : public dynamicgraph::Entity { +class SOTSMOOTHREACH_EXPORT SmoothReach : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName() const { return CLASS_NAME; } private: - dynamicgraph::Vector start, goal; + dg::Vector start, goal; int startTime, lengthTime; bool isStarted, isParam; int smoothMode; @@ -62,14 +64,15 @@ public: /* --- CONSTRUCTION --- */ virtual ~SmoothReach(void){}; public: /* --- SIGNAL --- */ - dynamicgraph::SignalPtr startSIN; - dynamicgraph::SignalTimeDependent goalSOUT; + dg::SignalPtr startSIN; + dg::SignalTimeDependent goalSOUT; + // dg::SignalTimeDependent percentSOUT; public: /* --- FUNCTION --- */ - dynamicgraph::Vector &goalSOUT_function(dynamicgraph::Vector &goal, const int &time); + dg::Vector &goalSOUT_function(dg::Vector &goal, const int &time); - void set(const dynamicgraph::Vector &goal, const int &length); - const dynamicgraph::Vector &getGoal(void); + void set(const dg::Vector &goal, const int &length); + const dg::Vector &getGoal(void); const int &getLength(void); const int &getStart(void); diff --git a/include/sot/core/sot.hh b/include/sot/core/sot.hh index 067573571..54ab52eef 100644 --- a/include/sot/core/sot.hh +++ b/include/sot/core/sot.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* Classes standards. */ #include /* Classe std::list */ @@ -158,7 +159,7 @@ public: /* --- CONTROL --- */ */ /*! \brief Compute the control law. */ - virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control, const int &time); + virtual dg::Vector &computeControlLaw(dg::Vector &control, const int &time); /*! @} */ @@ -180,18 +181,18 @@ public: /* --- SIGNALS --- */ * the recurence of the SOT (e.g. velocity coming from the other * OpenHRP plugins). */ - SignalPtr q0SIN; + SignalPtr q0SIN; /*! \brief A matrix K whose columns are a base of the desired velocity. * In other words, \f$ \dot{q} = K * u \f$ where \f$ u \f$ is the free * parameter to be computed. * \note K should be an orthonormal matrix. */ - SignalPtr proj0SIN; + SignalPtr proj0SIN; /*! \brief This signal allow to change the threshold for the damped pseudo-inverse on-line */ SignalPtr inversionThresholdSIN; /*! \brief Allow to get the result of the computed control law. */ - SignalTimeDependent controlSOUT; + SignalTimeDependent controlSOUT; /*! @} */ /*! \brief This method write the priority between tasks in the output stream diff --git a/include/sot/core/task-abstract.hh b/include/sot/core/task-abstract.hh index 248f1ec5c..cb9fe4dd5 100644 --- a/include/sot/core/task-abstract.hh +++ b/include/sot/core/task-abstract.hh @@ -17,6 +17,7 @@ /* Matrix */ #include #include +namespace dg = dynamicgraph; /* STD */ #include @@ -45,7 +46,7 @@ namespace sot { /// TaskAbstract. The value and Jacobian of a Task are computed from the /// features that are stored in the task. -class SOT_CORE_EXPORT TaskAbstract : public dynamicgraph::Entity { +class SOT_CORE_EXPORT TaskAbstract : public dg::Entity { public: /* Use a derivative of this class to store computational memory. */ class MemoryTaskAbstract { @@ -75,8 +76,8 @@ public: TaskAbstract(const std::string &n); public: /* --- SIGNALS --- */ - dynamicgraph::SignalTimeDependent taskSOUT; - dynamicgraph::SignalTimeDependent jacobianSOUT; + dg::SignalTimeDependent taskSOUT; + dg::SignalTimeDependent jacobianSOUT; }; } /* namespace sot */ diff --git a/include/sot/core/task-conti.hh b/include/sot/core/task-conti.hh index 4098ea325..01b71002a 100644 --- a/include/sot/core/task-conti.hh +++ b/include/sot/core/task-conti.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* STD */ #include @@ -47,6 +48,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; class SOTTASKCONTI_EXPORT TaskConti : public Task { protected: @@ -54,7 +56,7 @@ protected: int timeRef; double mu; - dynamicgraph::Vector q0; + dg::Vector q0; public: static const std::string CLASS_NAME; @@ -72,7 +74,7 @@ public: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr controlPrevSIN; + dg::SignalPtr controlPrevSIN; /* --- DISPLAY ------------------------------------------------------------ */ void display(std::ostream &os) const; diff --git a/include/sot/core/task-pd.hh b/include/sot/core/task-pd.hh index 8c674924c..2778afb40 100644 --- a/include/sot/core/task-pd.hh +++ b/include/sot/core/task-pd.hh @@ -37,26 +37,27 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; class SOTTASKPD_EXPORT TaskPD : public Task { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } - dynamicgraph::Vector previousError; + dg::Vector previousError; double beta; public: TaskPD(const std::string &n); /* --- COMPUTATION --- */ - dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &error, int time); + dg::Vector &computeErrorDot(dg::Vector &error, int time); VectorMultiBound &computeTaskModif(VectorMultiBound &error, int time); /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalTimeDependent errorDotSOUT; - dynamicgraph::SignalPtr errorDotSIN; + dg::SignalTimeDependent errorDotSOUT; + dg::SignalPtr errorDotSIN; /* --- PARAMS --- */ void initCommand(void); diff --git a/include/sot/core/task-unilateral.hh b/include/sot/core/task-unilateral.hh index 3a704a524..071669d6b 100644 --- a/include/sot/core/task-unilateral.hh +++ b/include/sot/core/task-unilateral.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* STD */ #include @@ -47,6 +48,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; class SOTTASKUNILATERAL_EXPORT TaskUnilateral : public Task { protected: @@ -64,10 +66,10 @@ public: /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr positionSIN; - dynamicgraph::SignalPtr referenceInfSIN; - dynamicgraph::SignalPtr referenceSupSIN; - dynamicgraph::SignalPtr dtSIN; + dg::SignalPtr positionSIN; + dg::SignalPtr referenceInfSIN; + dg::SignalPtr referenceSupSIN; + dg::SignalPtr dtSIN; /* --- DISPLAY ------------------------------------------------------------ */ void display(std::ostream &os) const; diff --git a/include/sot/core/task.hh b/include/sot/core/task.hh index 7417a0342..24e6e1a78 100644 --- a/include/sot/core/task.hh +++ b/include/sot/core/task.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* STD */ #include @@ -69,6 +70,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; class SOTTASK_EXPORT Task : public TaskAbstract { public: @@ -97,19 +99,19 @@ public: bool getWithDerivative(void); /* --- COMPUTATION --- */ - dynamicgraph::Vector &computeError(dynamicgraph::Vector &error, int time); + dg::Vector &computeError(dg::Vector &error, int time); VectorMultiBound &computeTaskExponentialDecrease(VectorMultiBound &errorRef, int time); - dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &J, int time); - dynamicgraph::Vector &computeErrorTimeDerivative(dynamicgraph::Vector &res, int time); + dg::Matrix &computeJacobian(dg::Matrix &J, int time); + dg::Vector &computeErrorTimeDerivative(dg::Vector &res, int time); /* --- SIGNALS ------------------------------------------------------------ */ public: - dynamicgraph::SignalPtr controlGainSIN; - dynamicgraph::SignalPtr dampingGainSINOUT; - dynamicgraph::SignalPtr controlSelectionSIN; - dynamicgraph::SignalTimeDependent errorSOUT; - dynamicgraph::SignalTimeDependent errorTimeDerivativeSOUT; + dg::SignalPtr controlGainSIN; + dg::SignalPtr dampingGainSINOUT; + dg::SignalPtr controlSelectionSIN; + dg::SignalTimeDependent errorSOUT; + dg::SignalTimeDependent errorTimeDerivativeSOUT; /* --- DISPLAY ------------------------------------------------------------ */ void display(std::ostream &os) const; diff --git a/include/sot/core/time-stamp.hh b/include/sot/core/time-stamp.hh index 93b3d9918..6c1a9d03b 100644 --- a/include/sot/core/time-stamp.hh +++ b/include/sot/core/time-stamp.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* Classes standards. */ #ifndef WIN32 @@ -49,8 +50,9 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; -class TimeStamp_EXPORT TimeStamp : public dynamicgraph::Entity { +class TimeStamp_EXPORT TimeStamp : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -70,17 +72,17 @@ public: /* --- DISPLAY --- */ public: /* --- SIGNALS --- */ /* These signals can be called several time per period, given * each time a different results. Useful for chronos. */ - dynamicgraph::Signal timeSOUT; - dynamicgraph::Signal timeDoubleSOUT; + dg::Signal timeSOUT; + dg::Signal timeDoubleSOUT; /* These signals can be called several time per period, but give * always the same results different results. Useful for synchro. */ - dynamicgraph::SignalTimeDependent timeOnceSOUT; - dynamicgraph::SignalTimeDependent timeOnceDoubleSOUT; + dg::SignalTimeDependent timeOnceSOUT; + dg::SignalTimeDependent timeOnceDoubleSOUT; protected: /* --- SIGNAL FUNCTIONS --- */ - dynamicgraph::Vector &getTimeStamp(dynamicgraph::Vector &res, const int &time); - double &getTimeStampDouble(const dynamicgraph::Vector &vect, double &res); + dg::Vector &getTimeStamp(dg::Vector &res, const int &time); + double &getTimeStampDouble(const dg::Vector &vect, double &res); }; } /* namespace sot */ diff --git a/include/sot/core/timer.hh b/include/sot/core/timer.hh index 0913968eb..7651c81af 100644 --- a/include/sot/core/timer.hh +++ b/include/sot/core/timer.hh @@ -47,8 +47,9 @@ /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ +namespace dg = dynamicgraph; -template class Timer_EXPORT Timer : public dynamicgraph::Entity { +template class Timer_EXPORT Timer : public dg::Entity { public: static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -71,13 +72,13 @@ public: /* --- DISPLAY --- */ } public: /* --- SIGNALS --- */ - dynamicgraph::SignalPtr sigSIN; - dynamicgraph::SignalTimeDependent sigSOUT; - dynamicgraph::SignalTimeDependent sigClockSOUT; - dynamicgraph::Signal timerSOUT; + dg::SignalPtr sigSIN; + dg::SignalTimeDependent sigSOUT; + dg::SignalTimeDependent sigClockSOUT; + dg::Signal timerSOUT; protected: /* --- SIGNAL FUNCTIONS --- */ - void plug(dynamicgraph::Signal &sig) { + void plug(dg::Signal &sig) { sigSIN = &sig; dt = 0.; } diff --git a/include/sot/core/trajectory.hh b/include/sot/core/trajectory.hh index b6957f8e1..7c25d02f7 100644 --- a/include/sot/core/trajectory.hh +++ b/include/sot/core/trajectory.hh @@ -17,6 +17,8 @@ #include #include +namespace dg = dynamicgraph; +namespace ba = boost::assign; namespace dynamicgraph { namespace sot { @@ -122,7 +124,7 @@ public: void display(std::ostream &os) const { boost::array names = - boost::assign::list_of("Positions")("Velocities")("Accelerations")("Effort"); + ba::list_of("Positions")("Velocities")("Accelerations")("Effort"); const std::vector *points = 0; diff --git a/include/sot/core/vector-constant.hh b/include/sot/core/vector-constant.hh index 245c00e5e..ba197a9b3 100644 --- a/include/sot/core/vector-constant.hh +++ b/include/sot/core/vector-constant.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- VECTOR ---------------------------------------------------------- */ @@ -41,10 +42,10 @@ public: virtual ~VectorConstant(void) {} - SignalTimeDependent SOUT; + SignalTimeDependent SOUT; /// \brief Set value of vector (and therefore of output signal) - void setValue(const dynamicgraph::Vector &inValue); + void setValue(const dg::Vector &inValue); }; } // namespace sot diff --git a/include/sot/core/vector-to-rotation.hh b/include/sot/core/vector-to-rotation.hh index 9a4bb2aeb..53e79bf54 100644 --- a/include/sot/core/vector-to-rotation.hh +++ b/include/sot/core/vector-to-rotation.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* STD */ #include @@ -39,8 +40,9 @@ /* --------------------------------------------------------------------- */ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; -class SOTVECTORTOROTATION_EXPORT VectorToRotation : public dynamicgraph::Entity { +class SOTVECTORTOROTATION_EXPORT VectorToRotation : public dg::Entity { static const std::string CLASS_NAME; virtual const std::string &getClassName(void) const { return CLASS_NAME; } @@ -54,10 +56,10 @@ public: virtual ~VectorToRotation(void) {} - dynamicgraph::SignalPtr SIN; - dynamicgraph::SignalTimeDependent SOUT; + dg::SignalPtr SIN; + dg::SignalTimeDependent SOUT; - MatrixRotation &computeRotation(const dynamicgraph::Vector &angles, + MatrixRotation &computeRotation(const dg::Vector &angles, MatrixRotation &res); }; diff --git a/include/sot/core/visual-point-projecter.hh b/include/sot/core/visual-point-projecter.hh index 98659a736..186aa2035 100644 --- a/include/sot/core/visual-point-projecter.hh +++ b/include/sot/core/visual-point-projecter.hh @@ -25,7 +25,7 @@ /* Matrix */ #include - +namespace dg = dynamicgraph; #include /* SOT */ @@ -52,12 +52,12 @@ public: /* --- ENTITY INHERITANCE --- */ virtual const std::string &getClassName(void) const { return CLASS_NAME; } public: /* --- SIGNALS --- */ - DECLARE_SIGNAL_IN(point3D, dynamicgraph::Vector); + DECLARE_SIGNAL_IN(point3D, dg::Vector); DECLARE_SIGNAL_IN(transfo, MatrixHomogeneous); - DECLARE_SIGNAL_OUT(point3Dgaze, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(point3Dgaze, dg::Vector); DECLARE_SIGNAL_OUT(depth, double); - DECLARE_SIGNAL_OUT(point2D, dynamicgraph::Vector); + DECLARE_SIGNAL_OUT(point2D, dg::Vector); }; // class VisualPointProjecter diff --git a/src/tools/parameter-server.cpp b/src/tools/parameter-server.cpp index 99c17c571..e87c0c2f5 100644 --- a/src/tools/parameter-server.cpp +++ b/src/tools/parameter-server.cpp @@ -213,7 +213,7 @@ void ParameterServer::setForceNameToForceId(const std::string &forceName, static_cast(forceId)); } -void ParameterServer::setJoints(const dynamicgraph::Vector &urdf_to_sot) { +void ParameterServer::setJoints(const dg::Vector &urdf_to_sot) { if (!m_initSucceeded) { SEND_WARNING_STREAM_MSG("Cannot set mapping to sot before initialization!"); return; diff --git a/src/tools/sequencer.cpp b/src/tools/sequencer.cpp index ececc599e..2cd18134a 100644 --- a/src/tools/sequencer.cpp +++ b/src/tools/sequencer.cpp @@ -59,7 +59,7 @@ class sotEventTaskBased : public Sequencer::sotEventAbstract { cmdArgs >> taskname; sotDEBUG(15) << "Add task " << taskname << std::endl; taskPtr = dynamic_cast( - &dynamicgraph::PoolStorage::getInstance()->getEntity(taskname)); + &dg::PoolStorage::getInstance()->getEntity(taskname)); } } virtual void display(std::ostream &os) const { diff --git a/tests/tools/test_robot_utils.cpp b/tests/tools/test_robot_utils.cpp index 47169e8fb..6f1955101 100644 --- a/tests/tools/test_robot_utils.cpp +++ b/tests/tools/test_robot_utils.cpp @@ -56,7 +56,7 @@ int main(void) { /*Test set urdf_to_sot */ - dynamicgraph::Vector urdf_to_sot(3); + dg::Vector urdf_to_sot(3); urdf_to_sot << 0, 2, 1; robot_util->set_urdf_to_sot(urdf_to_sot); if (urdf_to_sot == robot_util->m_dgv_urdf_to_sot) { @@ -65,10 +65,10 @@ int main(void) { std::cout << "ERROR: urdf_to_sot does not work !" << std::endl; } /*Test joints_urdf_to_sot and joints_sot_to_urdf */ - dynamicgraph::Vector q_urdf(3); + dg::Vector q_urdf(3); q_urdf << 10, 20, 30; - dynamicgraph::Vector q_sot(3); - dynamicgraph::Vector q_test_urdf(3); + dg::Vector q_sot(3); + dg::Vector q_test_urdf(3); robot_util->joints_urdf_to_sot(q_urdf, q_sot); robot_util->joints_sot_to_urdf(q_sot, q_test_urdf); if (q_urdf == q_test_urdf) { @@ -81,23 +81,23 @@ int main(void) { } /*Test velocity_sot_to_urdf and velocity_urdf_to_sot */ - dynamicgraph::Vector q2_urdf(10); - dynamicgraph::Vector v_urdf(9); - dynamicgraph::Vector v_sot(9); + dg::Vector q2_urdf(10); + dg::Vector v_urdf(9); + dg::Vector v_sot(9); robot_util->velocity_urdf_to_sot(q2_urdf, v_urdf, v_sot); robot_util->velocity_sot_to_urdf(q2_urdf, v_sot, v_urdf); std::cout << "velocity_sot_to_urdf and velocity_urdf_to_sot work !" << std::endl; /*Test base_urdf_to_sot and base_sot_to_urdf */ - dynamicgraph::Vector base_q_urdf(7); - dynamicgraph::Vector base_q_sot(6); + dg::Vector base_q_urdf(7); + dg::Vector base_q_sot(6); robot_util->base_urdf_to_sot(base_q_urdf, base_q_sot); robot_util->base_sot_to_urdf(base_q_sot, base_q_urdf); std::cout << "base_urdf_to_sot and base_sot_to_urdf work !" << std::endl; /*Test config_urdf_to_sot and config_sot_to_urdf */ - dynamicgraph::Vector q2_sot(9); + dg::Vector q2_sot(9); robot_util->config_urdf_to_sot(q2_urdf, q2_sot); robot_util->config_sot_to_urdf(q2_sot, q2_urdf); std::cout << "config_urdf_to_sot and config_sot_to_urdf work !" << std::endl; @@ -116,9 +116,9 @@ int main(void) { robot_util->m_force_util.set_name_to_force_id(lh, 2); robot_util->m_force_util.set_name_to_force_id(rh, 3); - dynamicgraph::Vector lf_lim(6); + dg::Vector lf_lim(6); lf_lim << 1, 2, 3, 4, 5, 6; - dynamicgraph::Vector uf_lim(6); + dg::Vector uf_lim(6); uf_lim << 10, 20, 30, 40, 50, 60; robot_util->m_force_util.set_force_id_to_limits(1, lf_lim, uf_lim); if (robot_util->m_force_util.get_id_from_name(rf) == 0 && From d1212e538b4144bdac6e442653f144e3778ac015 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:34:12 +0200 Subject: [PATCH 32/43] Revert "Remove definition of dg across various class." This reverts commit 468cac4453560c0145094c4a6e161e135b318159. --- include/sot/core/feature-abstract.hh | 17 +++++++++-------- include/sot/core/feature-generic.hh | 13 +++++++------ include/sot/core/feature-posture.hh | 10 +++++----- 3 files changed, 21 insertions(+), 19 deletions(-) diff --git a/include/sot/core/feature-abstract.hh b/include/sot/core/feature-abstract.hh index 282ae07fb..5ee14cfbc 100644 --- a/include/sot/core/feature-abstract.hh +++ b/include/sot/core/feature-abstract.hh @@ -16,6 +16,7 @@ /* Matrix */ #include +namespace dg = dynamicgraph; /* SOT */ #include "sot/core/api.hh" @@ -136,20 +137,20 @@ public: \par[in] time: The time at which the error is computed. \return The vector res with the appropriate value. */ - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time) = 0; + virtual dg::Vector &computeError(dg::Vector &res, int time) = 0; /*! \brief Compute the Jacobian of the error according the robot state. \par[out] res: The matrix in which the error will be written. \return The matrix res with the appropriate values. */ - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time) = 0; + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time) = 0; /// Callback for signal errordotSOUT /// /// Copy components of the input signal errordotSIN defined by selection /// flag selectionSIN. - virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); + virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); /*! @} */ @@ -169,7 +170,7 @@ public: SignalPtr selectionSIN; /// Derivative of the reference value. - SignalPtr errordotSIN; + SignalPtr errordotSIN; /*! @} */ @@ -178,15 +179,15 @@ public: /*! \brief This signal returns the error between the desired value and the current value : \f$ E(t) = {\bf s}(t) - {\bf s}^*(t)\f$ */ - SignalTimeDependent errorSOUT; + SignalTimeDependent errorSOUT; /*! \brief Derivative of the error with respect to time: * \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt} \f$ */ - SignalTimeDependent errordotSOUT; + SignalTimeDependent errordotSOUT; /*! \brief Jacobian of the error wrt the robot state: * \f$ J = \frac{\partial {\bf s}}{\partial {\bf q}}\f$ */ - SignalTimeDependent jacobianSOUT; + SignalTimeDependent jacobianSOUT; /*! \brief Returns the dimension of the feature as an output signal. */ SignalTimeDependent dimensionSOUT; @@ -195,7 +196,7 @@ public: FileName. */ virtual std::ostream &writeGraph(std::ostream &os) const; - virtual SignalTimeDependent &getErrorDot() { + virtual SignalTimeDependent &getErrorDot() { return errordotSOUT; } diff --git a/include/sot/core/feature-generic.hh b/include/sot/core/feature-generic.hh index 2d0e9e189..21bfe832f 100644 --- a/include/sot/core/feature-generic.hh +++ b/include/sot/core/feature-generic.hh @@ -38,6 +38,7 @@ namespace dynamicgraph { namespace sot { +namespace dg = dynamicgraph; /*! \class FeatureGeneric @@ -64,21 +65,21 @@ public: virtual const std::string &getClassName(void) const { return CLASS_NAME; } protected: - dynamicgraph::Vector::Index dimensionDefault; + dg::Vector::Index dimensionDefault; /* --- SIGNALS ------------------------------------------------------------ */ public: - /*! \name dynamicgraph::Signals + /*! \name dg::Signals @{ */ /*! \name Input signals @{ */ /*! \brief Input for the error. */ - dynamicgraph::SignalPtr errorSIN; + dg::SignalPtr errorSIN; /*! \brief Input for the Jacobian. */ - dynamicgraph::SignalPtr jacobianSIN; + dg::SignalPtr jacobianSIN; /*! @} */ @@ -109,10 +110,10 @@ public: /*! \brief Compute the error between the desired value and the value itself. */ - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int time); /*! \brief Compute the Jacobian of the value according to the robot state.. */ - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int time); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int time); /*! @} */ diff --git a/include/sot/core/feature-posture.hh b/include/sot/core/feature-posture.hh index dede94104..3ff6b6287 100644 --- a/include/sot/core/feature-posture.hh +++ b/include/sot/core/feature-posture.hh @@ -52,8 +52,8 @@ class SOTFEATUREPOSTURE_EXPORT FeaturePosture : public FeatureAbstract { DYNAMIC_GRAPH_ENTITY_DECL(); public: - typedef dynamicgraph::SignalPtr signalIn_t; - typedef dynamicgraph::SignalTimeDependent signalOut_t; + typedef dynamicgraph::SignalPtr signalIn_t; + typedef dynamicgraph::SignalTimeDependent signalOut_t; DECLARE_NO_REFERENCE; @@ -63,9 +63,9 @@ public: void selectDof(unsigned dofId, bool control); protected: - virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res, int); - virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res, int); - virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res, int time); + virtual dg::Vector &computeError(dg::Vector &res, int); + virtual dg::Matrix &computeJacobian(dg::Matrix &res, int); + virtual dg::Vector &computeErrorDot(dg::Vector &res, int time); signalIn_t state_; signalIn_t posture_; From 4b75a97223aca83c0cab04acac4153a6abacf95e Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:34:30 +0200 Subject: [PATCH 33/43] Revert "[tests] Fix namespace problem for test_feature_generic" This reverts commit f7cbede9bf4f9ff1bbb64673aa717c034449cf67. --- tests/features/test_feature_generic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index 18ad6645b..c082b7e7b 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -38,7 +38,6 @@ namespace dynamicgraph { namespace sot { -namespace dg = dynamicgraph; typedef pinocchio::CartesianProductOperation< pinocchio::VectorSpaceOperationTpl<3, double>, @@ -58,6 +57,7 @@ template struct LG_t { using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; +namespace dg = dynamicgraph; #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ From 8c4e6ce5b9b19f41023e587d68be468aadb2001e Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:34:47 +0200 Subject: [PATCH 34/43] Revert "[feature-pose] Fix the namespace problem." This reverts commit b052916a66653c9959ef6ce649ff2c5837b87b1b. --- src/feature/feature-pose.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index f328281c4..7d8906803 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -37,9 +37,5 @@ template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); -namespace dynamicgraph { -namespace sot { template class FeaturePose; template class FeaturePose; -} -} From db79299be2bf5fccdb6bd7263612a40a55912f3d Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:35:04 +0200 Subject: [PATCH 35/43] Revert "[feature-pose] Make it work on clang." This reverts commit fbbf38c4d5d27176101ad36e055b38d5abf94904. --- include/sot/core/feature-pose.hh | 4 ---- include/sot/core/feature-pose.hxx | 10 +++++----- tests/features/test_feature_generic.cpp | 21 ++------------------- 3 files changed, 7 insertions(+), 28 deletions(-) diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index 6ca40ac71..ed506c885 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -161,10 +161,6 @@ private: /// \todo Intermediate variables for internal computations }; -template < typename T > -Vector6d convertVelocity(const MatrixHomogeneous &M, - const MatrixHomogeneous &Mdes, - const Vector &faNufafbDes); #if __cplusplus >= 201103L extern template class SOT_CORE_DLLAPI FeaturePose; extern template class SOT_CORE_DLLAPI FeaturePose; diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx index 500dd4ecd..e8779ee5e 100644 --- a/include/sot/core/feature-pose.hxx +++ b/include/sot/core/feature-pose.hxx @@ -50,7 +50,7 @@ template struct LG_t { static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); template -FeaturePose::FeaturePose(const std::string &pointName) +FeaturePose::FeaturePose(const string &pointName) : FeatureAbstract(pointName), oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"), jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"), @@ -128,7 +128,7 @@ static inline void check(const FeaturePose &ft) { template unsigned int &FeaturePose::getDimension(unsigned int &dim, int time) { - sotDEBUG(25) << "# In {" << std::endl; + sotDEBUG(25) << "# In {" << endl; const Flags &fl = selectionSIN.access(time); @@ -137,7 +137,7 @@ unsigned int &FeaturePose::getDimension(unsigned int &dim, if (fl(i)) dim++; - sotDEBUG(25) << "# Out }" << std::endl; + sotDEBUG(25) << "# Out }" << endl; return dim; } @@ -259,8 +259,8 @@ Vector &FeaturePose::computeError(Vector &error, int time) { // This function is responsible of converting the input velocity expressed with // SE(3) convention onto a velocity expressed with the convention of this // feature (R^3xSO(3) or SE(3)), in the right frame. -template <> -Vector6d convertVelocity(const MatrixHomogeneous &M, +template +Vector6d convertVelocity(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes) { (void)M; diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index c082b7e7b..867a05317 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -32,28 +32,11 @@ #include #include #include +#include #include #include #include -namespace dynamicgraph { -namespace sot { - -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - -namespace internal { -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; -} -} -} - using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; @@ -308,7 +291,7 @@ Vector toVector(const std::vector &in) { template class TestFeaturePose : public FeatureTestBase { public: - typedef typename dg::sot::internal::LG_t::type LieGroup_t; + typedef typename LG_t::type LieGroup_t; FeaturePose feature_; bool relative_; pinocchio::Model model_; From ca802b8284106633a8edbff20e5b17dee2806b56 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:35:21 +0200 Subject: [PATCH 36/43] Revert "Fix usage of explicit instantiation" This reverts commit 362af0335a2a7457ef7be99722586de2288ee09d. --- include/sot/core/feature-pose.hh | 33 ++++++++++++++++++++++++++----- include/sot/core/feature-pose.hxx | 22 +++++---------------- src/feature/feature-pose.cpp | 23 +++++++++++++++++++-- 3 files changed, 54 insertions(+), 24 deletions(-) diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index ed506c885..b842de518 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -6,8 +6,8 @@ * */ -#ifndef __SOT_FEATURE_POSE_HH__ -#define __SOT_FEATURE_POSE_HH__ +#ifndef __SOT_FEATURE_TRANSFORMATION_HH__ +#define __SOT_FEATURE_TRANSFORMATION_HH__ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ @@ -166,13 +166,36 @@ extern template class SOT_CORE_DLLAPI FeaturePose; extern template class SOT_CORE_DLLAPI FeaturePose; #endif +} /* namespace sot */ +} /* namespace dynamicgraph */ + + +using namespace std; +using namespace dynamicgraph; +using namespace dynamicgraph::sot; + +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + +namespace dynamicgraph { +namespace sot { +namespace dg = dynamicgraph; + +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; + typedef FeaturePose FeaturePose_t; typedef FeaturePose FeaturePoseSE3_t; +} +} -} /* namespace sot */ -} /* namespace dynamicgraph */ -#endif // #ifndef __SOT_FEATURE_POSE_HH__ +#endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__ /* * Local variables: diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx index e8779ee5e..15292e895 100644 --- a/include/sot/core/feature-pose.hxx +++ b/include/sot/core/feature-pose.hxx @@ -29,19 +29,7 @@ namespace dynamicgraph { namespace sot { - -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - -namespace internal { -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; -} +namespace dg = dynamicgraph; /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ @@ -154,7 +142,7 @@ Vector7 toVector(const MatrixHomogeneous &M) { template Matrix &FeaturePose::computeJacobian(Matrix &J, int time) { - typedef typename internal::LG_t::type LieGroup_t; + typedef typename LG_t::type LieGroup_t; check(*this); @@ -239,7 +227,7 @@ Vector7 &FeaturePose::computeQfaMfbDes(Vector7 &res, int time) { template Vector &FeaturePose::computeError(Vector &error, int time) { - typedef typename internal::LG_t::type LieGroup_t; + typedef typename LG_t::type LieGroup_t; check(*this); const Flags &fl = selectionSIN(time); @@ -259,7 +247,7 @@ Vector &FeaturePose::computeError(Vector &error, int time) { // This function is responsible of converting the input velocity expressed with // SE(3) convention onto a velocity expressed with the convention of this // feature (R^3xSO(3) or SE(3)), in the right frame. -template +template Vector6d convertVelocity(const MatrixHomogeneous &M, const MatrixHomogeneous &Mdes, const Vector &faNufafbDes) { @@ -282,7 +270,7 @@ Vector6d convertVelocity(const MatrixHomogeneous &M, template Vector &FeaturePose::computeErrorDot(Vector &errordot, int time) { - typedef typename internal::LG_t::type LieGroup_t; + typedef typename LG_t::type LieGroup_t; check(*this); errordot.resize(dimensionSOUT(time)); diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index 7d8906803..62a56a737 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -25,17 +25,36 @@ #include +#include #include #include #include +using namespace std; +using namespace dynamicgraph; using namespace dynamicgraph::sot; +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; + + typedef FeaturePose FeaturePose_t; typedef FeaturePose FeaturePoseSE3_t; template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); -template class FeaturePose; -template class FeaturePose; +/* --------------------------------------------------------------------- */ +/* --- CLASS ----------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); + From f161b983cac9d867b72ef7671100f90e947368b6 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:35:39 +0200 Subject: [PATCH 37/43] Revert "[feature-pose] Reorganize such that feature-pose.hxx is not in feature-pose.hh" This reverts commit 9d9004a468d5dce2e0db9c2e9a50aacbd5ea5497. --- include/sot/core/feature-pose.hh | 26 +------------------------ include/sot/core/feature-pose.hxx | 19 ++++++++++++++++++ src/feature/feature-pose.cpp | 1 - tests/features/test_feature_generic.cpp | 3 --- 4 files changed, 20 insertions(+), 29 deletions(-) diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index b842de518..25e194dd7 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -169,31 +169,7 @@ extern template class SOT_CORE_DLLAPI FeaturePose; } /* namespace sot */ } /* namespace dynamicgraph */ - -using namespace std; -using namespace dynamicgraph; -using namespace dynamicgraph::sot; - -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - -namespace dynamicgraph { -namespace sot { -namespace dg = dynamicgraph; - -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; - -typedef FeaturePose FeaturePose_t; -typedef FeaturePose FeaturePoseSE3_t; -} -} - +#include #endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__ diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx index 15292e895..be7346b75 100644 --- a/include/sot/core/feature-pose.hxx +++ b/include/sot/core/feature-pose.hxx @@ -27,10 +27,29 @@ #include #include +using namespace std; +using namespace dynamicgraph; +using namespace dynamicgraph::sot; + +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; + namespace dynamicgraph { namespace sot { namespace dg = dynamicgraph; +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; + +typedef FeaturePose FeaturePose_t; +typedef FeaturePose FeaturePoseSE3_t; + + /* --------------------------------------------------------------------- */ /* --- CLASS ----------------------------------------------------------- */ /* --------------------------------------------------------------------- */ diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index 62a56a737..71a72e51d 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -28,7 +28,6 @@ #include #include #include -#include using namespace std; using namespace dynamicgraph; diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index 867a05317..435f4514c 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -27,12 +27,10 @@ #include #include -#include #include #include #include #include -#include #include #include #include @@ -40,7 +38,6 @@ using namespace std; using namespace dynamicgraph::sot; using namespace dynamicgraph; -namespace dg = dynamicgraph; #define EIGEN_VECTOR_IS_APPROX(Va, Vb, precision) \ BOOST_CHECK_MESSAGE((Va).isApprox(Vb, precision), \ From fa01a4c27ecb7bacabb77082985ac7d53faa3701 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:35:52 +0200 Subject: [PATCH 38/43] Revert "Put template feature-pose in hxx file." This reverts commit 9dbda566c83f839be3f445341d5cb7c09d3250f6. --- include/sot/core/feature-pose.hh | 9 +- include/sot/core/feature-pose.hxx | 363 ------------------------ src/feature/feature-pose.cpp | 296 ++++++++++++++++++- tests/features/test_feature_generic.cpp | 10 +- 4 files changed, 304 insertions(+), 374 deletions(-) delete mode 100644 include/sot/core/feature-pose.hxx diff --git a/include/sot/core/feature-pose.hh b/include/sot/core/feature-pose.hh index 25e194dd7..eeefcaeda 100644 --- a/include/sot/core/feature-pose.hh +++ b/include/sot/core/feature-pose.hh @@ -116,7 +116,7 @@ public: public: FeaturePose(const std::string &name); - virtual ~FeaturePose(void); + virtual ~FeaturePose(void) {} virtual unsigned int &getDimension(unsigned int &dim, int time); @@ -161,16 +161,9 @@ private: /// \todo Intermediate variables for internal computations }; -#if __cplusplus >= 201103L -extern template class SOT_CORE_DLLAPI FeaturePose; -extern template class SOT_CORE_DLLAPI FeaturePose; -#endif - } /* namespace sot */ } /* namespace dynamicgraph */ -#include - #endif // #ifndef __SOT_FEATURE_TRANSFORMATION_HH__ /* diff --git a/include/sot/core/feature-pose.hxx b/include/sot/core/feature-pose.hxx deleted file mode 100644 index be7346b75..000000000 --- a/include/sot/core/feature-pose.hxx +++ /dev/null @@ -1,363 +0,0 @@ -/* - * Copyright 2019 - * Joseph Mirabel - * - * LAAS-CNRS - * - */ - -/* --------------------------------------------------------------------- */ -/* --- INCLUDE --------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -/* --- SOT --- */ -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include - -using namespace std; -using namespace dynamicgraph; -using namespace dynamicgraph::sot; - -typedef pinocchio::CartesianProductOperation< - pinocchio::VectorSpaceOperationTpl<3, double>, - pinocchio::SpecialOrthogonalOperationTpl<3, double> > - R3xSO3_t; -typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - -namespace dynamicgraph { -namespace sot { -namespace dg = dynamicgraph; - -template struct LG_t { - typedef typename boost::mpl::if_c::type type; -}; - -typedef FeaturePose FeaturePose_t; -typedef FeaturePose FeaturePoseSE3_t; - - -/* --------------------------------------------------------------------- */ -/* --- CLASS ----------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); - -template -FeaturePose::FeaturePose(const string &pointName) - : FeatureAbstract(pointName), - oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"), - jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"), - oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"), - jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"), - jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"), - jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"), - faMfbDes(NULL, - CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"), - faNufafbDes(NULL, - CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"), - faMfb( - boost::bind(&FeaturePose::computefaMfb, this, _1, _2), - oMja << jaMfa << oMjb << jbMfb, - CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"), - q_faMfb(boost::bind(&FeaturePose::computeQfaMfb, this, _1, - _2), - faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"), - q_faMfbDes(boost::bind(&FeaturePose::computeQfaMfbDes, - this, _1, _2), - faMfbDes, - CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") { - oMja.setConstant(Id); - jaMfa.setConstant(Id); - jbMfb.setConstant(Id); - faMfbDes.setConstant(Id); - faNufafbDes.setConstant(Vector::Zero(6)); - - jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb); - - errorSOUT.addDependencies(q_faMfbDes << q_faMfb); - - signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb); - signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes); - - errordotSOUT.setFunction( - boost::bind(&FeaturePose::computeErrorDot, this, _1, _2)); - errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes); - - // Commands - // - { - using namespace dynamicgraph::command; - addCommand("keep", - makeCommandVoid1( - *this, &FeaturePose::servoCurrentPosition, - docCommandVoid1( - "modify the desired position to servo at current pos.", - "time"))); - } -} - -template -FeaturePose::~FeaturePose() -{ -} - - - -/* --------------------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ -/* --------------------------------------------------------------------- */ - -template -static inline void check(const FeaturePose &ft) { - (void)ft; - assert(ft.oMja.isPlugged()); - assert(ft.jaMfa.isPlugged()); - assert(ft.oMjb.isPlugged()); - assert(ft.jbMfb.isPlugged()); - assert(ft.faMfbDes.isPlugged()); - assert(ft.faNufafbDes.isPlugged()); -} - -template -unsigned int &FeaturePose::getDimension(unsigned int &dim, - int time) { - sotDEBUG(25) << "# In {" << endl; - - const Flags &fl = selectionSIN.access(time); - - dim = 0; - for (int i = 0; i < 6; ++i) - if (fl(i)) - dim++; - - sotDEBUG(25) << "# Out }" << endl; - return dim; -} - -void toVector(const MatrixHomogeneous &M, Vector7 &v) { - v.head<3>() = M.translation(); - QuaternionMap(v.tail<4>().data()) = M.linear(); -} - -Vector7 toVector(const MatrixHomogeneous &M) { - Vector7 ret; - toVector(M, ret); - return ret; -} - -template -Matrix &FeaturePose::computeJacobian(Matrix &J, int time) { - typedef typename LG_t::type LieGroup_t; - - check(*this); - - q_faMfb.recompute(time); - q_faMfbDes.recompute(time); - - const unsigned int &dim = dimensionSOUT(time); - const Flags &fl = selectionSIN(time); - - const Matrix &_jbJjb = jbJjb(time); - - const MatrixHomogeneous &_jbMfb = - (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id); - - const Matrix::Index cJ = _jbJjb.cols(); - J.resize(dim, cJ); - - MatrixTwist X; - Eigen::Matrix Jminus; - - buildFrom(_jbMfb.inverse(Eigen::Affine), X); - MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() * - oMja.access(time).rotation().transpose() * - oMjb.access(time).rotation() * _jbMfb.rotation(); - if (boost::is_same::value) - X.topRows<3>().applyOnTheLeft(faRfb); - LieGroup_t().template dDifference( - q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); - - // Contribution of b: - // J = Jminus * X * jbJjb; - unsigned int rJ = 0; - for (unsigned int r = 0; r < 6; ++r) - if (fl((int)r)) - J.row(rJ++) = (Jminus * X).row(r) * _jbJjb; - - if (jaJja.isPlugged()) { - const Matrix &_jaJja = jaJja(time); - const MatrixHomogeneous &_jaMfa = - (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id), - _faMfb = faMfb.accessCopy(); - - buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X); - if (boost::is_same::value) - X.topRows<3>().applyOnTheLeft(faRfb); - - // J -= (Jminus * X) * jaJja(time); - rJ = 0; - for (unsigned int r = 0; r < 6; ++r) - if (fl((int)r)) - J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja; - } - - return J; -} - -template -MatrixHomogeneous & -FeaturePose::computefaMfb(MatrixHomogeneous &res, int time) { - check(*this); - - res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) * - jbMfb(time); - return res; -} - -template -Vector7 &FeaturePose::computeQfaMfb(Vector7 &res, int time) { - check(*this); - - toVector(faMfb(time), res); - return res; -} - -template -Vector7 &FeaturePose::computeQfaMfbDes(Vector7 &res, int time) { - check(*this); - - toVector(faMfbDes(time), res); - return res; -} - -template -Vector &FeaturePose::computeError(Vector &error, int time) { - typedef typename LG_t::type LieGroup_t; - check(*this); - - const Flags &fl = selectionSIN(time); - - Eigen::Matrix v; - LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v); - - error.resize(dimensionSOUT(time)); - unsigned int cursor = 0; - for (unsigned int i = 0; i < 6; ++i) - if (fl((int)i)) - error(cursor++) = v(i); - - return error; -} - -// This function is responsible of converting the input velocity expressed with -// SE(3) convention onto a velocity expressed with the convention of this -// feature (R^3xSO(3) or SE(3)), in the right frame. -template -Vector6d convertVelocity(const MatrixHomogeneous &M, - const MatrixHomogeneous &Mdes, - const Vector &faNufafbDes) { - (void)M; - MatrixTwist X; - buildFrom(Mdes.inverse(Eigen::Affine), X); - return X * faNufafbDes; -} -template <> -Vector6d convertVelocity(const MatrixHomogeneous &M, - const MatrixHomogeneous &Mdes, - const Vector &faNufafbDes) { - Vector6d nu; - nu.head<3>() = - faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>()); - nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>(); - return nu; -} - -template -Vector &FeaturePose::computeErrorDot(Vector &errordot, - int time) { - typedef typename LG_t::type LieGroup_t; - check(*this); - - errordot.resize(dimensionSOUT(time)); - const Flags &fl = selectionSIN(time); - if (!faNufafbDes.isPlugged()) { - errordot.setZero(); - return errordot; - } - - q_faMfb.recompute(time); - q_faMfbDes.recompute(time); - faNufafbDes.recompute(time); - - const MatrixHomogeneous &_faMfbDes = faMfbDes(time); - - Eigen::Matrix Jminus; - - LieGroup_t().template dDifference( - q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); - Vector6d nu = convertVelocity(faMfb(time), _faMfbDes, - faNufafbDes.accessCopy()); - unsigned int cursor = 0; - for (unsigned int i = 0; i < 6; ++i) - if (fl((int)i)) - errordot(cursor++) = Jminus.row(i) * nu; - - return errordot; -} - -/* Modify the value of the reference (sdes) so that it corresponds - * to the current position. The effect on the servo is to maintain the - * current position and correct any drift. */ -template -void FeaturePose::servoCurrentPosition(const int &time) { - check(*this); - - const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id), - _jaMfa = - (jaMfa.isPlugged() ? jaMfa.access(time) : Id), - _oMjb = oMjb.access(time), - _jbMfb = - (jbMfb.isPlugged() ? jbMfb.access(time) : Id); - faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb; -} - -static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"}; -template -void FeaturePose::display(std::ostream &os) const { - os << CLASS_NAME << "<" << name << ">: ("; - - try { - const Flags &fl = selectionSIN.accessCopy(); - bool first = true; - for (int i = 0; i < 6; ++i) - if (fl(i)) { - if (first) { - first = false; - } else { - os << ","; - } - os << featureNames[i]; - } - os << ") "; - } catch (ExceptionAbstract e) { - os << " selectSIN not set."; - } -} - - -} -} diff --git a/src/feature/feature-pose.cpp b/src/feature/feature-pose.cpp index 71a72e51d..b9e640546 100644 --- a/src/feature/feature-pose.cpp +++ b/src/feature/feature-pose.cpp @@ -38,13 +38,11 @@ typedef pinocchio::CartesianProductOperation< pinocchio::SpecialOrthogonalOperationTpl<3, double> > R3xSO3_t; typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; - template struct LG_t { typedef typename boost::mpl::if_c::type type; }; - typedef FeaturePose FeaturePose_t; typedef FeaturePose FeaturePoseSE3_t; template <> DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePose_t, "FeaturePose"); @@ -57,3 +55,297 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoseSE3_t, "FeaturePoseSE3"); static const MatrixHomogeneous Id(MatrixHomogeneous::Identity()); +template +FeaturePose::FeaturePose(const string &pointName) + : FeatureAbstract(pointName), + oMja(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMja"), + jaMfa(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jaMfa"), + oMjb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::oMjb"), + jbMfb(NULL, CLASS_NAME + "(" + name + ")::input(matrixHomo)::jbMfb"), + jaJja(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jaJja"), + jbJjb(NULL, CLASS_NAME + "(" + name + ")::input(matrix)::jbJjb"), + faMfbDes(NULL, + CLASS_NAME + "(" + name + ")::input(matrixHomo)::faMfbDes"), + faNufafbDes(NULL, + CLASS_NAME + "(" + name + ")::input(vector)::faNufafbDes"), + faMfb( + boost::bind(&FeaturePose::computefaMfb, this, _1, _2), + oMja << jaMfa << oMjb << jbMfb, + CLASS_NAME + "(" + name + ")::output(matrixHomo)::faMfb"), + q_faMfb(boost::bind(&FeaturePose::computeQfaMfb, this, _1, + _2), + faMfb, CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfb"), + q_faMfbDes(boost::bind(&FeaturePose::computeQfaMfbDes, + this, _1, _2), + faMfbDes, + CLASS_NAME + "(" + name + ")::output(vector7)::q_faMfbDes") { + oMja.setConstant(Id); + jaMfa.setConstant(Id); + jbMfb.setConstant(Id); + faMfbDes.setConstant(Id); + faNufafbDes.setConstant(Vector::Zero(6)); + + jacobianSOUT.addDependencies(q_faMfbDes << q_faMfb << jaJja << jbJjb); + + errorSOUT.addDependencies(q_faMfbDes << q_faMfb); + + signalRegistration(oMja << jaMfa << oMjb << jbMfb << jaJja << jbJjb); + signalRegistration(faMfb << errordotSOUT << faMfbDes << faNufafbDes); + + errordotSOUT.setFunction( + boost::bind(&FeaturePose::computeErrorDot, this, _1, _2)); + errordotSOUT.addDependencies(q_faMfbDes << q_faMfb << faNufafbDes); + + // Commands + // + { + using namespace dynamicgraph::command; + addCommand("keep", + makeCommandVoid1( + *this, &FeaturePose::servoCurrentPosition, + docCommandVoid1( + "modify the desired position to servo at current pos.", + "time"))); + } +} + +/* --------------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +template +static inline void check(const FeaturePose &ft) { + (void)ft; + assert(ft.oMja.isPlugged()); + assert(ft.jaMfa.isPlugged()); + assert(ft.oMjb.isPlugged()); + assert(ft.jbMfb.isPlugged()); + assert(ft.faMfbDes.isPlugged()); + assert(ft.faNufafbDes.isPlugged()); +} + +template +unsigned int &FeaturePose::getDimension(unsigned int &dim, + int time) { + sotDEBUG(25) << "# In {" << endl; + + const Flags &fl = selectionSIN.access(time); + + dim = 0; + for (int i = 0; i < 6; ++i) + if (fl(i)) + dim++; + + sotDEBUG(25) << "# Out }" << endl; + return dim; +} + +void toVector(const MatrixHomogeneous &M, Vector7 &v) { + v.head<3>() = M.translation(); + QuaternionMap(v.tail<4>().data()) = M.linear(); +} + +Vector7 toVector(const MatrixHomogeneous &M) { + Vector7 ret; + toVector(M, ret); + return ret; +} + +template +Matrix &FeaturePose::computeJacobian(Matrix &J, int time) { + typedef typename LG_t::type LieGroup_t; + + check(*this); + + q_faMfb.recompute(time); + q_faMfbDes.recompute(time); + + const int &dim = dimensionSOUT(time); + const Flags &fl = selectionSIN(time); + + const Matrix &_jbJjb = jbJjb(time); + + const MatrixHomogeneous &_jbMfb = + (jbMfb.isPlugged() ? jbMfb.accessCopy() : Id); + + const Matrix::Index cJ = _jbJjb.cols(); + J.resize(dim, cJ); + + MatrixTwist X; + Eigen::Matrix Jminus; + + buildFrom(_jbMfb.inverse(Eigen::Affine), X); + MatrixRotation faRfb = jaMfa.access(time).rotation().transpose() * + oMja.access(time).rotation().transpose() * + oMjb.access(time).rotation() * _jbMfb.rotation(); + if (boost::is_same::value) + X.topRows<3>().applyOnTheLeft(faRfb); + LieGroup_t().template dDifference( + q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); + + // Contribution of b: + // J = Jminus * X * jbJjb; + unsigned int rJ = 0; + for (unsigned int r = 0; r < 6; ++r) + if (fl(r)) + J.row(rJ++) = (Jminus * X).row(r) * _jbJjb; + + if (jaJja.isPlugged()) { + const Matrix &_jaJja = jaJja(time); + const MatrixHomogeneous &_jaMfa = + (jaMfa.isPlugged() ? jaMfa.accessCopy() : Id), + _faMfb = faMfb.accessCopy(); + + buildFrom((_jaMfa * _faMfb).inverse(Eigen::Affine), X); + if (boost::is_same::value) + X.topRows<3>().applyOnTheLeft(faRfb); + + // J -= (Jminus * X) * jaJja(time); + rJ = 0; + for (unsigned int r = 0; r < 6; ++r) + if (fl(r)) + J.row(rJ++).noalias() -= (Jminus * X).row(r) * _jaJja; + } + + return J; +} + +template +MatrixHomogeneous & +FeaturePose::computefaMfb(MatrixHomogeneous &res, int time) { + check(*this); + + res = (oMja(time) * jaMfa(time)).inverse(Eigen::Affine) * oMjb(time) * + jbMfb(time); + return res; +} + +template +Vector7 &FeaturePose::computeQfaMfb(Vector7 &res, int time) { + check(*this); + + toVector(faMfb(time), res); + return res; +} + +template +Vector7 &FeaturePose::computeQfaMfbDes(Vector7 &res, int time) { + check(*this); + + toVector(faMfbDes(time), res); + return res; +} + +template +Vector &FeaturePose::computeError(Vector &error, int time) { + typedef typename LG_t::type LieGroup_t; + check(*this); + + const Flags &fl = selectionSIN(time); + + Eigen::Matrix v; + LieGroup_t().difference(q_faMfbDes(time), q_faMfb(time), v); + + error.resize(dimensionSOUT(time)); + unsigned int cursor = 0; + for (unsigned int i = 0; i < 6; ++i) + if (fl(i)) + error(cursor++) = v(i); + + return error; +} + +// This function is responsible of converting the input velocity expressed with +// SE(3) convention onto a velocity expressed with the convention of this +// feature (R^3xSO(3) or SE(3)), in the right frame. +template +Vector6d convertVelocity(const MatrixHomogeneous &M, + const MatrixHomogeneous &Mdes, + const Vector &faNufafbDes) { + (void)M; + MatrixTwist X; + buildFrom(Mdes.inverse(Eigen::Affine), X); + return X * faNufafbDes; +} +template <> +Vector6d convertVelocity(const MatrixHomogeneous &M, + const MatrixHomogeneous &Mdes, + const Vector &faNufafbDes) { + Vector6d nu; + nu.head<3>() = + faNufafbDes.head<3>() - M.translation().cross(faNufafbDes.tail<3>()); + nu.tail<3>() = Mdes.rotation().transpose() * faNufafbDes.tail<3>(); + return nu; +} + +template +Vector &FeaturePose::computeErrorDot(Vector &errordot, + int time) { + typedef typename LG_t::type LieGroup_t; + check(*this); + + errordot.resize(dimensionSOUT(time)); + const Flags &fl = selectionSIN(time); + if (!faNufafbDes.isPlugged()) { + errordot.setZero(); + return errordot; + } + + q_faMfb.recompute(time); + q_faMfbDes.recompute(time); + faNufafbDes.recompute(time); + + const MatrixHomogeneous &_faMfbDes = faMfbDes(time); + + Eigen::Matrix Jminus; + + LieGroup_t().template dDifference( + q_faMfbDes.accessCopy(), q_faMfb.accessCopy(), Jminus); + Vector6d nu = convertVelocity(faMfb(time), _faMfbDes, + faNufafbDes.accessCopy()); + unsigned int cursor = 0; + for (unsigned int i = 0; i < 6; ++i) + if (fl(i)) + errordot(cursor++) = Jminus.row(i) * nu; + + return errordot; +} + +/* Modify the value of the reference (sdes) so that it corresponds + * to the current position. The effect on the servo is to maintain the + * current position and correct any drift. */ +template +void FeaturePose::servoCurrentPosition(const int &time) { + check(*this); + + const MatrixHomogeneous &_oMja = (oMja.isPlugged() ? oMja.access(time) : Id), + _jaMfa = + (jaMfa.isPlugged() ? jaMfa.access(time) : Id), + _oMjb = oMjb.access(time), + _jbMfb = + (jbMfb.isPlugged() ? jbMfb.access(time) : Id); + faMfbDes = (_oMja * _jaMfa).inverse(Eigen::Affine) * _oMjb * _jbMfb; +} + +static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"}; +template +void FeaturePose::display(std::ostream &os) const { + os << CLASS_NAME << "<" << name << ">: ("; + + try { + const Flags &fl = selectionSIN.accessCopy(); + bool first = true; + for (int i = 0; i < 6; ++i) + if (fl(i)) { + if (first) { + first = false; + } else { + os << ","; + } + os << featureNames[i]; + } + os << ") "; + } catch (ExceptionAbstract e) { + os << " selectSIN not set."; + } +} diff --git a/tests/features/test_feature_generic.cpp b/tests/features/test_feature_generic.cpp index 435f4514c..ca36505cc 100644 --- a/tests/features/test_feature_generic.cpp +++ b/tests/features/test_feature_generic.cpp @@ -270,6 +270,15 @@ MatrixHomogeneous randomM() { } typedef pinocchio::SE3 SE3; +typedef pinocchio::SpecialEuclideanOperationTpl<3, double> SE3_t; +typedef pinocchio::CartesianProductOperation< + pinocchio::VectorSpaceOperationTpl<3, double>, + pinocchio::SpecialOrthogonalOperationTpl<3, double> > + R3xSO3_t; +template struct LG_t { + typedef typename boost::mpl::if_c::type type; +}; Vector7 toVector(const pinocchio::SE3 &M) { Vector7 v; @@ -325,7 +334,6 @@ class TestFeaturePose : public FeatureTestBase { setJointFrame(); } - void _setFrame() { setSignal( feature_.jaMfa, From f8be8eb542e49ef5d8145e368f98acec481fe191 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:36:13 +0200 Subject: [PATCH 39/43] Revert "[cmake] Link is made PUBLIC." This reverts commit d7ccb2e2cc27f5af9fa57a09783cfe2f3b355594. --- tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d60266155..26f0957c4 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -122,7 +122,7 @@ FOREACH(path ${tests}) dynamic-graph::dynamic-graph) IF(UNIX) - TARGET_LINK_LIBRARIES(${test} PUBLIC ${CMAKE_DL_LIBS}) + TARGET_LINK_LIBRARIES(${test} ${CMAKE_DL_LIBS}) ENDIF(UNIX) ENDFOREACH(path ${tests}) From f90ea7ae12852f8d26cc36da39c68a62391dd8b8 Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:36:32 +0200 Subject: [PATCH 40/43] Revert "[cmake] Remove useless dependency to Boost_LIBRARIES" This reverts commit 4a53404755a940c8c95e31b7164c4311aac10ae7. --- tests/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 26f0957c4..86340da85 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -118,6 +118,7 @@ FOREACH(path ${tests}) TARGET_LINK_LIBRARIES(${test} PUBLIC ${PROJECT_NAME} + ${Boost_LIBRARIES} ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS} dynamic-graph::dynamic-graph) From a211f6eed1b7c5bbec8a4bb2d0c243eb19cad06c Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:37:01 +0200 Subject: [PATCH 41/43] Revert "[cmake] Fix target_link_libraries to fix the link dependencies and interface." This reverts commit b62848e588b76a9490f278764e14c41de50500ee. --- tests/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 86340da85..e0666b881 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -115,13 +115,13 @@ FOREACH(path ${tests}) GET_FILENAME_COMPONENT(test ${path} NAME) ADD_UNIT_TEST(${test} ${path}.cpp) - TARGET_LINK_LIBRARIES(${test} - PUBLIC - ${PROJECT_NAME} - ${Boost_LIBRARIES} + TARGET_LINK_LIBRARIES(${test} ${PROJECT_NAME} ${Boost_LIBRARIES} ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS} dynamic-graph::dynamic-graph) + TARGET_INCLUDE_DIRECTORIES(${test} + SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) + IF(UNIX) TARGET_LINK_LIBRARIES(${test} ${CMAKE_DL_LIBS}) ENDIF(UNIX) From 7ddb9724c3d5d64f4f492a905b015a298ae511ba Mon Sep 17 00:00:00 2001 From: Olivier Stasse Date: Sun, 12 Jul 2020 11:37:14 +0200 Subject: [PATCH 42/43] Revert "[cmake] Add target_include_directories for Boost." This reverts commit beeb05d9683c433de603f50d6ed2923243918868. --- CMakeLists.txt | 2 -- tests/CMakeLists.txt | 6 ------ 2 files changed, 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1415ecdc4..642ad1725 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -170,8 +170,6 @@ SET(${PROJECT_NAME}_SOURCES ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $) -TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} - SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${Boost_LIBRARIES} dynamic-graph::dynamic-graph pinocchio::pinocchio) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e0666b881..5b6311d8a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -106,9 +106,6 @@ IF(UNIX) ADD_EXECUTABLE(test_abstract_interface tools/test_abstract_interface.cpp) TARGET_LINK_LIBRARIES(test_abstract_interface pluginabstract ${CMAKE_DL_LIBS} ${Boost_LIBRARIES}) - TARGET_INCLUDE_DIRECTORIES(test_abstract_interface - SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) - ENDIF(UNIX) FOREACH(path ${tests}) @@ -119,9 +116,6 @@ FOREACH(path ${tests}) ${TEST_${test}_LIBS} ${TEST_${test}_EXT_LIBS} dynamic-graph::dynamic-graph) - TARGET_INCLUDE_DIRECTORIES(${test} - SYSTEM PUBLIC ${Boost_INCLUDE_DIRS}) - IF(UNIX) TARGET_LINK_LIBRARIES(${test} ${CMAKE_DL_LIBS}) ENDIF(UNIX) From 1b7e086509bdba58ecf4c9565cd75dfb9f468d7d Mon Sep 17 00:00:00 2001 From: NoelieRamuzat Date: Fri, 31 Jul 2020 10:55:09 +0200 Subject: [PATCH 43/43] =?UTF-8?q?[generic-device]=C2=A0Take=20remarks=20in?= =?UTF-8?q?to=20account?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Use CMAKE path to find the urdf --- CMakeLists.txt | 9 ++----- src/tools/device.cpp | 2 +- src/tools/robot-simu.cpp | 2 +- tests/CMakeLists.txt | 15 +++++------ tests/tools/test_generic_device.cpp | 42 +++++++++++++++++++++-------- 5 files changed, 42 insertions(+), 28 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f9035fd22..4aefb726a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ SET(${PROJECT_NAME}_HEADERS include/${CUSTOM_HEADER_DIR}/debug.hh include/${CUSTOM_HEADER_DIR}/derivator.hh include/${CUSTOM_HEADER_DIR}/device.hh + include/${CUSTOM_HEADER_DIR}/generic-device.hh include/${CUSTOM_HEADER_DIR}/double-constant.hh include/${CUSTOM_HEADER_DIR}/event.hh include/${CUSTOM_HEADER_DIR}/exception-abstract.hh @@ -164,6 +165,7 @@ SET(${PROJECT_NAME}_SOURCES src/tools/utils-windows src/tools/periodic-call src/tools/device + src/tools/generic-device src/tools/trajectory src/tools/robot-utils src/matrix/matrix-svd @@ -171,13 +173,6 @@ SET(${PROJECT_NAME}_SOURCES src/utils/stop-watch ) -IF (YAML_CPP_FOUND) - SET(${PROJECT_NAME}_HEADERS ${${PROJECT_NAME}_HEADERS} - include/${CUSTOM_HEADER_DIR}/generic-device.hh ) - SET(${PROJECT_NAME}_SOURCES ${${PROJECT_NAME}_SOURCES} - src/tools/generic-device.cpp ) -ENDIF () - ADD_LIBRARY(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $) diff --git a/src/tools/device.cpp b/src/tools/device.cpp index b27b35c7c..2ecf4be70 100644 --- a/src/tools/device.cpp +++ b/src/tools/device.cpp @@ -535,4 +535,4 @@ void Device::cmdDisplay() { std::cout << name << ": " << state_ << endl << "sanityCheck: " << sanityCheck_ << endl << "controlInputType:" << controlInputType_ << endl; -} \ No newline at end of file +} diff --git a/src/tools/robot-simu.cpp b/src/tools/robot-simu.cpp index e5e9a736f..a456953d7 100644 --- a/src/tools/robot-simu.cpp +++ b/src/tools/robot-simu.cpp @@ -36,4 +36,4 @@ RobotSimu::RobotSimu(const std::string &inName) : Device(inName) { makeDirectSetter(*this, &this->timestep_, docstring)); } } // namespace sot -} // namespace dynamicgraph \ No newline at end of file +} // namespace dynamicgraph diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7c7b881e0..c0ccc15a1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -48,6 +48,9 @@ SET(TEST_test_feature_generic_EXT_LIBS SET(TEST_test_device_EXT_LIBS pinocchio::pinocchio) +SET(TEST_test_generic_device_LIBS + yaml-cpp) + SET(TEST_test_filter_differentiator_LIBS filter-differentiator) @@ -85,6 +88,7 @@ SET(tests tools/test_boost tools/test_device + tools/test_generic_device tools/test_mailbox tools/test_matrix tools/test_robot_utils @@ -95,13 +99,8 @@ SET(tests matrix/test_operator ) -IF (YAML_CPP_FOUND) - SET(tests ${tests} tools/test_generic_device ) - configure_file(tools/sot_controller.yaml ${CMAKE_CURRENT_BINARY_DIR} COPYONLY) - configure_file(tools/sot_params.yaml ${CMAKE_CURRENT_BINARY_DIR} COPYONLY) - SET(TEST_test_generic_device_LIBS - yaml-cpp) -ENDIF () +configure_file(tools/sot_controller.yaml ${CMAKE_CURRENT_BINARY_DIR} COPYONLY) +configure_file(tools/sot_params.yaml ${CMAKE_CURRENT_BINARY_DIR} COPYONLY) # TODO IF(WIN32) @@ -136,4 +135,4 @@ ENDFOREACH(path ${tests}) IF(BUILD_PYTHON_INTERFACE) ADD_SUBDIRECTORY(python) -ENDIF(BUILD_PYTHON_INTERFACE) \ No newline at end of file +ENDIF(BUILD_PYTHON_INTERFACE) diff --git a/tests/tools/test_generic_device.cpp b/tests/tools/test_generic_device.cpp index d3a32902f..090b5280a 100644 --- a/tests/tools/test_generic_device.cpp +++ b/tests/tools/test_generic_device.cpp @@ -19,6 +19,8 @@ using namespace std; #include +#include +#include #include #include #include @@ -217,17 +219,35 @@ namespace dg = dynamicgraph; BOOST_AUTO_TEST_CASE(test_generic_device) { unsigned int debug_mode = 5; - 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. @@ -245,12 +265,12 @@ BOOST_AUTO_TEST_CASE(test_generic_device) { return; } - /// Fix constant vector for the control entry in position + /// Fix constant vector for the control input in position dg::Vector aStateVector(30); for (unsigned int i = 0; i < 30; i++) { aStateVector[i] = -0.5; } - aDevice.stateSIN.setConstant(aStateVector); // entry signal in position + aDevice.stateSIN.setConstant(aStateVector); // input signal in position for (unsigned int i = 0; i < 2000; i++) aDevice.motorcontrolSOUT_.recompute(i);