sot-talos-balance  1.6.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  if(s.size()!=6)
156  s.resize(6);
157 
159 
160  const Vector &force = m_forceSIN(iter);
161  const Vector &q = m_qSIN(iter);
162  assert(force.size() == m_n && "Unexpected size of signal force");
163  assert(q.size() == m_model.nq && "Unexpected size of signal q");
164 
165  // Get sensorPlacement
166  pinocchio::framesForwardKinematics(m_model, *m_data, q);
167  pinocchio::SE3 sensorPlacement = m_data->oMf[m_sensorFrameId];
168 
169  s = sensorPlacement.act(pinocchio::Force(force)).toVector();
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  if(s.size()!=6)
184  s.resize(6);
185 
187 
188  const Vector &w_forceDes = m_w_forceDesSIN(iter);
189  const Vector &w_force = m_w_forceSINNER(iter);
190  const Vector &Kp = m_KpSIN(iter);
191  const Vector &Kd = m_KdSIN(iter);
192  const Vector &dqSaturation = m_dqSaturationSIN(iter);
193  assert(w_force.size() == m_n && "Unexpected size of signal force");
194  assert(w_forceDes.size() == m_n && "Unexpected size of signal w_forceDes");
195  assert(Kp.size() == m_n && "Unexpected size of signal Kp");
196  assert(Kd.size() == m_n && "Unexpected size of signal Kd");
197  assert(dqSaturation.size() == m_n && "Unexpected size of signal dqSaturation");
198 
199  m_w_dq = m_w_dq + m_dt * (Kp.cwiseProduct(w_forceDes - w_force)) - Kd.cwiseProduct(m_w_dq);
200 
201  for (int i = 0; i < m_n; i++)
202  {
203  if (m_w_dq[i] > dqSaturation[i])
204  m_w_dq[i] = dqSaturation[i];
205  if (m_w_dq[i] < -dqSaturation[i])
206  m_w_dq[i] = -dqSaturation[i];
207  }
208 
209  s = m_w_dq;
210 
212 
213  return s;
214 }
215 
217 {
218  if (!m_initSucceeded)
219  {
220  SEND_WARNING_STREAM_MSG("Cannot compute signal dq before initialization!");
221  return s;
222  }
223  if(s.size()!=6)
224  s.resize(6);
225 
227 
228  const Vector &w_dq = m_w_dqSINNER(iter);
229  assert(w_dq.size() == m_n && "Unexpected size of signal w_dq");
230 
231  // Get endEffectorPlacement
232  pinocchio::SE3 placement = m_data->oMi[m_endEffectorId];
233 
234  s = placement.actInv(pinocchio::Motion(w_dq)).toVector();
235 
237 
238  return s;
239 }
240 
241 /* --- COMMANDS ---------------------------------------------------------- */
242 
243 /* ------------------------------------------------------------------- */
244 /* --- ENTITY -------------------------------------------------------- */
245 /* ------------------------------------------------------------------- */
246 void AdmittanceControllerEndEffector::display(std::ostream &os) const
247 {
248  os << "AdmittanceControllerEndEffector " << getName();
249  try
250  {
251  getProfiler().report_all(3, os);
252  }
253  catch (ExceptionSignal e)
254  {
255  }
256 }
257 } // namespace talos_balance
258 } // namespace sot
259 } // 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.