sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
torque-offset-estimator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
3  *
4  */
5 
6 #include <Eigen/Dense>
7 
8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
13 
14 namespace dynamicgraph {
15 namespace sot {
16 namespace torque_control {
17 
18 #define ALL_INPUT_SIGNALS m_base6d_encodersSIN << m_accelerometerSIN << m_jointTorquesSIN << m_gyroscopeSIN
19 
20 #define ALL_OUTPUT_SIGNALS m_jointTorquesEstimatedSOUT
21 
22 namespace dynamicgraph = ::dynamicgraph;
23 using namespace dynamicgraph;
24 using namespace dynamicgraph::command;
25 using namespace Eigen;
26 
29 typedef TorqueOffsetEstimator EntityClassName;
30 typedef int dummy;
31 
32 /* --- DG FACTORY ------------------------------------------------------- */
34 
35 /* --- CONSTRUCTION ----------------------------------------------------- */
36 /* --- CONSTRUCTION ----------------------------------------------------- */
37 /* --- CONSTRUCTION ----------------------------------------------------- */
39  : Entity(name),
40  CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
41  CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector),
42  CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector),
43  CONSTRUCT_SIGNAL_IN(jointTorques, dynamicgraph::Vector),
44  CONSTRUCT_SIGNAL_INNER(collectSensorData, dynamicgraph::Vector, ALL_INPUT_SIGNALS),
45  CONSTRUCT_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector,
46  m_collectSensorDataSINNER << ALL_INPUT_SIGNALS),
47  sensor_offset_status(PRECOMPUTATION),
48  current_progress(0) {
49  Entity::signalRegistration(ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);
50  addCommand("init", makeCommandVoid5(*this, &TorqueOffsetEstimator::init,
51  docCommandVoid5("Initialize the estimator", "urdfFilePath",
52  "Homogeneous transformation from chest frame to IMU frame",
53  "Maximum angular velocity allowed in either axis",
54  "Freeflyer joint name", "chest joint name")));
55  addCommand("computeOffset",
56  makeCommandVoid2(*this, &TorqueOffsetEstimator::computeOffset,
57  docCommandVoid2("Compute the offset for sensor calibration",
58  "Number of iteration to average over.", "Maximum allowed offset")));
59  addCommand("getSensorOffsets", makeDirectGetter(*this, &jointTorqueOffsets,
60  docDirectGetter("Return the computed sensor offsets", "vector")));
61 
62  // encSignals.clear();
63  // accSignals.clear();
64  // gyrSignals.clear();
65  // tauSignals.clear();
66  // stdVecJointTorqueOffsets.clear();
67 }
68 
69 /* --- COMMANDS ---------------------------------------------------------- */
70 /* --- COMMANDS ---------------------------------------------------------- */
71 /* --- COMMANDS ---------------------------------------------------------- */
72 void TorqueOffsetEstimator::init(const std::string& robotRef, const Eigen::Matrix4d& m_torso_X_imu_,
73  const double& gyro_epsilon_, const std::string& ffJointName,
74  const std::string& torsoJointName) {
75  try {
76  /* Retrieve m_robot_util informations */
77  std::string localName(robotRef);
78  if (isNameInRobotUtil(localName)) {
79  m_robot_util = getRobotUtil(localName);
80  std::cerr << "m_robot_util:" << m_robot_util << std::endl;
81  } else {
82  SEND_MSG("You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
83  return;
84  }
85 
86  pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), m_model);
87  // assert(m_model.nq == N_JOINTS+7);
88  // assert(m_model.nv == N_JOINTS+6);
89 
90  jointTorqueOffsets.resize(m_model.nv - 6);
91  jointTorqueOffsets.setZero();
92  ffIndex = m_model.getJointId(ffJointName);
93  torsoIndex = m_model.getJointId(torsoJointName);
94  } catch (const std::exception& e) {
95  std::cout << e.what();
96  SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
97  return;
98  }
99  m_data = new pinocchio::Data(m_model);
100  m_torso_X_imu.rotation() = m_torso_X_imu_.block<3, 3>(0, 0);
101  m_torso_X_imu.translation() = m_torso_X_imu_.block<3, 1>(0, 3);
102  gyro_epsilon = gyro_epsilon_;
103 }
104 
105 void TorqueOffsetEstimator::computeOffset(const int& nIterations, const double& epsilon_) {
106  if (sensor_offset_status == PRECOMPUTATION) {
107  SEND_MSG("Starting offset computation with no. iterations:" + nIterations, MSG_TYPE_DEBUG);
108  n_iterations = nIterations;
109  epsilon = epsilon_;
110  sensor_offset_status = INPROGRESS;
111  } else if (sensor_offset_status == INPROGRESS) {
112  SEND_MSG("Collecting input signals. Please keep the graph running", MSG_TYPE_WARNING);
113  } else { // sensor_offset_status == COMPUTED
114  SEND_MSG("Recomputing offset with no. iterations:" + nIterations, MSG_TYPE_DEBUG);
115 
116  // stdVecJointTorqueOffsets.clear();
117  current_progress = 0;
118  jointTorqueOffsets.setZero();
119 
120  n_iterations = nIterations;
121  epsilon = epsilon_;
122  sensor_offset_status = INPROGRESS;
123  }
124  return;
125 }
126 
127 DEFINE_SIGNAL_INNER_FUNCTION(collectSensorData, dummy) {
128  if (sensor_offset_status == INPROGRESS) {
129  const Eigen::VectorXd& gyro = m_gyroscopeSIN(iter);
130  if (gyro.array().abs().maxCoeff() >= gyro_epsilon) {
131  SEND_MSG("Very High Angular Rotations.", MSG_TYPE_ERROR_STREAM);
132  }
133 
134  // Check the current iteration status
135  int i = current_progress;
136 
137  if (i < n_iterations) {
138  SEND_MSG("Collecting signals for iteration no:" + i, MSG_TYPE_DEBUG_STREAM);
139 
140  const Eigen::VectorXd& sot_enc = m_base6d_encodersSIN(iter);
141  const Eigen::VectorXd& IMU_acc = m_accelerometerSIN(iter);
142  const Eigen::VectorXd& tau = m_jointTorquesSIN(iter);
143 
144  Eigen::VectorXd enc(m_model.nq);
145  // joints_sot_to_urdf(sot_enc.tail(m_model.nv-6), enc.tail(m_model.nv-6));
146  enc.tail(m_model.nv - 6) = sot_enc.tail(m_model.nv - 6);
147  // base_sot_to_urdf(sot_enc.head<6>(), enc.head<7>());
148  enc.head<6>().setZero();
149  enc[6] = 1.0;
150 
151  // Get the transformation from ff(f) to torso (t) to IMU(i) frame:
152  // fMi = oMf^-1 * fMt * tMi
153  pinocchio::forwardKinematics(m_model, *m_data, enc);
154  // pinocchio::SE3 fMi = m_data->oMi[ffIndex].inverse()*m_data->oMi[torsoIndex]*m_torso_X_imu;
155  pinocchio::SE3 oMimu = m_data->oMi[torsoIndex] * m_torso_X_imu;
156 
157  // Move the IMU signal to the base frame.
158  // angularAcceleration is zero. Intermediate frame acc and velocities are zero
159  const dynamicgraph::Vector acc = oMimu.rotation() * IMU_acc; // Torso Acceleration
160 
161  // Deal with gravity predefined in robot model. Robot Z should be pointing upwards
162  m_model.gravity.linear() = acc;
163  // Set fixed for DEBUG
164  // m_model.gravity.linear() = m_model.gravity981;
165 
166  const Eigen::VectorXd& tau_rnea =
167  pinocchio::rnea(m_model, *m_data, enc, Eigen::VectorXd::Zero(m_model.nv), Eigen::VectorXd::Zero(m_model.nv));
168  const Eigen::VectorXd current_offset = tau - tau_rnea.tail(m_model.nv - 6);
169  if (current_offset.array().abs().maxCoeff() >= epsilon) {
170  SEND_MSG("Too high torque offset estimated for iteration" + i, MSG_TYPE_ERROR);
171  assert(false);
172  }
173  // encSignals.push_back(_enc);
174  // accSignals.push_back(_acc);
175  // gyrSignals.push_back(_gyr);
176  // tauSignals.push_back(_tau);
177  jointTorqueOffsets += current_offset;
178  current_progress++;
179  } else if (i == n_iterations) {
180  // Find the mean, enough signals collected
181  jointTorqueOffsets /= n_iterations;
182  sensor_offset_status = COMPUTED;
183  } else { // i > n_iterations
184  // Doesn't reach here.
185  assert(false && "Error in collectSensorData. ");
186  }
187  }
188  // else { // sensor_offset_status == COMPUTED
189  // }
190  return s;
191 }
192 
193 DEFINE_SIGNAL_OUT_FUNCTION(jointTorquesEstimated, dynamicgraph::Vector) {
194  m_collectSensorDataSINNER(iter);
195 
196  if (s.size() != m_model.nv - 6) s.resize(m_model.nv - 6);
197 
198  if (sensor_offset_status == PRECOMPUTATION || sensor_offset_status == INPROGRESS) {
199  s = m_jointTorquesSIN(iter);
200  } else { // sensor_offset_status == COMPUTED
201  const dynamicgraph::Vector& inputTorques = m_jointTorquesSIN(iter);
202  s = inputTorques - jointTorqueOffsets;
203  }
204  return s;
205 }
206 
207 void TorqueOffsetEstimator::display(std::ostream& os) const {
208  os << "TorqueOffsetEstimator" << getName() << ":\n";
209  try {
210  getProfiler().report_all(3, os);
211  } catch (ExceptionSignal e) {
212  }
213 }
214 
215 } // namespace torque_control
216 } // namespace sot
217 } // namespace dynamicgraph
void computeOffset(const int &nIterations, const double &epsilon)
void init(const std::string &urdfFile, const Eigen::Matrix4d &_m_torso_X_imu, const double &gyro_epsilon, const std::string &ffJointName, const std::string &torsoJointName)
CommandVoid5< E, T1, T2, T3, T4, T5 > * makeCommandVoid5(E &entity, typename CommandVoid5< E, T1, T2, T3, T4, T5 >::function_t function, const std::string &docString)
std::string docCommandVoid5(const std::string &doc, const std::string &type1, const std::string &type2, const std::string &type3, const std::string &type4, const std::string &type5)
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
to read text file
Definition: treeview.dox:22
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS