Commit 6d6a2d9d authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[tools] Move parameter server to sot-core

Remove robot-utils dependency to pinocchio
Uses shared_ptr<RobotUtil> instead of RobotUtil *
parent 929edf83
......@@ -31,6 +31,7 @@ SET(NEWHEADERS
sot/core/additional-functions.hh
sot/core/factory.hh
sot/core/macros-signal.hh
sot/core/parameter-server.hh
sot/core/pool.hh
sot/core/op-point-modifier.hh
sot/core/feature-point6d.hh
......
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control 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-torque-control 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-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_parameter_server_H__
#define __sot_torque_control_parameter_server_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_parameter_server_H__)
# define SOTParameterServer_EXPORT __declspec(dllexport)
# else
# define SOTParameterServer_EXPORT __declspec(dllimport)
# endif
#else
# define SOTParameterServer_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
class SOTParameterServer_EXPORT ParameterServer
:public::dynamicgraph::Entity
{
typedef ParameterServer EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ParameterServer( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
/* --- SIGNALS --- */
/* --- COMMANDS --- */
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId, const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId, const dynamicgraph::Vector &lq, const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName, const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
RobotUtilShrPtr m_robot_util;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
bool m_emergency_stop_triggered; /// true if an emergency condition as been triggered either by an other entity, or by control limit violation
bool m_is_first_iter; /// true at the first iteration, false otherwise
int m_iter;
double m_sleep_time; /// time to sleep at every iteration (to slow down simulation)
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ParameterServer
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
......@@ -28,246 +28,249 @@ namespace dynamicgraph {
namespace sot {
struct SOT_CORE_EXPORT JointLimits
{
double upper;
double lower;
JointLimits():
upper(0.0),
lower(0.0)
{
double upper;
double lower;
JointLimits():
upper(0.0),
lower(0.0)
{}
JointLimits(double l, double u):
upper(u),
lower(l)
{}
};
JointLimits(double l, double u):
upper(u),
lower(l)
{}
};
typedef Eigen::VectorXd::Index Index;
typedef Eigen::VectorXd::Index Index;
struct SOT_CORE_EXPORT ForceLimits
{
Eigen::VectorXd upper;
Eigen::VectorXd lower;
struct SOT_CORE_EXPORT ForceLimits
{
Eigen::VectorXd upper;
Eigen::VectorXd lower;
ForceLimits():
upper(Vector6d::Zero()),
lower(Vector6d::Zero())
{}
ForceLimits():
upper(Vector6d::Zero()),
lower(Vector6d::Zero())
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
upper(u),
lower(l)
{}
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
upper(u),
lower(l)
{}
void display(std::ostream &os) const;
};
void display(std::ostream &os) const;
};
struct SOT_CORE_EXPORT ForceUtil
{
std::map<Index,ForceLimits> m_force_id_to_limits;
std::map<std::string,Index> m_name_to_force_id;
std::map<Index,std::string> m_force_id_to_name;
{
std::map<Index,ForceLimits> m_force_id_to_limits;
std::map<std::string,Index> m_name_to_force_id;
std::map<Index,std::string> m_force_id_to_name;
Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;
Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;
void set_name_to_force_id(const std::string & name,
const Index &force_id);
void set_name_to_force_id(const std::string & name,
const Index &force_id);
void set_force_id_to_limits(const Index &force_id,
const dg::Vector &lf,
const dg::Vector &uf);
void set_force_id_to_limits(const Index &force_id,
const dg::Vector &lf,
const dg::Vector &uf);
void create_force_id_to_name_map();
void create_force_id_to_name_map();
Index get_id_from_name(const std::string &name);
Index get_id_from_name(const std::string &name);
const std::string & get_name_from_id(Index idx);
std::string cp_get_name_from_id(Index idx);
const std::string & get_name_from_id(Index idx);
std::string cp_get_name_from_id(Index idx);
const ForceLimits & get_limits_from_id(Index force_id);
ForceLimits cp_get_limits_from_id(Index force_id);
const ForceLimits & get_limits_from_id(Index force_id);
ForceLimits cp_get_limits_from_id(Index force_id);
Index get_force_id_left_hand()
{
return m_Force_Id_Left_Hand;
}
Index get_force_id_left_hand()
{
return m_Force_Id_Left_Hand;
}
void set_force_id_left_hand(Index anId)
{
m_Force_Id_Left_Hand = anId;
}
void set_force_id_left_hand(Index anId)
{
m_Force_Id_Left_Hand = anId;
}
Index get_force_id_right_hand()
{
return m_Force_Id_Right_Hand;
}
Index get_force_id_right_hand()
{
return m_Force_Id_Right_Hand;
}
void set_force_id_right_hand( Index anId)
{
m_Force_Id_Right_Hand = anId;
}
void set_force_id_right_hand( Index anId)
{
m_Force_Id_Right_Hand = anId;
}
Index get_force_id_left_foot()
{
return m_Force_Id_Left_Foot;
}
Index get_force_id_left_foot()
{
return m_Force_Id_Left_Foot;
}
void set_force_id_left_foot(Index anId)
{
m_Force_Id_Left_Foot = anId;
}
void set_force_id_left_foot(Index anId)
{
m_Force_Id_Left_Foot = anId;
}
Index get_force_id_right_foot()
{
return m_Force_Id_Right_Foot;
}
Index get_force_id_right_foot()
{
return m_Force_Id_Right_Foot;
}
void set_force_id_right_foot( Index anId)
{
m_Force_Id_Right_Foot = anId;
}
void set_force_id_right_foot( Index anId)
{
m_Force_Id_Right_Foot = anId;
}
void display(std::ostream & out) const;
void display(std::ostream & out) const;
}; // struct ForceUtil
}; // struct ForceUtil
struct SOT_CORE_EXPORT FootUtil
{
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream & os) const;
};
{
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream & os) const;
};
struct SOT_CORE_EXPORT RobotUtil
{
public:
struct SOT_CORE_EXPORT RobotUtil
{
public:
RobotUtil();
RobotUtil();
/// Forces data
ForceUtil m_force_util;
/// Forces data
ForceUtil m_force_util;
/// Foot information
FootUtil m_foot_util;
/// Foot information
FootUtil m_foot_util;
/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;
/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;
/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;
/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;
/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;
/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;
/// The map between id and name
std::map<Index,std::string> m_id_to_name;
/// The map between id and name
std::map<Index,std::string> m_id_to_name;
/// The joint limits map.
std::map<Index,JointLimits> m_limits_map;
/// The joint limits map.
std::map<Index,JointLimits> m_limits_map;
/// The name of the joint IMU is attached to
std::string m_imu_joint_name;
/// The name of the joint IMU is attached to
std::string m_imu_joint_name;
/// This method creates the map between id and name.
/// It is called each time a new link between id and name is inserted
/// (i.e. when set_name_to_id is called).
void create_id_to_name_map();
/// This method creates the map between id and name.
/// It is called each time a new link between id and name is inserted
/// (i.e. when set_name_to_id is called).
void create_id_to_name_map();
/// URDF file path
std::string m_urdf_filename;
/// URDF file path
std::string m_urdf_filename;
dynamicgraph::Vector m_dgv_urdf_to_sot;
/** Given a joint name it finds the associated joint id.
* If the specified joint name is not found it returns -1;
* @param name Name of the joint to find.
* @return The id of the specified joint, -1 if not found. */
const Index & get_id_from_name(const std::string &name);
dynamicgraph::Vector m_dgv_urdf_to_sot;
/** Given a joint id it finds the associated joint name.
* If the specified joint is not found it returns "Joint name not found";
* @param id Id of the joint to find.
* @return The name of the specified joint, "Joint name not found" if not found. */
/** Given a joint name it finds the associated joint id.
* If the specified joint name is not found it returns -1;
* @param name Name of the joint to find.
* @return The id of the specified joint, -1 if not found. */
const Index get_id_from_name(const std::string &name);
/// Get the joint name from its index
const std::string & get_name_from_id(Index id);
/** Given a joint id it finds the associated joint name.
* If the specified joint is not found it returns "Joint name not found";
* @param id Id of the joint to find.
* @return The name of the specified joint, "Joint name not found" if not found. */
/// Set relation between the name and the SoT id
void set_name_to_id(const std::string &jointName,
const Index & jointId);
/// Get the joint name from its index
const std::string & get_name_from_id(Index id);
/// Set the map between urdf index and sot index
void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
/// Set relation between the name and the SoT id
void set_name_to_id(const std::string &jointName,
const Index & jointId);
/// Set the limits (lq,uq) for joint idx
void set_joint_limits_for_id(const Index &idx,
const double &lq,
const double &uq);
/// Set the map between urdf index and sot index
void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
/// Set the limits (lq,uq) for joint idx
void set_joint_limits_for_id(const Index &idx,
const double &lq,
const double &uq);
bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
bool joints_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool velocity_urdf_to_sot(ConstRefVector q_urdf,
ConstRefVector v_urdf, RefVector v_sot);
bool joints_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
bool velocity_sot_to_urdf(ConstRefVector q_urdf,
ConstRefVector v_sot, RefVector v_urdf);
bool velocity_urdf_to_sot(ConstRefVector q_urdf,
ConstRefVector v_urdf, RefVector v_sot);
bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
bool velocity_sot_to_urdf(ConstRefVector q_urdf,
ConstRefVector v_sot, RefVector v_urdf);
bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
bool config_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool config_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
bool base_urdf_to_sot(ConstRefVector q_urdf, RefVector q_sot);
bool base_sot_to_urdf(ConstRefVector q_sot, RefVector q_urdf);
/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
* @param id Id of the joint to find.
* @return The limits of the specified joint, JointLimits(0,0) if not found. */
const JointLimits & get_joint_limits_from_id(Index id);
JointLimits cp_get_joint_limits_from_id(Index id);
/** \name Logger related methods */
/** \{*/
/// \brief Send messages \param msg with level t. Add string file and line to message.
void sendMsg(const std::string &msg,
MsgType t=MSG_TYPE_INFO,
const char *file="",
int line=0);
/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
* @param id Id of the joint to find.
* @return The limits of the specified joint, JointLimits(0,0) if not found. */
const JointLimits & get_joint_limits_from_id(Index id);
JointLimits cp_get_joint_limits_from_id(Index id);
/// \brief Specify the verbosity level of the logger.
void setLoggerVerbosityLevel(LoggerVerbosity lv)
{logger_.setVerbosity(lv);}
/** \name Logger related methods */
/** \{*/
/// \brief Send messages \param msg with level t. Add string file and line to message.
void sendMsg(const std::string &msg,
MsgType t=MSG_TYPE_INFO,
const char *file="",
int line=0);
/// \brief Get the logger's verbosity level.
LoggerVerbosity getLoggerVerbosityLevel()
{ return logger_.getVerbosity(); };
/// \brief Specify the verbosity level of the logger.
void setLoggerVerbosityLevel(LoggerVerbosity lv)
{logger_.setVerbosity(lv);}
void display(std::ostream &os) const;
protected:
Logger logger_;
}; // struct RobotUtil
/// \brief Get the logger's verbosity level.
LoggerVerbosity getLoggerVerbosityLevel()
{ return logger_.getVerbosity(); };
/// Accessors - This should be changed to RobotUtilPtrShared
typedef boost::shared_ptr<RobotUtil> RobotUtilShrPtr;
void display(std::ostream &os) const;
protected:
Logger logger_;
}; // struct RobotUtil
RobotUtil * RefVoidRobotUtil();
RobotUtilShrPtr RefVoidRobotUtil();
RobotUtil * getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtil * createRobotUtil(std::string &robotName);
RobotUtilShrPtr getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtilShrPtr createRobotUtil(std::string &robotName);
bool base_se3_to_sot(ConstRefVector pos,
ConstRefMatrix R,
RefVector q_sot);
bool base_se3_to_sot(ConstRefVector pos,
ConstRefMatrix R,
RefVector q_sot);
} // namespace sot
} // namespace dynamicgraph
......
......@@ -78,6 +78,7 @@ SET(plugins
tools/switch
tools/exp-moving-avg
tools/gradient-ascent
tools/parameter-server
control/control-gr
control/control-pd
......
/*
* Copyright 2014, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control 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-torque-control 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-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#include <iostream>
#include <sot/core/parameter-server.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
namespace dynamicgraph
{
namespace sot
{
namespace dynamicgraph = ::dynamicgraph;
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
//Size to be aligned "-------------------------------------------------------"
#define PROFILE_PWM_DESIRED_COMPUTATION "Control manager "
#define PROFILE_DYNAMIC_GRAPH_PERIOD "Control period "
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef ParameterServer EntityClassName;
</