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

Adding pinocchio-device.

parent 9c7ae90f
Pipeline #2037 failed with stage
in 6 minutes and 45 seconds
......@@ -46,6 +46,7 @@ ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.3.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-tools >= 2.0.0")
ADD_OPTIONAL_DEPENDENCY("yaml-cpp")
SET(BOOST_COMPONENTS filesystem system unit_test_framework)
......@@ -70,6 +71,7 @@ SET(plugins
angle-estimator
waist-attitude-from-sensor
zmp-from-forces
pinocchio-device
)
SET(LIBRARY_NAME ${PROJECT_NAME})
......
/*
* Copyright 2010-2018, CNRS
* Florent Lamiraux
* Olivier Stasse
*
* CNRS
*
* See LICENSE.txt
*/
#ifndef SOT_PINOCCHIO_DEVICE_HH
#define SOT_PINOCCHIO_DEVICE_HH
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <urdf_parser/urdf_parser.h>
/* -- MaaL --- */
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include "sot/core/periodic-call.hh"
#include <sot/core/matrix-geometry.hh>
#include "sot/core/api.hh"
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/math/quaternion.hpp"
namespace dynamicgraph {
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"
};
//@}
struct JointSoTHWControlType
{
ControlType SoTcontrol;
ControlType HWcontrol;
int control_index;
int pinocchio_index;
};
typedef std::map<std::string,JointSoTHWControlType>::iterator
JointSHControlType_iterator;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOT_CORE_EXPORT PinocchioDevice
: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:
/// \name Vectors related to the state.
///@{
/// State vector of the robot (deprecated)
dg::Vector state_;
/// Position of the robot wrt pinocchio.
Eigen::VectorXd position_;
/// Velocity of the robot wrt pinocchio.
Eigen::VectorXd velocity_;
/// Acceleration vector of each actuator.
dg::Vector acceleration_;
/// Torque vector of each actuator.
dg::Vector torque_;
///@}
bool sanityCheck_;
/// Specifies the control input by each element of the state vector.
std::map<std::string,ControlType> sotControlType_;
std::map<std::string,ControlType> hwControlType_;
/// Maps of joint devices.
std::map<std::string,JointSoTHWControlType> jointPinocchioDevices_;
///
bool withForceSignals[4];
PeriodicCall periodicCallBefore_;
PeriodicCall periodicCallAfter_;
public:
/* --- CONSTRUCTION --- */
PinocchioDevice(const std::string& name);
/* --- DESTRUCTION --- */
virtual ~PinocchioDevice();
virtual void setState(const dg::Vector& st);
void setVelocitySize(const unsigned int& size);
virtual void setVelocity(const dg::Vector & vel);
/// 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 double & dt = 5e-2);
/// Read directly the URDF model
void setURDFModel(const std::string &aURDFModel);
/// \name Sanity check parameterization
/// \{
void setSanityCheck (const bool & enableCheck);
/// \}
/// \name 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 PinocchioDevice& 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, 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;
///@}
/// \name PinocchioDevice current state.
/// \{
/// \brief Output integrated state from control.
dynamicgraph::Signal<dg::Vector,int> stateSOUT;
/// \brief Output integrated velocity from control
dynamicgraph::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;
/// \}
/// \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<dg::Vector, int> robotState_;
/// Motor velocities
dynamicgraph::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;
/// \}
protected:
void setControlType(const std::string &strCtrlType,
ControlType &aCtrlType);
/// 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;
// Pinocchio Model of the robot
se3::Model model_;
public:
const se3::Model & getModel()
{ return model_;}
};
} // namespace sot
} // namespace dynamicgraph
#endif /* #ifndef SOT_PINOCCHIO_DEVICE_HH */
/*
* Copyright 2010,
* Nicolas Mansard, Olivier Stasse, François Bleibel, Florent Lamiraux
*
* 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/>.
*/
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#define ENABLE_RT_LOG
#include "sot-dynamic-pinocchio/pinocchio-device.hh"
#include <sot/core/debug.hh>
using namespace std;
#include <dynamic-graph/factory.h>
#include <dynamic-graph/real-time-logger.h>
#include <dynamic-graph/all-commands.h>
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/matrix-geometry.hh>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
const std::string PinocchioDevice::CLASS_NAME = "PinocchioDevice";
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
const MatrixHomogeneous& PinocchioDevice::freeFlyerPose() const
{
return ffPose_;
}
PinocchioDevice::
~PinocchioDevice( )
{
for( unsigned int i=0; i<4; ++i ) {
delete forcesSOUT[i];
}
}
PinocchioDevice::
PinocchioDevice( const std::string& n )
:Entity(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" )
,robotState_ ("PinocchioDevice("+n+")::output(vector)::robotState")
,robotVelocity_ ("PinocchioDevice("+n+")::output(vector)::robotVelocity")
,pseudoTorqueSOUT("PinocchioDevice("+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<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
<< robotState_
<< robotVelocity_
<< velocitySOUT
<< attitudeSOUT
<< attitudeSIN
<< zmpSIN
<< *forcesSOUT[0]
<< *forcesSOUT[1]
<< *forcesSOUT[2]
<< *forcesSOUT[3]
<< previousControlSOUT
<< pseudoTorqueSOUT
<< motorcontrolSOUT
<< ZMPPreviousControllerSOUT );
position_.fill(.0); stateSOUT.setConstant( position_ );
velocity_.resize(position_.size()); velocity_.setZero();
velocitySOUT.setConstant( velocity_ );
/* --- Commands --- */
{
std::string docstring;
docstring =
"\n"
" Set state vector value\n"
"\n";
addCommand("set",
new command::Setter<PinocchioDevice, Vector>
(*this, &PinocchioDevice::setState, docstring));
docstring =
"\n"
" Set velocity vector value\n"
"\n";
addCommand("setVelocity",
new command::Setter<PinocchioDevice, Vector>
(*this, &PinocchioDevice::setVelocity, docstring));
void(PinocchioDevice::*setRootPtr)(const Matrix&) = &PinocchioDevice::setRoot;
docstring
= command::docCommandVoid1("Set the root position.",
"matrix homogeneous");
addCommand("setRoot",
command::makeCommandVoid1(*this,setRootPtr,
docstring));
/* SET of SoT control input type per joint */
docstring =
"\n"
" Set the type of control input per joint on the SoT side \n"
" which can be \n"
" torque, acceleration, velocity, or position\n"
"\n";
addCommand("setSoTControlType",
command::makeCommandVoid2(*this,&PinocchioDevice::setSoTControlType,
command::docCommandVoid2 ("Set SoT control input type per joint",
"Joint name",
"Control type: [TORQUE|ACCELERATION|VELOCITY|POSITION]")
));
/* SET of HW control input type per joint */
docstring =
"\n"
" Set the type of control input per joint which can be \n"
" torque, acceleration, velocity, or position\n"
"\n";
addCommand("setHWControlType",
command::makeCommandVoid2(*this,&PinocchioDevice::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"
"\n";
addCommand("setSanityCheck",
new command::Setter<PinocchioDevice, bool>
(*this, &PinocchioDevice::setSanityCheck, docstring));
// Handle commands and signals called in a synchronous way.
periodicCallBefore_.addSpecificCommands(*this, commandMap, "before.");
periodicCallAfter_.addSpecificCommands(*this, commandMap, "after.");
}
}
void PinocchioDevice::
setState( const Vector& st )
{
if (sanityCheck_) {
}
position_ = st;
stateSOUT .setConstant( position_ );
motorcontrolSOUT .setConstant( position_ );
}
void PinocchioDevice::
setVelocity( const Vector& vel )
{
velocity_ = vel;
velocitySOUT .setConstant( velocity_ );
}
void PinocchioDevice::
setRoot( const Matrix & root )
{
Eigen::Matrix4d _matrix4d(root);
MatrixHomogeneous _root(_matrix4d);
setRoot( _root );
}
void PinocchioDevice::
setRoot( const MatrixHomogeneous & worldMwaist )
{
VectorRollPitchYaw r = (worldMwaist.linear().eulerAngles(2,1,0)).reverse();
Vector q = position_;
q = worldMwaist.translation(); // abusive ... but working.
for( unsigned int i=0;i<3;++i ) q(i+3) = r(i);
}
void PinocchioDevice::
setSanityCheck(const bool & enableCheck)
{
sanityCheck_ = enableCheck;
}
void PinocchioDevice::
setControlType(const std::string &strCtrlType,
ControlType &aCtrlType)
{
for(int j=0;j<4;j++)
{
if (strCtrlType==ControlType_s[j])
aCtrlType = (ControlType)j;
}
}
void PinocchioDevice::
setSoTControlType(const std::string &jointNames,
const std::string &strCtrlType)
{
setControlType(strCtrlType,jointPinocchioDevices_[jointNames].SoTcontrol);
}
void PinocchioDevice::
setHWControlType(const std::string &jointNames,
const std::string &strCtrlType)
{
setControlType(strCtrlType,jointPinocchioDevices_[jointNames].HWcontrol);
}
void PinocchioDevice::
setControlPos(const std::string &jointName,
const unsigned & index)
{
jointPinocchioDevices_[jointName].control_index = index;
}
void PinocchioDevice::
setURDFModel(const std::string &aURDFModel)
{
se3::urdf::buildModelFromXML(aURDFModel,model_,false);
/// Build the map between pinocchio and the alphabetical order.
for(unsigned int i=0;i<model_.names.size();i++)
jointPinocchioDevices_[model_.names[i]].pinocchio_index=i;
// Initialize pinocchio vector.
position_.resize(model_.nq);
velocity_.resize(model_.nv);
acceleration_.resize(model_.nv);
}
void PinocchioDevice::
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)
{
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;
}
/* Force the recomputation of the control. */
controlSIN( time );
sotDEBUG(25) << "u" <<time<<" = " << controlSIN.accessCopy() << endl;
integrate(dt);
sotDEBUG(25) << "q" << time << " = " << position_ << endl;
/* Position the signals corresponding to sensors. */
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.
try
{
periodicCallAfter_.run(time+1);
}
catch (std::exception& e)
{
dgRTLOG()