Commit 3497249a authored by Olivier Stasse's avatar Olivier Stasse
Browse files

First test to integrate device by device.

This first version tests only the velocity (SoT) -> position (HW)
scheme.
It should be able to handle acceleration (SoT) -> position/velocity (HW).
current (SoT) -> current (HW)
torque (SoT) -> torque (HW)

The following possibility is currently not handled:
current (SoT) -> position (HW)
This would requires to handle actuator model.
parent 4a2d6006
Pipeline #1997 failed with stage
......@@ -49,6 +49,7 @@ SET(BOOST_COMPONENTS thread filesystem program_options unit_test_framework syste
SEARCH_FOR_EIGEN()
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_OPTIONAL_DEPENDENCY("yaml-cpp")
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
......
......@@ -26,9 +26,8 @@ namespace dg = dynamicgraph;
#include "sot/core/periodic-call.hh"
#include <sot/core/matrix-geometry.hh>
#include "sot/core/api.hh"
#include "sot/core/joint-device.hh"
#include "sot/core/joint-device.hxx"
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
namespace dynamicgraph {
namespace sot {
......@@ -78,16 +77,33 @@ namespace dynamicgraph {
friend class JointDeviceVectorElement<LINEAR>;
protected:
/// State vector of the robot
/// \name Vectors related to the state.
///@{
/// State vector of the robot (deprecated)
dg::Vector state_;
/// Velocity vector of the robot.
/// Position of each actuator
dg::Vector position_;
/// Velocity vector of each actuator.
dg::Vector velocity_;
/// Acceleration vector of each actuator.
dg::Vector acceleration_;
/// Torque vector of each actuator.
dg::Vector torque_;
///@}
bool sanityCheck_;
dg::Vector vel_control_;
/// Specifies the control input by each element of the state vector.
std::vector<ControlInput> controlInputType_;
ControlInput controlInputType_;
std::map<std::string,ControlType> sotControlType_;
std::map<std::string,ControlType> hwControlType_;
/// Maps of joint devices.
std::map<std::string,JointDeviceVariant> jointDevices_;
///
bool withForceSignals[4];
PeriodicCall periodicCallBefore_;
......@@ -117,8 +133,14 @@ namespace dynamicgraph {
virtual void setNoIntegration();
/// Set control input type.
virtual void setControlInputType(const std::string& cit);
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(std::string &aURDFModel);
/// \name Sanity check parameterization
/// \{
void setSanityCheck (const bool & enableCheck);
......@@ -127,6 +149,17 @@ namespace dynamicgraph {
void setTorqueBounds (const Vector& lower, const Vector& upper);
/// \}
/// \name Set index in vector (position, velocity, acceleration, control)
/// \{
void setControlPos(const std::string &jointName,
const unsigned & index);
void setPositionPos(const std::string &jointName,
const unsigned & index);
void setVelocityPos(const std::string &jointName,
const unsigned & index);
void setAccelerationPos(const std::string &jointName,
const unsigned & index);
/// \}
public: /* --- DISPLAY --- */
virtual void display(std::ostream& os) const;
SOT_CORE_EXPORT friend std::ostream&
......@@ -185,6 +218,10 @@ namespace dynamicgraph {
/// \}
protected:
void setControlType(const std::string &jointNames,
const std::string &strCtrlType,
std::map<std::string, ControlType> &aCtrlType);
/// Compute roll pitch yaw angles of freeflyer joint.
void integrateRollPitchYaw(dg::Vector& state, const dg::Vector& control,
double dt);
......@@ -214,6 +251,10 @@ namespace dynamicgraph {
private:
// Intermediate variable to avoid dynamic allocation
dg::Vector forceZero6;
// URDF Model of the robot
urdf::ModelInterfaceSharedPtr modelURDF_;
};
} // namespace sot
} // namespace dynamicgraph
......
......@@ -14,6 +14,8 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <boost/variant.hpp>
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
......@@ -25,6 +27,9 @@ namespace dg = dynamicgraph;
namespace dynamicgraph{
namespace sot {
class Device;
/// Per Joint type
/// Specifies the nature of one joint control
......@@ -37,6 +42,11 @@ namespace dynamicgraph{
TORQUE=3
};
const std::string ControlType_s[] =
{
"POSITION","VELOCITY","ACCELERATION","TORQUE"
};
/// Enum joint type
enum JointType
{
......@@ -58,49 +68,81 @@ namespace dynamicgraph{
/// \brief Boolean to verify sanity check.
unsigned int sanityCheckFlags_;
/// \brief Joint Name.
string jointName_;
std::string jointName_;
/// \brief Control type from the SoT side.
ControlType sotControlType_;
/// \brief Hardware control type
ControlType hwControlType_;
/// \brief Joint type
JointType jointType_;
/// Vector of velocity.
dg::Vector velocity_;
/// \brief Size of the control information
unsigned int size_;
/// \brief Position of the joint state in the global state vector.
unsigned int stateVectorPos_;
/// \brief Position of the joint state in the global position vector.
unsigned int positionVectorPos_;
/// \brief Position of the joint velocity in the global velocity vector.
unsigned int velocityVectorPos_;
/// \brief Position of the joint control in the global control vector.
unsigned int controlVectorPos_;
/// \brief Acceleration of the joint control in the global acceleration vector.
unsigned int accelerationVectorPos_;
/// \brief Upper, lower velocity bounds;
double lowerPos_,upperPos_,lowerVel_,upperVel_,
dg::Vector lowerPos_,upperPos_,lowerVel_,upperVel_,
lowerTorque_,upperTorque_;
public:
// Contrustor.
JointDeviceVectorElement(unsigned int lsize=1);
/// \name Set joint name.
void setJointName(const std::string &aname);
/// \name Set control type for the SoT side and the Hardware side.
///@{
void setSotControlType(ControlType aControlType);
void setHWControlType(ControlType aControlType);
///@}
void setJointType(JointType aJointType);
void setStateVectorPos(unsigned int aStateVectorPos_);
void setVelocityVectorPos(unsigned int aVelocityVectorPos_);
/// \name Set the position of the actuator in their respective vector
/// @{
/// \brief Position vector position
void setPositionVectorPos(unsigned int aStateVectorPos);
/// \brief Velocity vector position
void setVelocityVectorPos(unsigned int aVelocityVectorPos);
/// \brief Acceleration vector position
void setAccelerationVectorPos(unsigned int anAccelerationVectorPos);
/// @}
void setControlVectorPos(unsigned int aControlVectorPos);
/// Integrate the control part
void integrate(const double &dt);
void integrate(Device &aDevice, const double &dt);
const JointType & getJointType();
/// \name Handle bounds
/// @{
/// \brief Saturate bounds
bool saturateBounds(double &val,
const double& lower,
const double& upper);
/// \brief Check bounds
void checkBounds(dg::Vector &val, unsigned int start,
dg::Vector &lower,
dg::Vector &upper,
const std::string &what);
/// \name Set limits
/// Here we assume that Joint
///@{
/// \brief Set position limits
void setPositionBounds(const double &lower, const double &upper);
void setPositionBounds(const Vector &lower, const Vector &upper);
/// \brief Set velocity limits
void setVelocityBounds(const double &lower, const double &upper);
void setVelocityBounds(const Vector &lower, const Vector &upper);
/// \brief Set torque limits
void setTorqueBounds(const double &lower, const double &upper);
void setTorqueBounds(const Vector &lower, const Vector &upper);
/// \brief Modifies the control part
void saturateBounds();
/// \brief Check the boundaries and displays the problematic joint
......@@ -109,6 +151,14 @@ namespace dynamicgraph{
///@}
};
typedef JointDeviceVectorElement<FREE_FLYER> JointDeviceFreeFlyer;
typedef JointDeviceVectorElement<REVOLUTE> JointDeviceRevolute;
typedef JointDeviceVectorElement<LINEAR> JointDeviceLinear;
typedef boost::variant<JointDeviceFreeFlyer,
JointDeviceRevolute,
JointDeviceLinear>
JointDeviceVariant;
} //namespace sot
} // namespace dynamicgraph
......
......@@ -14,15 +14,21 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <iostream>
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/real-time-logger.h>
#include "sot/core/periodic-call.hh"
#include <sot/core/matrix-geometry.hh>
#include "sot/core/debug.hh"
#include "sot/core/api.hh"
#include "sot/core/joint-device.hh"
#include "sot/core/device.hh"
namespace dynamicgraph{
namespace sot {
......@@ -30,42 +36,48 @@ namespace dynamicgraph{
{
SOT_CONTROL_TYPE=0,
HW_CONTROL_TYPE=1,
JOINT_TYPE=2,
STATE_VECTOR_POS=3,
CONTROL_VECTOR_POS=4,
VELOCITY_VECTOR_POS=5,
STATE_VECTOR_POS=2,
CONTROL_VECTOR_POS=3,
VELOCITY_VECTOR_POS=4,
ACCELERATION_VECTOR_POS=5,
POSITION_BOUNDS=6,
VELOCITY_BOUNDS=7,
TORQUE_BOUNDS=8
}
};
template<JointType temp_JointType>
JointDeviceVectorElement<temp_JointType>::
JointDeviceVectorElement():
sanityCheck(true),
JointDeviceVectorElement(unsigned int lsize):
sanityCheckFlags_(0),
sotControlType_(VELOCITY),
hwControlType_(POSITION),
jointType_(temp_JointType),
size_(1),
stateVectorPos_(0),
size_(lsize),
positionVectorPos_(0),
velocityVectorPos_(0),
lowerPos_(-3.14),
upperPos_(3.14),
lowerVel_(-3.14),
upperVel_(3.14),
lowerTorque_(-20),
upperTorque_(20)
accelerationVectorPos_(0),
lowerPos_(size_),
upperPos_(size_),
lowerVel_(size_),
upperVel_(size_),
lowerTorque_(size_),
upperTorque_(size_)
{
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setJointName(const std::string &aJointName)
{
jointName_ = aJointName;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setSotControlType(ControlType aControlType)
{
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< SOT_CONTROL_TYPE);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
sotControlType_ = aControlType;
}
......@@ -74,88 +86,74 @@ namespace dynamicgraph{
setHWControlType(ControlType aControlType)
{
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< HW_CONTROL_TYPE);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
hwControlType_ = aControlType;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setJointType(JointType aJointType)
setPositionVectorPos(unsigned int aPositionVectorPos)
{
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< JOINT_TYPE);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
jointType_ = aJointType;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< STATE_VECTOR_POS);
positionVectorPos_ = aPositionVectorPos;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setStateVectorPos(unsigned int aStateVectorPos)
setVelocityVectorPos(unsigned int aVelocityVectorPos)
{
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< STATE_VECTOR_POS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< VELOCITY_VECTOR_POS);
stateVectorPos_(aStateVectorPos);
velocityVectorPos_= aVelocityVectorPos;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setVelocityVectorPos(unsigned int aVelocityVectorPos)
setAccelerationVectorPos(unsigned int anAccelerationVectorPos)
{
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< VELOCITY_VECTOR_POS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< ACCELERATION_VECTOR_POS);
velocityVectorPos_(aVelocityVectorPos);
accelerationVectorPos_= anAccelerationVectorPos;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setVelocityVectorPos(unsigned int aControlVectorPos)
setControlVectorPos(unsigned int aControlVectorPos)
{
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< CONTROL_VECTOR_POS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
controlVectorPos_(aControlVectorPos);
controlVectorPos_=aControlVectorPos;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setPositionBounds(const double &lower, const double &upper)
setPositionBounds(const Vector &lower, const Vector &upper)
{
lowerPos_ = lower; upperPos_ = upper;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< POSITION_BOUNDS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setVelocityBounds(const double &lower, const double &upper)
setVelocityBounds(const Vector &lower, const Vector &upper)
{
lowerVel_ = lower; upperVel_ = upper;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< VELOCITY_BOUNDS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
setTorqueBounds(const double &lower, const double &upper)
{
lowerTorque_ = lower; upperTorque_ = upper;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< TORQUE_BOUNDS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
}
template<JointType temp_JointType>
void JointDeviceVectorElement<temp_JointType>::
checkBounds()
setTorqueBounds(const Vector &lower, const Vector &upper)
{
lowerTorque_ = lower; upperTorque_ = upper;
sanityCheckFlags_ = sanityCheckFlags_ | ( 1<< TORQUE_BOUNDS);
std::cout << std::hex << sanityCheckFlags_ << std::endl;
}
template<FREE_FLYER>
template<>
void JointDeviceVectorElement<FREE_FLYER>::
integrate(Device &aDevice, const double &dt)
{
......@@ -167,12 +165,12 @@ namespace dynamicgraph{
/// Creates references to good vectors.
const Vector & control = aDevice.controlSIN.accessCopy();
Vector & state = aDevice.state_;
Vector & state = aDevice.position_;
MatrixHomogeneous & ffPose = aDevice.ffPose_;
typedef se3::SpecialEuclideanOperation<3> SE3;
Eigen::Matrix<double, 7, 1> qin, qout;
qin.head<3>() = state.segment<3>(stateVectorPos_);
qin.head<3>() = state.segment<3>(positionVectorPos_);
QuaternionMapd quat (qin.tail<4>().data());
quat = AngleAxisd(state(5), Vector3d::UnitZ())
......@@ -183,72 +181,279 @@ namespace dynamicgraph{
// Update freeflyer pose
ffPose.translation() = qout.head<3>();
state.segment<3>(stateVectorPos_) = qout.head<3>();
state.segment<3>(positionVectorPos_) = qout.head<3>();
ffPose.linear() = QuaternionMapd(qout.tail<4>().data()).toRotationMatrix();
state.segment<3>(stateVectorPos_+3) =
ffPose_.linear().eulerAngles(2,1,0).reverse();
state.segment<3>(positionVectorPos_+3) =
ffPose.linear().eulerAngles(2,1,0).reverse();
// std::cout << "ffPose.translation(): "<< ffPose.translation() << std::endl;
//std::cout << "ffPose.rotation(): "<< ffPose.rotation() << std::endl;
}
}
template<JointType aJointType>
const JointType & JointDeviceVectorElement<aJointType>::
getJointType()
{
return jointType_;
}
// Return true if it saturates.
template<JointType aJointType>
bool JointDeviceVectorElement<aJointType>::
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;
}
template<JointType aJointType>
void JointDeviceVectorElement<aJointType>::
integrate(Device &aDevice, const double &dt)
checkBounds(dg::Vector &val,
unsigned int start,
dg::Vector &lower,
dg::Vector &upper,
const std::string &what)
{
Vector & state = aDevice.state_;
std::cout << "Checking bound for joint "
<< jointName_ << ": ("
<< lower(0) << ","
<< upper(0) << ") val:"
<< val(start) << " size: "
<< size_
<< std::endl;
unsigned int j=0;
for (unsigned int i = start; i < start+size_; ++i) {
double old = val(i);
if (saturateBounds (val(i), lower(j), upper(j)))
{
dgRTLOG () << "Robot " << what << " bound violation at DoF " << i <<
": requested " << old << " but set " << val(i) << '\n';
std::cout <<"Robot " << what << " bound violation at DoF " << i <<
": requested " << old << " but set " << val(i) << '\n';
}
j++;
}
}
template<JointType aJointType>
void JointDeviceVectorElement<aJointType>::
integrate(Device &aDevice, const double &dt)
{
Vector & position = aDevice.position_;
const Vector & controlIN = aDevice.controlSIN.accessCopy();
Vector & velocity = aDevice.velocity_;
Vector & acceleration = aDevice.acceleration_;
Vector & torque = aDevice.torque_;
std::cout << "integrate for :" << jointName_ << " sot:"
<< sotControlType_ << " hw:" << hwControlType_ << std::endl;
if (sotControlType_==TORQUE)
{
/// Checking torque boundaries
if ( (controlIN(controlVectorPos_) > upperTorque_(stateVectorPos_)) ||
(controlIN(controlVectorPos_) < lowerTorque_(stateVectorPos_)))
{
dgRTLOG () << "Robot torque bound violation at DoF " << i <<
": requested " << old << " but set " << val(i) << '\n';
}
checkBounds(torque,
controlVectorPos_,
lowerTorque_,
upperTorque_,
"torque");
}
if (sotControlType_==ACCELERATION)
{
/// Set acceleration
acceleration(accelerationVectorPos_) = controlIN(controlVectorPos_);
/// Integrate acceleration
vel_control_ = velocity_(velocityVectorPos_) +
(0.5*dt)*controlIN(controlVectorPos_);
// Velocity integration.
velocity_(velocityVectorPos_) += controlIN(controlVectorPos_)*dt;
velocity(velocityVectorPos_) = velocity(velocityVectorPos_) +
dt*controlIN(controlVectorPos_);
/// Check velocity
if ( (lowerVel_ > velocity_(velocityVectorPos_)) &&
(upperVel_ < velocity_(velocityVectorPos_)))
{
dgRTLOG () << "Robot velocity bound violation at DoF " << i <<
": requested " << old << " but set " << val(i) << '\n';
}
checkBounds(velocity,
velocityVectorPos_,
lowerVel_,
upperVel_,
"velocity");
}
if (sotControlType_==VELOCITY)
{ vel_control_(velocityVectorPos_) = controlIN(controlVectorPos_);}
{ velocity(velocityVectorPos_) = controlIN(controlVectorPos_);}
if ((sotControlType_==VELOCITY) || (sotControlType_==ACCELERATION))
{
state(stateVectorPos_) +=
vel_control_(velocityVectorPos_) *dt;
position(positionVectorPos_) +=
velocity(velocityVectorPos_) *dt;
std::cout << "position control("<< positionVectorPos_
<< ") =" << position(positionVectorPos_)
<< std::endl;
}
if (sotControlType_==POSITION)
{
state(stateVectorPos_)= controlIN(controlVectorPos_);
position(positionVectorPos_)= controlIN(controlVectorPos_);
}
checkBounds(position,
positionVectorPos_,
lowerPos_,
upperPos_,
"position");
if ( (state_ > lowerPosition_(stateVectorPos_)) &&
(state_ < upperPosition_(stateVectorPos_)))
{
dgRTLOG () << "Robot position bound violation at DoF " << i <<
": requested " << old << " but set " << val(i) << '\n';