Commit 5d658d91 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Format]

parent e473bd15
......@@ -10,16 +10,15 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sot_admittance_controller_EXPORTS)
# define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
# define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(sot_admittance_controller_EXPORTS)
#define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
#else
# define SOTADMITTANCECONTROLLER_EXPORT
#define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
......@@ -37,120 +36,113 @@
#include <sot/core/robot-utils.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController
:public::dynamicgraph::Entity
{
typedef AdmittanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceController( const std::string & name );
void init(const double& dt, const std::string& robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightFootFiltered,dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
// DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d estimated force
// DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector); ///
DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector); ///
// DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
// DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
Eigen::VectorXd m_u; /// control (i.e. motor currents)
bool m_firstIter;
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_useJacobianTranspose; /// if true it uses the Jacobian transpose rather than the pseudoinverse
double m_dt; /// control loop time period
int m_nj;
/// robot geometric/inertial data
int m_frame_id_rf; /// frame id of right foot
int m_frame_id_lf; /// frame id of left foot
/// tsid
tsid::robots::RobotWrapper * m_robot;
pinocchio::Data* m_data;
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
tsid::math::Vector m_q_urdf;
tsid::math::Vector m_v_urdf;
tsid::math::Vector m_dq_des_urdf;
tsid::math::Vector m_dqErrIntegral; /// integral of the velocity error
// tsid::math::Vector m_dqDesIntegral; /// integral of the desired joint velocities
tsid::math::Vector m_dq_fd; /// joint velocities computed with finite differences
tsid::math::Vector m_qPrev; /// previous value of encoders
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
RobotUtilShrPtr m_robot_util;
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des; /// 3d desired global zmp
// tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp; /// 3d global zmp
}; // class AdmittanceController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_admittance_controller_H__
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgraph::Entity {
typedef AdmittanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceController(const std::string& name);
void init(const double& dt, const std::string& robotRef);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector); /// 6d estimated force filtered
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
// DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
// DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d estimated force
// DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector); ///
DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector); ///
// DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
// DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[" + name + "] " + msg, t, file, line);
}
protected:
Eigen::VectorXd m_u; /// control (i.e. motor currents)
bool m_firstIter;
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_useJacobianTranspose; /// if true it uses the Jacobian transpose rather than the pseudoinverse
double m_dt; /// control loop time period
int m_nj;
/// robot geometric/inertial data
int m_frame_id_rf; /// frame id of right foot
int m_frame_id_lf; /// frame id of left foot
/// tsid
tsid::robots::RobotWrapper* m_robot;
pinocchio::Data* m_data;
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
tsid::math::Vector m_q_urdf;
tsid::math::Vector m_v_urdf;
tsid::math::Vector m_dq_des_urdf;
tsid::math::Vector m_dqErrIntegral; /// integral of the velocity error
// tsid::math::Vector m_dqDesIntegral; /// integral of the desired joint velocities
tsid::math::Vector m_dq_fd; /// joint velocities computed with finite differences
tsid::math::Vector m_qPrev; /// previous value of encoders
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
RobotUtilShrPtr m_robot_util;
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in local frame
// tsid::math::Vector3 m_zmp_des; /// 3d desired global zmp
// tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp; /// 3d global zmp
}; // class AdmittanceController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_admittance_controller_H__
......@@ -10,26 +10,23 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
# else
# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(__sot_torque_control_control_manager_H__)
#define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
#else
# define SOTCONTROLMANAGER_EXPORT
#define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTCONTROLMANAGER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
......@@ -40,139 +37,128 @@
#include <sot/torque_control/utils/vector-conversions.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// Number of time step to transition from one ctrl mode to another
#define CTRL_MODE_TRANSITION_TIME_STEP 1000.0
class CtrlMode
{
public:
int id;
std::string name;
CtrlMode(): id(-1), name("None"){}
CtrlMode(int id, const std::string& name):
id(id), name(name) {}
};
std::ostream& operator<<( std::ostream& os, const CtrlMode& s )
{
os<<s.id<<"_"<<s.name;
return os;
}
class SOTCONTROLMANAGER_EXPORT ControlManager
:public::dynamicgraph::Entity
{
typedef ControlManager EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ControlManager( 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 --- */
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector,int>*> m_ctrlInputsSIN;
std::vector< dynamicgraph::SignalPtr<bool,int>* > m_emergencyStopSIN; /// emergency stop inputs. If one is true, control is set to zero forever
std::vector<dynamicgraph::Signal<dynamicgraph::Vector,int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector); /// motor currents
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector); /// estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector); /// predicted joint torques (using motor model)
DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector); /// max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(u_max, dynamicgraph::Vector); /// max desired current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector); /// max torque allowed before stopping the controller
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector); /// same as u when everything is fine, 0 otherwise
/* --- COMMANDS --- */
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// 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();
/// Commands related to HandUtil
void setHandFrameName(const std::string &, const std::string &);
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
void setStreamPrintPeriod(const double & s);
void setSleepTime(const double &seconds);
void addEmergencyStopSIN(const std::string& name);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
Entity::sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtilShrPtr m_robot_util;
tsid::robots::RobotWrapper * m_robot;
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)
std::vector<std::string> m_ctrlModes; /// existing control modes
std::vector<CtrlMode> m_jointCtrlModes_current; /// control mode of the joints
std::vector<CtrlMode> m_jointCtrlModes_previous; /// previous control mode of the joints
std::vector<int> m_jointCtrlModesCountDown; /// counters used for the transition between two ctrl modes
bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ControlManager
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
class CtrlMode {
public:
int id;
std::string name;
CtrlMode() : id(-1), name("None") {}
CtrlMode(int id, const std::string& name) : id(id), name(name) {}
};
std::ostream& operator<<(std::ostream& os, const CtrlMode& s) {
os << s.id << "_" << s.name;
return os;
}
class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity {
typedef ControlManager EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
/* --- CONSTRUCTOR ---- */
ControlManager(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 --- */
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector, int>*> m_ctrlInputsSIN;
std::vector<dynamicgraph::SignalPtr<bool, int>*>
m_emergencyStopSIN; /// emergency stop inputs. If one is true, control is set to zero forever
std::vector<dynamicgraph::Signal<dynamicgraph::Vector, int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(i_measured, dynamicgraph::Vector); /// motor currents
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector); /// estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector); /// predicted joint torques (using motor model)
DECLARE_SIGNAL_IN(i_max, dynamicgraph::Vector); /// max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(u_max,
dynamicgraph::Vector); /// max desired current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector); /// max torque allowed before stopping the controller
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(u_safe, dynamicgraph::Vector); /// same as u when everything is fine, 0 otherwise
/* --- COMMANDS --- */
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// 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();
/// Commands related to HandUtil
void setHandFrameName(const std::string&, const std::string&);
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector&);
void setStreamPrintPeriod(const double& s);
void setSleepTime(const double& seconds);
void addEmergencyStopSIN(const std::string& name);
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream& os) const;
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[ControlManager-" + name + "] " + msg, t, file, line);
}
protected:
RobotUtilShrPtr m_robot_util;
tsid::robots::RobotWrapper* m_robot;
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)
std::vector<std::string> m_ctrlModes; /// existing control modes
std::vector<CtrlMode> m_jointCtrlModes_current; /// control mode of the joints
std::vector<CtrlMode> m_jointCtrlModes_previous; /// previous control mode of the joints
std::vector<int> m_jointCtrlModesCountDown; /// counters used for the transition between two ctrl modes
bool convertStringToCtrlMode(const std::string& name, CtrlMode& cm);
bool convertJointNameToJointId(const std::string& name, unsigned int& id);
bool isJointInRange(unsigned int id, double q);
void updateJointCtrlModesOutputSignal();
}; // class ControlManager
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_control_manager_H__
......@@ -10,16 +10,15 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_current_controller_H__)
# define SOTCURRENTCONTROLLER_EXPORT __declspec(dllexport)
# else
# define SOTCURRENTCONTROLLER_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(__sot_torque_control_current_controller_H__)
#define SOTCURRENTCONTROLLER_EXPORT __declspec(dllexport)
#else
# define SOTCURRENTCONTROLLER_EXPORT
#define SOTCURRENTCONTROLLER_EXPORT __declspec(dllimport)
#endif
#else
#define SOTCURRENTCONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
......@@ -28,7 +27,6 @@
#include <map>
#include "boost/assign.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
......@@ -39,98 +37,101 @@
#include <sot/torque_control/utils/vector-conversions.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {