Commit af1bad3f authored by Olivier Stasse's avatar Olivier Stasse
Browse files

pinocchio-device: Add YAML parsing to make the map with the sensors.

WIP.
parent a6224adb
Pipeline #2042 failed with stage
in 4 minutes and 36 seconds
......@@ -15,22 +15,33 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// URDF DOM
#include <urdf_parser/urdf_parser.h>
/// YAML CPP
#include <yaml-cpp/yaml.h>
/* -- MaaL --- */
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
/// dg
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
/// sot-core
#include "sot/core/periodic-call.hh"
#include <sot/core/matrix-geometry.hh>
#include "sot/core/api.hh"
#include <sot/core/abstract-sot-external-interface.hh>
/// pinocchio
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/math/quaternion.hpp"
namespace dgsot=dynamicgraph::sot;
namespace dg=dynamicgraph;
namespace dynamicgraph {
namespace sot {
......@@ -51,17 +62,51 @@ namespace dynamicgraph {
//@}
/// \brief Store information on each joint.
struct JointSoTHWControlType
{
/// Defines the control from the Stack-of-Tasks side (for instance, velocity)
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 pinocchio/URDF index.
int pinocchio_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
{
dg::Signal<MatrixRotation, int> attitudeSOUT;
dg::Signal<dg::Vector,int> accelerometerSOUT;
dg::Signal<dg::Vector,int> gyrometerSOUT;
};
typedef std::map<std::string,JointSoTHWControlType>::iterator
JointSHControlType_iterator;
JointSHWControlType_iterator;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -75,14 +120,6 @@ namespace dynamicgraph {
return CLASS_NAME;
}
enum ForceSignalSource
{
FORCE_SIGNAL_RLEG,
FORCE_SIGNAL_LLEG,
FORCE_SIGNAL_RARM,
FORCE_SIGNAL_LARM
};
protected:
/// \name Vectors related to the state.
......@@ -111,7 +148,6 @@ namespace dynamicgraph {
/// Maps of joint devices.
std::map<std::string,JointSoTHWControlType> jointPinocchioDevices_;
///
bool withForceSignals[4];
PeriodicCall periodicCallBefore_;
PeriodicCall periodicCallAfter_;
......@@ -158,32 +194,19 @@ namespace dynamicgraph {
/// This entity needs a control vector to be send to the hardware.
/// The control vector can be position, velocity and effort.
/// It depends on each of the actuator
dynamicgraph::SignalPtr<dg::Vector,int> controlSIN;
/// \name This part is specific to robot where a stabilizer is provided outside the
/// SoT framework and needs input.
/// @{
/// Input signal handling the attitude of the freeflyer.
dynamicgraph::SignalPtr<dg::Vector,int> attitudeSIN;
/// Input signal handling the ZMP of the system
dynamicgraph::SignalPtr<dg::Vector,int> zmpSIN;
///@}
dg::SignalPtr<dg::Vector,int> controlSIN;
/// \name PinocchioDevice current state.
/// \{
/// \brief Output integrated state from control.
dynamicgraph::Signal<dg::Vector,int> stateSOUT;
dg::Signal<dg::Vector,int> stateSOUT_;
/// \brief Output integrated velocity from control
dynamicgraph::Signal<dg::Vector,int> velocitySOUT;
dg::Signal<dg::Vector,int> *velocitySOUT_;
/// \brief Output attitude provided by the hardware
/// Typically this can be provided by an external estimator
/// such an integrated/hardware implemented EKF.
dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
/*! \brief The current state of the robot from the command viewpoint. */
dynamicgraph::Signal<dg::Vector,int> motorcontrolSOUT;
dynamicgraph::Signal<dg::Vector,int> previousControlSOUT;
/*! \brief The ZMP reference send by the previous controller. */
dynamicgraph::Signal<dg::Vector,int> ZMPPreviousControllerSOUT;
dg::Signal<dg::Vector,int> motorcontrolSOUT_;
dg::Signal<dg::Vector,int> previousControlSOUT_;
/// \}
/// \name Real robot current state
......@@ -192,16 +215,41 @@ namespace dynamicgraph {
/// does *not* match the state control input signal.
/// \{
/// Motor positions
dynamicgraph::Signal<dg::Vector, int> robotState_;
dg::Signal<dg::Vector, int> robotState_;
/// Motor velocities
dynamicgraph::Signal<dg::Vector, int> robotVelocity_;
dg::Signal<dg::Vector, int> robotVelocity_;
/// The force torque sensors
dynamicgraph::Signal<dg::Vector,int>* forcesSOUT[4];
/// Motor torques
/// \todo why pseudo ?
dynamicgraph::Signal<dg::Vector,int> pseudoTorqueSOUT;
std::vector<dg::Signal<dg::Vector,int>*> forcesSOUT_;
/// The imu sensors
std::vector<dg::Signal<dg::Vector, int>*> imuSOUT_;
/// Motor or pseudo torques (estimated or build from PID)
dg::Signal<dg::Vector,int> * pseudoTorqueSOUT_;
/// Temperature signal
dg::Signal<dg::Vector,int> * temperatureSOUT_;
/// Current signal
dg::Signal<dg::Vector,int> * currentsSOUT_;
/// Motor angles signal;
dg::Signal<dg::Vector, int> * motor_anglesSOUT_;
/// Joint angles signal;
dg::Signal<dg::Vector, int> * joint_anglesSOUT_;
/// \}
/// 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<std::string,dgsot::SensorValues> &sensorsIn);
void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void nominalSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
/// \brief Provides to the robot the control information.
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
///@}
protected:
void setControlType(const std::string &strCtrlType,
ControlType &aCtrlType);
......@@ -211,6 +259,7 @@ namespace dynamicgraph {
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
......@@ -224,13 +273,46 @@ namespace dynamicgraph {
/// pinocchio and the contact forces in order to estimate
/// the joint torques for the given acceleration.
virtual void integrate( const double & dt );
protected:
/// \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 sencors from YAML-cpp node.
int ParseYAMLJointSensor(std::string & joint_name,
YAML::Node &aJointSensors);
/// @}
/// \name Signals related methods
///@{
/// \brief Creates a signal called PinocchioDevice(DeviceName)::output(vector6)::force_sensor_name
void CreateAForceSignal(const std::string & force_sensor_name);
/// \brief Creates a signal called PinocchioDevice(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;
public:
virtual void setRoot( const dg::Matrix & root );
/// Protected methods for internal variables filling
void setSensorsForce(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsIMU(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsEncoders(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsVelocities(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsTorquesCurrents(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsGains(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
public:
virtual void setRoot( const dg::Matrix & root );
virtual void setRoot( const MatrixHomogeneous & worldMwaist );
private:
......@@ -240,6 +322,15 @@ namespace dynamicgraph {
// Pinocchio Model of the robot
se3::Model model_;
// Debug mode
int debug_mode_;
// Intermediate index when parsing YAML file.
int temperature_index_,velocity_index_,
current_index_,torque_index_,
joint_angles_index_,
motor_angles_index_
;
public:
const se3::Model & getModel()
{ return model_;}
......
/*
* Copyright 2010,
* Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
* Copyright 2019, CNRS
* Author: Olivier Stasse
*
* CNRS
*
* This file is part of sot-core.
* sot-core is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-core is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-core. If not, see <http://www.gnu.org/licenses/>.
* Please check LICENSE.txt for licensing
*
*/
/* --------------------------------------------------------------------- */
......@@ -43,7 +32,17 @@ using namespace dynamicgraph;
const std::string PinocchioDevice::CLASS_NAME = "PinocchioDevice";
JointSoTHWControlType::JointSoTHWControlType():
control_index(-1)
,pinocchio_index(-1)
,temperature_index(-1)
,velocity_index(-1)
,current_index(-1)
,torque_index(-1)
,joint_angle_index(-1)
,motor_angle_index(-1)
{
}
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -57,9 +56,14 @@ const MatrixHomogeneous& PinocchioDevice::freeFlyerPose() const
PinocchioDevice::
~PinocchioDevice( )
{
for( unsigned int i=0; i<4; ++i ) {
delete forcesSOUT[i];
for( unsigned int i=0; i<forcesSOUT_.size(); ++i ) {
delete forcesSOUT_[i];
}
for( unsigned int i=0; i<imuSOUT_.size(); ++i ) {
delete imuSOUT_[i];
}
}
PinocchioDevice::
......@@ -68,55 +72,36 @@ PinocchioDevice( const std::string& n )
,position_(6)
,sanityCheck_(true)
,controlSIN( NULL,"PinocchioDevice("+n+")::input(double)::control" )
,attitudeSIN(NULL,"PinocchioDevice("+ n +")::input(vector3)::attitudeIN")
,zmpSIN(NULL,"PinocchioDevice("+n+")::input(vector3)::zmp")
,stateSOUT( "PinocchioDevice("+n+")::output(vector)::state" )
//,attitudeSIN(NULL,"PinocchioDevice::input(matrixRot)::attitudeIN")
,velocitySOUT( "PinocchioDevice("+n+")::output(vector)::velocity" )
,attitudeSOUT( "PinocchioDevice("+n+")::output(matrixRot)::attitude" )
,motorcontrolSOUT ( "PinocchioDevice("+n+")::output(vector)::motorcontrol" )
,previousControlSOUT( "PinocchioDevice("+n+")::output(vector)::previousControl" )
,ZMPPreviousControllerSOUT( "PinocchioDevice("+n+")::output(vector)::zmppreviouscontroller" )
,stateSOUT_( "PinocchioDevice("+n+")::output(vector)::state" )
,velocitySOUT_( "PinocchioDevice("+n+")::output(vector)::velocity" )
,motorcontrolSOUT_ ( "PinocchioDevice("+n+")::output(vector)::motorcontrol" )
,previousControlSOUT_( "PinocchioDevice("+n+")::output(vector)::previousControl" )
,robotState_ ("PinocchioDevice("+n+")::output(vector)::robotState")
,robotVelocity_ ("PinocchioDevice("+n+")::output(vector)::robotVelocity")
,pseudoTorqueSOUT("PinocchioDevice("+n+")::output(vector)::ptorque" )
,ffPose_()
,forceZero6 (6)
,debug_mode_(5)
,temperature_index_(0)
,velocity_index_(0)
,current_index_(0)
,torque_index_(0)
,joint_angles_index_(0)
,motor_angles_index_(0)
{
forceZero6.fill (0);
/* --- SIGNALS --- */
for( int i=0;i<4;++i ){ withForceSignals[i] = false; }
forcesSOUT[0] =
new Signal<Vector, int>("PinocchioDevice("+n+")::output(vector6)::forceRLEG");
forcesSOUT[1] =
new Signal<Vector, int>("PinocchioDevice("+n+")::output(vector6)::forceLLEG");
forcesSOUT[2] =
new Signal<Vector, int>("PinocchioDevice("+n+")::output(vector6)::forceRARM");
forcesSOUT[3] =
new Signal<Vector, int>("PinocchioDevice("+n+")::output(vector6)::forceLARM");
signalRegistration( controlSIN
<< stateSOUT
<< stateSOUT_
<< robotState_
<< robotVelocity_
<< velocitySOUT
<< attitudeSOUT
<< attitudeSIN
<< zmpSIN
<< *forcesSOUT[0]
<< *forcesSOUT[1]
<< *forcesSOUT[2]
<< *forcesSOUT[3]
<< previousControlSOUT
<< pseudoTorqueSOUT
<< motorcontrolSOUT
<< ZMPPreviousControllerSOUT );
position_.fill(.0); stateSOUT.setConstant( position_ );
<< velocitySOUT_
<< previousControlSOUT_
<< motorcontrolSOUT_);
position_.fill(.0); stateSOUT_.setConstant( position_ );
velocity_.resize(position_.size()); velocity_.setZero();
velocitySOUT.setConstant( velocity_ );
velocitySOUT_.setConstant( velocity_ );
/* --- Commands --- */
{
......@@ -201,15 +186,15 @@ setState( const Vector& st )
if (sanityCheck_) {
}
position_ = st;
stateSOUT .setConstant( position_ );
motorcontrolSOUT .setConstant( position_ );
stateSOUT_ .setConstant( position_ );
motorcontrolSOUT_ .setConstant( position_ );
}
void PinocchioDevice::
setVelocity( const Vector& vel )
{
velocity_ = vel;
velocitySOUT .setConstant( velocity_ );
velocitySOUT_ .setConstant( velocity_ );
}
void PinocchioDevice::
......@@ -268,6 +253,7 @@ setControlPos(const std::string &jointName,
jointPinocchioDevices_[jointName].control_index = index;
}
void PinocchioDevice::
setURDFModel(const std::string &aURDFModel)
{
......@@ -286,7 +272,7 @@ setURDFModel(const std::string &aURDFModel)
void PinocchioDevice::
increment( const double & dt )
{
int time = stateSOUT.getTime();
int time = stateSOUT_.getTime();
sotDEBUG(25) << "Time : " << time << std::endl;
// Run Synchronous commands and evaluate signals outside the main
......@@ -324,13 +310,8 @@ increment( const double & dt )
sotDEBUG(25) << "q" << time << " = " << position_ << endl;
/* Position the signals corresponding to sensors. */
stateSOUT .setConstant( position_ ); stateSOUT.setTime( time+1 );
stateSOUT_ .setConstant( position_ ); stateSOUT_.setTime( time+1 );
for( int i=0;i<4;++i ){
if( !withForceSignals[i] ) forcesSOUT[i]->setConstant(forceZero6);
}
Vector zmp(3); zmp.fill( .0 );
ZMPPreviousControllerSOUT .setConstant( zmp );
// Run Synchronous commands and evaluate signals outside the main
// connected component of the graph.
......@@ -359,7 +340,7 @@ increment( const double & dt )
// Others signals.
motorcontrolSOUT .setConstant( position_ );
motorcontrolSOUT_ .setConstant( position_ );
}
......@@ -376,7 +357,7 @@ void PinocchioDevice::integrate( const double & dt )
bool shouldIntegrateVelocity=false;
JointSHControlType_iterator it_control_type;
JointSHWControlType_iterator it_control_type;
for(it_control_type = jointPinocchioDevices_.begin();
it_control_type != jointPinocchioDevices_.end();
it_control_type++)
......@@ -458,14 +439,538 @@ void PinocchioDevice::integrate( const double & dt )
}
}
stateSOUT .setConstant( position_ );
velocitySOUT .setConstant( velocity_ );
stateSOUT_ .setConstant( position_ );
velocitySOUT_ .setConstant( velocity_ );
}
}
int PinocchioDevice::
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<string>() << std::endl;
}
std::string name_elt_of_sot_controller = it->first.as<string>();
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;
}
}
return 0;
}
int PinocchioDevice::
ParseYAMLJointSensor(std::string & joint_name,
YAML::Node &aJointSensor)
{
JointSoTHWControlType & aJointSoTHWControlType =
jointPinocchioDevices_[joint_name];
if (debug_mode_>1)
std::cout << "JointSensor for " << joint_name << std::endl;
for (std::size_t i=0;i<aJointSensor.size();i++)
{
std::string aSensor = aJointSensor[i].as<string>();
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_angles_index_;
joint_angles_index_++;
}
else if (aSensor=="motor_angles")
{
aJointSoTHWControlType.motor_angle_index = motor_angles_index_;
motor_angles_index_++;
}
}
return 0;
}
int PinocchioDevice::
ParseYAMLMapHardware(YAML::Node & map_hs_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<string>() << std::endl;
}
std::string jointName = it->first.as<string>();
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<string>();
if (debug_mode_>1)
std::cout << "-- key:" << aKey << std::endl;
if (aKey=="hw")
{
std::string value = it2->second.as<string>();
if (debug_mode_>1)
std::cout << "-- Value: " << value << std::endl;
setHWControlType(jointName,value);
}
else if (aKey=="sot")
{
std::string value = it2->second.as<string>();
if (debug_mode_>1)
std::cout << "-- Value: " << value << std::endl;
setSoTControlType(jointName,value);
}
else if (aKey=="controlPos")