sot-talos-balance  1.5.0
admittance-controller-end-effector.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  *
4  * LAAS-CNRS
5  *
6  * Fanny Risbourg
7  * This file is part of sot-talos-balance.
8  * See license file.
9  */
10 
12 #include <sot/core/debug.hh>
13 #include <dynamic-graph/factory.h>
14 #include <dynamic-graph/all-commands.h>
15 #include <sot/core/stop-watch.hh>
16 
17 namespace dynamicgraph
18 {
19 namespace sot
20 {
21 namespace talos_balance
22 {
23 namespace dg = ::dynamicgraph;
24 using namespace dg;
25 using namespace pinocchio;
26 using namespace dg::command;
27 
28 #define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WFORCE_COMPUTATION \
29  "AdmittanceControllerEndEffector: w_force computation "
30 
31 #define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WDQ_COMPUTATION \
32  "AdmittanceControllerEndEffector: w_dq computation "
33 
34 #define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_DQ_COMPUTATION \
35  "AdmittanceControllerEndEffector: dq computation "
36 
37 #define INPUT_SIGNALS m_KpSIN << m_KdSIN << m_dqSaturationSIN << m_forceSIN << m_w_forceDesSIN << m_qSIN
38 
39 #define INNER_SIGNALS m_w_forceSINNER << m_w_dqSINNER
40 
41 #define OUTPUT_SIGNALS m_dqSOUT
42 
46 
47 /* --- DG FACTORY ---------------------------------------------------- */
49  "AdmittanceControllerEndEffector");
50 
51 /* ------------------------------------------------------------------- */
52 /* --- CONSTRUCTION -------------------------------------------------- */
53 /* ------------------------------------------------------------------- */
55  : Entity(name),
56  CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
57  CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
58  CONSTRUCT_SIGNAL_IN(dqSaturation, dynamicgraph::Vector),
59  CONSTRUCT_SIGNAL_IN(force, dynamicgraph::Vector),
60  CONSTRUCT_SIGNAL_IN(w_forceDes, dynamicgraph::Vector),
61  CONSTRUCT_SIGNAL_IN(q, dynamicgraph::Vector),
62  CONSTRUCT_SIGNAL_INNER(w_force, dynamicgraph::Vector, m_forceSIN),
63  CONSTRUCT_SIGNAL_INNER(w_dq, dynamicgraph::Vector, INPUT_SIGNALS << m_w_forceSINNER),
64  CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_w_dqSINNER),
65  m_initSucceeded(false),
66  m_robot_util(),
67  m_model(),
68  m_data(),
69  m_sensorFrameId(),
70  m_endEffectorId()
71 {
72  Entity::signalRegistration(INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS);
73 
74  /* Commands. */
75  addCommand("init", makeCommandVoid3(*this,
77  docCommandVoid3("Initialize the entity.",
78  "time step",
79  "sensor frame name",
80  "end Effector Joint Name")));
81  addCommand("resetDq", makeCommandVoid0(*this,
83  docCommandVoid0("resetDq")));
84 }
85 
87  const std::string &sensorFrameName,
88  const std::string &endEffectorName)
89 {
90  if (!m_dqSaturationSIN.isPlugged())
91  return SEND_MSG("Init failed: signal dqSaturation is not plugged", MSG_TYPE_ERROR);
92  if (!m_KpSIN.isPlugged())
93  return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
94  if (!m_KdSIN.isPlugged())
95  return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
96  if (!m_forceSIN.isPlugged())
97  return SEND_MSG("Init failed: signal force is not plugged", MSG_TYPE_ERROR);
98  if (!m_w_forceDesSIN.isPlugged())
99  return SEND_MSG("Init failed: signal w_forceDes is not plugged", MSG_TYPE_ERROR);
100  if (!m_qSIN.isPlugged())
101  return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
102 
103  m_n = 6;
104  m_dt = dt;
105  m_w_dq.setZero(m_n);
106 
107  try
108  {
109  /* Retrieve m_robot_util informations */
110  std::string localName("robot");
111  if (isNameInRobotUtil(localName))
112  {
113  m_robot_util = getRobotUtil(localName);
114  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
115  }
116  else
117  {
118  SEND_MSG("You should have a robotUtil pointer initialized before", MSG_TYPE_ERROR);
119  return;
120  }
121 
122  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
123  m_data = new pinocchio::Data(m_model);
124 
125  m_endEffectorId = m_model.getJointId(endEffectorName);
126  m_sensorFrameId = m_model.getFrameId(sensorFrameName);
127  }
128  catch (const std::exception &e)
129  {
130  std::cout << e.what();
131  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
132  MSG_TYPE_ERROR);
133  return;
134  }
135 
136  m_initSucceeded = true;
137 }
138 
140 {
141  m_w_dq.setZero(m_n);
142  return;
143 }
144 
145 /* ------------------------------------------------------------------- */
146 /* --- SIGNALS ------------------------------------------------------- */
147 /* ------------------------------------------------------------------- */
149 {
150  if (!m_initSucceeded)
151  {
152  SEND_WARNING_STREAM_MSG("Cannot compute signal w_force before initialization!");
153  return s;
154  }
155 
157 
158  const Vector &force = m_forceSIN(iter);
159  const Vector &q = m_qSIN(iter);
160  assert(force.size() == m_n && "Unexpected size of signal force");
161  assert(q.size() == m_model.nq && "Unexpected size of signal q");
162 
163  // Get sensorPlacement
164  pinocchio::framesForwardKinematics(m_model, *m_data, q);
165  pinocchio::SE3 sensorPlacement = m_data->oMf[m_sensorFrameId];
166 
167  Vector w_force = sensorPlacement.act(pinocchio::Force(force)).toVector();
168 
169  s = w_force;
170 
172 
173  return s;
174 }
175 
177 {
178  if (!m_initSucceeded)
179  {
180  SEND_WARNING_STREAM_MSG("Cannot compute signal w_dq before initialization!");
181  return s;
182  }
183 
185 
186  const Vector &w_forceDes = m_w_forceDesSIN(iter);
187  const Vector &w_force = m_w_forceSINNER(iter);
188  const Vector &Kp = m_KpSIN(iter);
189  const Vector &Kd = m_KdSIN(iter);
190  const Vector &dqSaturation = m_dqSaturationSIN(iter);
191  assert(force.size() == m_n && "Unexpected size of signal force");
192  assert(w_forceDes.size() == m_n && "Unexpected size of signal w_forceDes");
193  assert(Kp.size() == m_n && "Unexpected size of signal Kp");
194  assert(Kd.size() == m_n && "Unexpected size of signal Kd");
195  assert(dqSaturation.size() == m_n && "Unexpected size of signal dqSaturation");
196 
197  m_w_dq = m_w_dq + m_dt * (Kp.cwiseProduct(w_forceDes - w_force)) - Kd.cwiseProduct(m_w_dq);
198 
199  for (int i = 0; i < m_n; i++)
200  {
201  if (m_w_dq[i] > dqSaturation[i])
202  m_w_dq[i] = dqSaturation[i];
203  if (m_w_dq[i] < -dqSaturation[i])
204  m_w_dq[i] = -dqSaturation[i];
205  }
206 
207  s = m_w_dq;
208 
210 
211  return s;
212 }
213 
215 {
216  if (!m_initSucceeded)
217  {
218  SEND_WARNING_STREAM_MSG("Cannot compute signal dq before initialization!");
219  return s;
220  }
221 
223 
224  const Vector &w_dq = m_w_dqSINNER(iter);
225  assert(w_dq.size() == m_n && "Unexpected size of signal w_dq");
226 
227  // Get endEffectorPlacement
228  pinocchio::SE3 placement = m_data->oMi[m_endEffectorId];
229 
230  Vector velocity = placement.actInv(pinocchio::Motion(w_dq)).toVector();
231 
232  s = velocity;
233 
235 
236  return s;
237 }
238 
239 /* --- COMMANDS ------------s---------------------------------------------- */
240 
241 /* ------------------------------------------------------------------- */
242 /* --- ENTITY -------------------------------------------------------- */
243 /* ------------------------------------------------------------------- */
244 void AdmittanceControllerEndEffector::display(std::ostream &os) const
245 {
246  os << "AdmittanceControllerEndEffector " << getName();
247  try
248  {
249  getProfiler().report_all(3, os);
250  }
251  catch (ExceptionSignal e)
252  {
253  }
254 }
255 } // namespace talos_balance
256 } // namespace sot
257 } // namespace dynamicgraph
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
RobotUtilShrPtr m_robot_util
Robot Util instance to get the sensor frame.
void init(const double &dt, const std::string &sensorFrameName, const std::string &endeffectorName)
Initialize the entity.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerEndEffector, "AdmittanceControllerEndEffector")
EIGEN_MAKE_ALIGNED_OPERATOR_NEW AdmittanceControllerEndEffector(const std::string &name)
bool m_initSucceeded
True if the entity has been successfully initialized.
#define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WFORCE_COMPUTATION
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector)
Admittance controller for an upper body end-effector (right or left wrist)
#define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_WDQ_COMPUTATION
#define PROFILE_ADMITTANCECONTROLLERENDEFFECTOR_DQ_COMPUTATION
pinocchio::JointIndex m_endEffectorId
Id of the joint of the end-effector.