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