1 |
|
|
/* |
2 |
|
|
* Copyright 2015, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#ifndef __sot_torque_control_position_controller_H__ |
7 |
|
|
#define __sot_torque_control_position_controller_H__ |
8 |
|
|
|
9 |
|
|
/* --------------------------------------------------------------------- */ |
10 |
|
|
/* --- API ------------------------------------------------------------- */ |
11 |
|
|
/* --------------------------------------------------------------------- */ |
12 |
|
|
|
13 |
|
|
#if defined(WIN32) |
14 |
|
|
#if defined(position_controller_EXPORTS) |
15 |
|
|
#define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllexport) |
16 |
|
|
#else |
17 |
|
|
#define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllimport) |
18 |
|
|
#endif |
19 |
|
|
#else |
20 |
|
|
#define SOTPOSITIONCONTROLLER_EXPORT |
21 |
|
|
#endif |
22 |
|
|
|
23 |
|
|
/* --------------------------------------------------------------------- */ |
24 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
25 |
|
|
/* --------------------------------------------------------------------- */ |
26 |
|
|
|
27 |
|
|
#include <pinocchio/fwd.hpp> |
28 |
|
|
|
29 |
|
|
// include pinocchio first |
30 |
|
|
|
31 |
|
|
#include <dynamic-graph/signal-helper.h> |
32 |
|
|
|
33 |
|
|
#include <boost/assign.hpp> |
34 |
|
|
#include <map> |
35 |
|
|
#include <sot/core/matrix-geometry.hh> |
36 |
|
|
#include <sot/core/robot-utils.hh> |
37 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
38 |
|
|
|
39 |
|
|
namespace dynamicgraph { |
40 |
|
|
namespace sot { |
41 |
|
|
namespace torque_control { |
42 |
|
|
|
43 |
|
|
/* --------------------------------------------------------------------- */ |
44 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
45 |
|
|
/* --------------------------------------------------------------------- */ |
46 |
|
|
|
47 |
|
|
class SOTPOSITIONCONTROLLER_EXPORT PositionController |
48 |
|
|
: public ::dynamicgraph::Entity { |
49 |
|
|
typedef PositionController EntityClassName; |
50 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
51 |
|
|
|
52 |
|
|
public: |
53 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
54 |
|
|
|
55 |
|
|
/* --- CONSTRUCTOR ---- */ |
56 |
|
|
PositionController(const std::string& name); |
57 |
|
|
|
58 |
|
|
void init(const double& dt, const std::string& robotRef); |
59 |
|
|
|
60 |
|
|
void resetIntegral(); |
61 |
|
|
|
62 |
|
|
/* --- SIGNALS --- */ |
63 |
|
|
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector); |
64 |
|
|
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector); |
65 |
|
|
DECLARE_SIGNAL_IN(qRef, dynamicgraph::Vector); |
66 |
|
|
DECLARE_SIGNAL_IN(dqRef, dynamicgraph::Vector); |
67 |
|
|
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector); /// joint proportional gains |
68 |
|
|
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector); /// joint derivative gains |
69 |
|
|
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector); /// joint integral gains |
70 |
|
|
|
71 |
|
|
DECLARE_SIGNAL_OUT(pwmDes, |
72 |
|
|
dynamicgraph::Vector); /// Kp*e_q + Kd*de_q + Ki*int(e_q) |
73 |
|
|
// DEBUG SIGNALS |
74 |
|
|
DECLARE_SIGNAL_OUT(qError, dynamicgraph::Vector); /// qRef-q |
75 |
|
|
|
76 |
|
|
/* --- COMMANDS --- */ |
77 |
|
|
/* --- ENTITY INHERITANCE --- */ |
78 |
|
|
virtual void display(std::ostream& os) const; |
79 |
|
|
|
80 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
81 |
|
|
const char* = "", int = 0) { |
82 |
|
|
logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n'; |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
protected: |
86 |
|
|
RobotUtilShrPtr m_robot_util; /// Robot Util |
87 |
|
|
Eigen::VectorXd m_pwmDes; |
88 |
|
|
bool |
89 |
|
|
m_initSucceeded; /// true if the entity has been successfully initialized |
90 |
|
|
double m_dt; /// control loop time period |
91 |
|
|
|
92 |
|
|
/// Integral of the joint tracking errors |
93 |
|
|
Eigen::VectorXd m_e_integral; |
94 |
|
|
|
95 |
|
|
Eigen::VectorXd m_q, m_dq; |
96 |
|
|
|
97 |
|
|
}; // class PositionController |
98 |
|
|
|
99 |
|
|
} // namespace torque_control |
100 |
|
|
} // namespace sot |
101 |
|
|
} // namespace dynamicgraph |
102 |
|
|
|
103 |
|
|
#endif // #ifndef __sot_torque_control_position_controller_H__ |