Commit 29b53772 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files
parent 1427311c
......@@ -90,8 +90,8 @@ class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgrap
/* --- 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);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -160,8 +160,8 @@ class SOTBASEESTIMATOR_EXPORT BaseEstimator : public ::dynamicgraph::Entity {
/* --- 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);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -130,8 +130,8 @@ class SOTCONTROLMANAGER_EXPORT ControlManager : public ::dynamicgraph::Entity {
/* --- 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);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[ControlManager-" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -106,8 +106,8 @@ class SOTCURRENTCONTROLLER_EXPORT CurrentController : public ::dynamicgraph::Ent
/* --- 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("[CurrentController-" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[CurrentController-" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -75,8 +75,8 @@ class DeviceTorqueCtrl : public dgsot::Device {
virtual void integrate(const double& dt);
void computeForwardDynamics();
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[DeviceTorqueCtrl] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[DeviceTorqueCtrl] " + msg) << '\n';
}
/// \brief Current integration step.
......
......@@ -78,8 +78,8 @@ class SOTFREEFLYERLOCATOR_EXPORT FreeFlyerLocator : public ::dynamicgraph::Entit
/* --- 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("[FreeFlyerLocator-" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -67,7 +67,7 @@ class SOTIMUOFFSETCOMPENSATION_EXPORT ImuOffsetCompensation : public ::dynamicgr
/* --- METHODS --- */
void update_offset_impl(int iter);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line);
logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line);
}
protected:
......
......@@ -190,8 +190,8 @@ class SOTINVERSEDYNAMICSBALANCECONTROLLER_EXPORT InverseDynamicsBalanceControlle
/* --- 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);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -113,8 +113,8 @@ class SOTJOINTTORQUECONTROLLER_EXPORT JointTorqueController : public ::dynamicgr
RobotUtilShrPtr m_robot_util;
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
public:
......
......@@ -147,8 +147,8 @@ class SOTJOINTTRAJECTORYGENERATOR_EXPORT JointTrajectoryGenerator : public ::dyn
/* --- 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("[JointTrajectoryGenerator-" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[JointTrajectoryGenerator-" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -95,8 +95,8 @@ class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference : public ::dynamicgraph:
void init(const double& timestep, const int& sigSize, const double& delay, const int& polyOrder);
protected:
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
public: /* --- ENTITY INHERITANCE --- */
......
......@@ -72,8 +72,8 @@ class SOTPOSITIONCONTROLLER_EXPORT PositionController : public ::dynamicgraph::E
/* --- 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("[PositionController-" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -121,8 +121,8 @@ class SOTSE3TRAJECTORYGENERATOR_EXPORT SE3TrajectoryGenerator : public ::dynamic
/* --- 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("[SE3TrajectoryGenerator-" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[SE3TrajectoryGenerator-" + name + "] " + msg) << '\n';
}
protected:
......
......@@ -21,16 +21,15 @@
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (simple_inverse_dyn_EXPORTS)
# define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllexport)
# else
# define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllimport)
# endif
#if defined(WIN32)
#if defined(simple_inverse_dyn_EXPORTS)
#define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllexport)
#else
# define SOTSIMPLEINVERSEDYN_EXPORT
#define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSIMPLEINVERSEDYN_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
......@@ -63,22 +62,15 @@ namespace dynamicgraph {
namespace sot {
namespace torque_control {
enum ControlOutput {
CONTROL_OUTPUT_VELOCITY = 0,
CONTROL_OUTPUT_TORQUE = 1,
CONTROL_OUTPUT_SIZE = 2
};
enum ControlOutput { CONTROL_OUTPUT_VELOCITY = 0, CONTROL_OUTPUT_TORQUE = 1, CONTROL_OUTPUT_SIZE = 2 };
const std::string ControlOutput_s[] = {
"velocity", "torque"
};
const std::string ControlOutput_s[] = {"velocity", "torque"};
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn
: public::dynamicgraph::Entity {
class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn : public ::dynamicgraph::Entity {
typedef SimpleInverseDyn EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
......@@ -86,95 +78,94 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
SimpleInverseDyn( const std::string & name );
SimpleInverseDyn(const std::string& name);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_posture, double);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_forces, double);
DECLARE_SIGNAL_IN(mu, double);
DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(f_min, double);
DECLARE_SIGNAL_IN(f_max, double);
DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_waist, double);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_posture, double);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_forces, double);
DECLARE_SIGNAL_IN(mu, double);
DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(f_min, double);
DECLARE_SIGNAL_IN(f_max, double);
DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_waist, double);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
/* --- COMMANDS --- */
void init(const double& dt, const std::string& robotRef);
virtual void setControlOutputType(const std::string& type);
virtual void setControlOutputType(const std::string& type);
void updateComOffset();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
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);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
protected:
double m_dt; /// control loop time period
double m_t; /// current time
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_enabled; /// True if controler is enabled
bool m_firstTime; /// True at the first iteration of the controller
double m_dt; /// control loop time period
double m_t; /// current time
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_enabled; /// True if controler is enabled
bool m_firstTime; /// True at the first iteration of the controller
/// TSID
/// Robot
tsid::robots::RobotWrapper * m_robot;
tsid::robots::RobotWrapper* m_robot;
/// Solver and problem formulation
tsid::solvers::SolverHQPBase * m_hqpSolver;
tsid::InverseDynamicsFormulationAccForce * m_invDyn;
tsid::solvers::SolverHQPBase* m_hqpSolver;
tsid::InverseDynamicsFormulationAccForce* m_invDyn;
/// TASKS
tsid::contacts::Contact6d * m_contactRF;
tsid::contacts::Contact6d * m_contactLF;
tsid::tasks::TaskComEquality * m_taskCom;
tsid::tasks::TaskSE3Equality * m_taskWaist;
tsid::tasks::TaskJointPosture * m_taskPosture;
tsid::tasks::TaskJointPosture * m_taskBlockedJoints;
tsid::contacts::Contact6d* m_contactRF;
tsid::contacts::Contact6d* m_contactLF;
tsid::tasks::TaskComEquality* m_taskCom;
tsid::tasks::TaskSE3Equality* m_taskWaist;
tsid::tasks::TaskJointPosture* m_taskPosture;
tsid::tasks::TaskJointPosture* m_taskBlockedJoints;
/// Trajectories of the tasks
tsid::trajectories::TrajectorySample m_sampleCom;
tsid::trajectories::TrajectorySample m_sampleWaist;
tsid::trajectories::TrajectorySample m_samplePosture;
tsid::trajectories::TrajectorySample m_sampleCom;
tsid::trajectories::TrajectorySample m_sampleWaist;
tsid::trajectories::TrajectorySample m_samplePosture;
/// Weights of the Tasks (which can be changed)
double m_w_com;
......@@ -182,23 +173,23 @@ class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn
double m_w_waist;
/// Computed solutions (accelerations and torques) and their derivatives
tsid::math::Vector m_dv_sot; /// desired accelerations (sot order)
tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order)
tsid::math::Vector m_v_sot; /// desired velocities (sot order)
tsid::math::Vector m_v_urdf; /// desired and current velocities (urdf order) (close the TSID loop on it)
tsid::math::Vector m_q_sot; /// desired positions (sot order)
tsid::math::Vector m_q_urdf; /// desired and current positions (urdf order) (close the TSID loop on it)
tsid::math::Vector m_tau_sot; /// desired torques (sot order)
tsid::math::Vector3 m_com_offset; /// 3d CoM offset
unsigned int m_timeLast; /// Final time of the control loop
RobotUtilShrPtr m_robot_util; /// Share pointer to the robot utils methods
ControlOutput m_ctrlMode; /// ctrl mode desired for the output (velocity or torque)
}; // class SimpleInverseDyn
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_simple_inverse_dyn_H__
tsid::math::Vector m_dv_sot; /// desired accelerations (sot order)
tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order)
tsid::math::Vector m_v_sot; /// desired velocities (sot order)
tsid::math::Vector m_v_urdf; /// desired and current velocities (urdf order) (close the TSID loop on it)
tsid::math::Vector m_q_sot; /// desired positions (sot order)
tsid::math::Vector m_q_urdf; /// desired and current positions (urdf order) (close the TSID loop on it)
tsid::math::Vector m_tau_sot; /// desired torques (sot order)
tsid::math::Vector3 m_com_offset; /// 3d CoM offset
unsigned int m_timeLast; /// Final time of the control loop
RobotUtilShrPtr m_robot_util; /// Share pointer to the robot utils methods
ControlOutput m_ctrlMode; /// ctrl mode desired for the output (velocity or torque)
}; // class SimpleInverseDyn
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_simple_inverse_dyn_H__
......@@ -90,8 +90,8 @@ class TORQUEOFFSETESTIMATOR_EXPORT TorqueOffsetEstimator : public ::dynamicgraph
// stdAlignedVector stdVecJointTorqueOffsets;
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[" + name + "] " + msg, t, file, line);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
private:
......
......@@ -74,8 +74,8 @@ class SOTTRACEPLAYER_EXPORT TracePlayer : public ::dynamicgraph::Entity {
/* --- 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);
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
}
protected:
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment