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