diff --git a/CMakeLists.txt b/CMakeLists.txt index 9934f4fb3..4aefb726a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ OPTION(INSTALL_PYTHON_INTERFACE_ONLY "Install *ONLY* the python bindings" OFF) OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON) # Project configuration +SET(CMAKE_CXX_STANDARD 11) IF(NOT INSTALL_PYTHON_INTERFACE_ONLY) SET(PROJECT_USE_CMAKE_EXPORT TRUE) ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY) @@ -40,6 +41,8 @@ ADD_PROJECT_DEPENDENCY(example-robot-data) SET(BOOST_COMPONENTS filesystem system thread program_options unit_test_framework regex) +ADD_REQUIRED_DEPENDENCY("yaml-cpp") + IF(BUILD_PYTHON_INTERFACE) FINDPYTHON() STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR}) @@ -69,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 @@ -161,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 @@ -174,7 +179,7 @@ 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) + dynamic-graph::dynamic-graph pinocchio::pinocchio yaml-cpp) IF(SUFFIX_SO_VERSION) SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION}) diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt index 67618fd74..ed8c407f8 100644 --- a/include/CMakeLists.txt +++ b/include/CMakeLists.txt @@ -1 +1 @@ -# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST +# Copyright 2010, François Bleibel, Olivier Stasse, JRL, CNRS/AIST \ No newline at end of file diff --git a/include/sot/core/device.hh b/include/sot/core/device.hh index 0b835dc3f..d1e7fc801 100644 --- a/include/sot/core/device.hh +++ b/include/sot/core/device.hh @@ -175,4 +175,4 @@ private: } // 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/tools/generic-device.cpp b/src/tools/generic-device.cpp new file mode 100644 index 000000000..0a2b13c48 --- /dev/null +++ b/src/tools/generic-device.cpp @@ -0,0 +1,832 @@ +/* + * Copyright 2019, CNRS + * Author: Olivier Stasse + * + * Please check LICENSE.txt for licensing + * + */ + +/* --------------------------------------------------------------------- */ +/* --- INCLUDE --------------------------------------------------------- */ +/* --------------------------------------------------------------------- */ + +/* SOT */ +#define ENABLE_RT_LOG + +#include "sot/core/generic-device.hh" +#include +using namespace std; + +#include +#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 (unsigned(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/tests/CMakeLists.txt b/tests/CMakeLists.txt index d60266155..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,6 +99,9 @@ SET(tests matrix/test_operator ) +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) LIST(REMOVE_ITEM tests tools/test_mailbox) diff --git a/tests/tools/example_of_yaml_config_for_device.yaml b/tests/tools/example_of_yaml_config_for_device.yaml new file mode 100644 index 000000000..d0faa7922 --- /dev/null +++ b/tests/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/tests/tools/sot_controller.yaml b/tests/tools/sot_controller.yaml new file mode 100644 index 000000000..d406f6fd0 --- /dev/null +++ b/tests/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/tests/tools/sot_params.yaml b/tests/tools/sot_params.yaml new file mode 100644 index 000000000..1f33f8bf9 --- /dev/null +++ b/tests/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/tests/tools/test_generic_device.cpp b/tests/tools/test_generic_device.cpp new file mode 100644 index 000000000..090b5280a --- /dev/null +++ b/tests/tools/test_generic_device.cpp @@ -0,0 +1,323 @@ +/* + * Copyright 2018, + * Olivier Stasse, + * + * CNRS + * See LICENSE.txt + * + */ + +#include +#include + +#include + +#ifndef WIN32 +#include +#endif + +using namespace std; + +#include +#include +#include +#include +#include +#include + +#include +#include + + +#define BOOST_TEST_MODULE test-device + +void CreateYAMLFILE() { + YAML::Emitter yaml_out; + 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); + if (of.is_open()) { + of << aNode; + } + of.close(); +} + +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("sot_controller.yaml"); + std::string yaml_string_controller; + yaml_string_controller.assign((std::istreambuf_iterator(yaml_file_controller) ), + (std::istreambuf_iterator() ) ); + aDevice.ParseYAMLString(yaml_string_controller); + + std::ifstream yaml_file_params("sot_params.yaml"); + std::string yaml_string_params; + yaml_string_params.assign((std::istreambuf_iterator(yaml_file_params) ), + (std::istreambuf_iterator() ) ); + aDevice.ParseYAMLString(yaml_string_params); + + // 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; +} + +namespace dg = dynamicgraph; + +BOOST_AUTO_TEST_CASE(test_generic_device) { + unsigned int debug_mode = 5; + + // Get environment variable CMAKE_PREFIX_PATH + const string s_cmake_prefix_path = getenv( "CMAKE_PREFIX_PATH" ); + + // Read the various paths + vector paths; + boost::split(paths, s_cmake_prefix_path, boost::is_any_of(":;")); + + // Search simple_humanoid.urdf + string filename=""; + for (auto test_path : paths) + { + filename = test_path + + string("/share/simple_humanoid_description/urdf/simple_humanoid.urdf"); + if ( boost::filesystem::exists(filename)) + break; + } + + // If not found fails + if (filename.size()==0) + { + cerr << "Unable to find simple_humanoid.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. + dg::sot::GenericDevice aDevice(std::string("simple_humanoid")); + aDevice.setDebugMode(debug_mode); + aDevice.setURDFModel(robot_description); + + // 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) { + std::cout << "Error while reading YAML files" << std::endl; + BOOST_CHECK(false); + return; + } + + /// 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); // input signal in position + + for (unsigned int i = 0; i < 2000; i++) + aDevice.motorcontrolSOUT_.recompute(i); + + const urdf::ModelInterfaceSharedPtr aModel = aDevice.getModel(); + + const dg::Vector & aControl = aDevice.motorcontrolSOUT_(2001); + double diff = 0, ldiff; + + vector< ::urdf::JointSharedPtr > urdf_joints = aDevice.getURDFJoints(); + + dgsot::JointSHWControlType_iterator it_control_type; + for (it_control_type = aDevice.jointDevices_.begin(); + it_control_type != aDevice.jointDevices_.end(); + it_control_type++) { + int lctl_index = it_control_type->second.control_index; + int u_index = it_control_type->second.urdf_index; + std::cout << "\n ########### \n " << std::endl; + std::cout << "urdf_joints: " << urdf_joints[u_index]->name << std::endl; + + if (it_control_type->second.SoTcontrol == dgsot::POSITION) { + if (u_index != -1 && (urdf_joints[u_index]->limits)) { + double lowerLim = urdf_joints[u_index]->limits->lower; + ldiff = (aControl[lctl_index] - lowerLim); + diff += ldiff; + std::cout << "Position lowerLim: " << lowerLim << "\n" + << "motorcontrolSOUT: " << aControl[lctl_index] << " -- " + << "diff: " << ldiff << "\n" + << "Velocity limit: " << urdf_joints[u_index]->limits->velocity + << std::endl; + } + } else 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" + << std::endl; + } + } else { + std::cout << "motorcontrolSOUT: " << aControl[lctl_index] << std::endl; + } + } + std::cout << "\n ########### \n " << std::endl; + std::cout << "totalDiff: " << diff << std::endl; + + BOOST_CHECK_CLOSE(diff, 23.567, 1e-3); +}