sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
position-controller.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <sot/core/debug.hh>
9#include <sot/core/stop-watch.hh>
12
13namespace dynamicgraph {
14namespace sot {
15namespace torque_control {
16namespace dynamicgraph = ::dynamicgraph;
17using namespace dynamicgraph;
18using namespace dynamicgraph::command;
19using namespace std;
20// Size to be aligned "-------------------------------------------------------"
21#define PROFILE_PWM_DES_COMPUTATION \
22 "PositionController: desired pwm computation "
23
24#define GAIN_SIGNALS m_KpSIN << m_KdSIN << m_KiSIN
25#define REF_JOINT_SIGNALS m_qRefSIN << m_dqRefSIN
26#define STATE_SIGNALS m_base6d_encodersSIN << m_jointsVelocitiesSIN
27
28#define INPUT_SIGNALS STATE_SIGNALS << REF_JOINT_SIGNALS << GAIN_SIGNALS
29
30#define OUTPUT_SIGNALS m_pwmDesSOUT << m_qErrorSOUT
31
34typedef PositionController EntityClassName;
35typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN;
36typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorN6;
37
38/* --- DG FACTORY ---------------------------------------------------- */
40
41/* ------------------------------------------------------------------- */
42/* --- CONSTRUCTION -------------------------------------------------- */
43/* ------------------------------------------------------------------- */
45 : Entity(name),
46 CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
47 CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector),
48 CONSTRUCT_SIGNAL_IN(qRef, dynamicgraph::Vector),
49 CONSTRUCT_SIGNAL_IN(dqRef, dynamicgraph::Vector),
50 CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
51 CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
52 CONSTRUCT_SIGNAL_IN(Ki, dynamicgraph::Vector),
53 CONSTRUCT_SIGNAL_OUT(pwmDes, dynamicgraph::Vector, INPUT_SIGNALS),
54 CONSTRUCT_SIGNAL_OUT(qError, dynamicgraph::Vector,
55 m_base6d_encodersSIN << m_qRefSIN),
56 m_robot_util(RefVoidRobotUtil()),
57 m_initSucceeded(false) {
58 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
59
60 /* Commands. */
61 addCommand("init", makeCommandVoid2(
63 docCommandVoid2("Initialize the entity.",
64 "Time period in seconds (double)",
65 "Reference to the robot (string)")));
66 addCommand("resetIntegral",
67 makeCommandVoid0(*this, &PositionController::resetIntegral,
68 docCommandVoid0("Reset the integral.")));
69}
70
71void PositionController::init(const double& dt, const std::string& robotRef) {
72 if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
73 if (!m_base6d_encodersSIN.isPlugged())
74 return SEND_MSG("Init failed: signal base6d_encoders is not plugged",
75 MSG_TYPE_ERROR);
76 if (!m_jointsVelocitiesSIN.isPlugged())
77 return SEND_MSG("Init failed: signal jointsVelocities is not plugged",
78 MSG_TYPE_ERROR);
79 if (!m_qRefSIN.isPlugged())
80 return SEND_MSG("Init failed: signal qRef is not plugged", MSG_TYPE_ERROR);
81 if (!m_dqRefSIN.isPlugged())
82 return SEND_MSG("Init failed: signal dqRef is not plugged", MSG_TYPE_ERROR);
83 if (!m_KpSIN.isPlugged())
84 return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
85 if (!m_KdSIN.isPlugged())
86 return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
87 if (!m_KiSIN.isPlugged())
88 return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
89
90 /* Retrieve m_robot_util informations */
91 std::string localName(robotRef);
92 if (isNameInRobotUtil(localName))
93 m_robot_util = getRobotUtil(localName);
94 else {
95 SEND_MSG("You should have an entity controller manager initialized before",
96 MSG_TYPE_ERROR);
97 return;
98 }
99
100 m_dt = dt;
101
102 m_pwmDes.setZero(m_robot_util->m_nbJoints);
103 m_q.setZero(m_robot_util->m_nbJoints + 6);
104 m_dq.setZero(m_robot_util->m_nbJoints);
105
107
108 m_initSucceeded = true;
109}
110
112 m_e_integral.setZero(m_robot_util->m_nbJoints);
113}
114
115/* ------------------------------------------------------------------- */
116/* --- SIGNALS ------------------------------------------------------- */
117/* ------------------------------------------------------------------- */
118
119DEFINE_SIGNAL_OUT_FUNCTION(pwmDes, dynamicgraph::Vector) {
120 if (!m_initSucceeded) {
121 SEND_WARNING_STREAM_MSG(
122 "Cannot compute signal pwmDes before initialization!");
123 return s;
124 }
125
126 getProfiler().start(PROFILE_PWM_DES_COMPUTATION);
127 {
128 const VectorN& Kp = m_KpSIN(iter); // n
129 const VectorN& Kd = m_KdSIN(iter); // n
130 const VectorN6& q = m_base6d_encodersSIN(iter); // n+6
131 const VectorN& dq = m_jointsVelocitiesSIN(iter); // n
132 const VectorN& qRef = m_qRefSIN(iter); // n
133 const VectorN& dqRef = m_dqRefSIN(iter); // n
134
135 assert(q.size() == static_cast<Eigen::VectorXd::Index>(
136 m_robot_util->m_nbJoints + 6) &&
137 "Unexpected size of signal base6d_encoder");
138 assert(dq.size() ==
139 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
140 "Unexpected size of signal dq");
141 assert(qRef.size() ==
142 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
143 "Unexpected size of signal qRef");
144 assert(dqRef.size() ==
145 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
146 "Unexpected size of signal dqRef");
147 assert(Kp.size() ==
148 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
149 "Unexpected size of signal Kd");
150 assert(Kd.size() ==
151 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
152 "Unexpected size of signal Kd");
153
154 m_pwmDes = Kp.cwiseProduct(qRef - q.tail(m_robot_util->m_nbJoints)) +
155 Kd.cwiseProduct(dqRef - dq);
156
157 if (s.size() !=
158 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
159 s.resize(m_robot_util->m_nbJoints);
160 s = m_pwmDes;
161 }
162 getProfiler().stop(PROFILE_PWM_DES_COMPUTATION);
163
164 return s;
165}
166
167DEFINE_SIGNAL_OUT_FUNCTION(qError, dynamicgraph::Vector) {
168 if (!m_initSucceeded) {
169 SEND_MSG("Cannot compute signal qError before initialization!",
170 MSG_TYPE_WARNING_STREAM);
171 return s;
172 }
173
174 const VectorN6& q = m_base6d_encodersSIN(iter); // n+6
175 const VectorN& qRef = m_qRefSIN(iter); // n
176 assert(q.size() == static_cast<Eigen::VectorXd::Index>(
177 m_robot_util->m_nbJoints + 6) &&
178 "Unexpected size of signal base6d_encoder");
179 assert(qRef.size() ==
180 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
181 "Unexpected size of signal qRef");
182
183 if (s.size() != static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
184 s.resize(m_robot_util->m_nbJoints);
185 s = qRef - q.tail(m_robot_util->m_nbJoints);
186
187 return s;
188}
189
190/* --- COMMANDS ---------------------------------------------------------- */
191
192/* ------------------------------------------------------------------- */
193/* --- ENTITY -------------------------------------------------------- */
194/* ------------------------------------------------------------------- */
195
196void PositionController::display(std::ostream& os) const {
197 os << "PositionController " << getName();
198 try {
199 getProfiler().report_all(3, os);
200 } catch (ExceptionSignal e) {
201 }
202}
203} // namespace torque_control
204} // namespace sot
205} // namespace dynamicgraph
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
Eigen::VectorXd m_e_integral
control loop time period
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PositionController(const std::string &name)
double m_dt
true if the entity has been successfully initialized
virtual void display(std::ostream &os) const
qRef-q
void init(const double &dt, const std::string &robotRef)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
to read text file
Definition: treeview.dox:22
#define PROFILE_PWM_DES_COMPUTATION