1 |
|
|
/* |
2 |
|
|
* Copyright 2017, Thomas Flayols, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <sot/core/debug.hh> |
9 |
|
|
#include <sot/core/stop-watch.hh> |
10 |
|
|
#include <sot/torque_control/base-estimator.hh> |
11 |
|
|
#include <sot/torque_control/commands-helper.hh> |
12 |
|
|
|
13 |
|
|
#include "pinocchio/algorithm/frames.hpp" |
14 |
|
|
|
15 |
|
|
namespace dynamicgraph { |
16 |
|
|
namespace sot { |
17 |
|
|
namespace torque_control { |
18 |
|
|
namespace dg = ::dynamicgraph; |
19 |
|
|
using namespace dg; |
20 |
|
|
using namespace dg::command; |
21 |
|
|
using namespace std; |
22 |
|
|
using namespace pinocchio; |
23 |
|
|
using boost::math::normal; // typedef provides default type is double. |
24 |
|
|
|
25 |
|
|
void se3Interp(const pinocchio::SE3& s1, const pinocchio::SE3& s2, |
26 |
|
|
const double alpha, pinocchio::SE3& s12) { |
27 |
|
|
const Eigen::Vector3d t_(s1.translation() * alpha + |
28 |
|
|
s2.translation() * (1 - alpha)); |
29 |
|
|
|
30 |
|
|
const Eigen::Vector3d w(pinocchio::log3(s1.rotation()) * alpha + |
31 |
|
|
pinocchio::log3(s2.rotation()) * (1 - alpha)); |
32 |
|
|
|
33 |
|
|
s12 = pinocchio::SE3(pinocchio::exp3(w), t_); |
34 |
|
|
} |
35 |
|
|
void rpyToMatrix(double roll, double pitch, double yaw, Eigen::Matrix3d& R) { |
36 |
|
|
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); |
37 |
|
|
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); |
38 |
|
|
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); |
39 |
|
|
Eigen::Quaternion<double> q = yawAngle * pitchAngle * rollAngle; |
40 |
|
|
R = q.matrix(); |
41 |
|
|
} |
42 |
|
|
|
43 |
|
|
void rpyToMatrix(const Eigen::Vector3d& rpy, Eigen::Matrix3d& R) { |
44 |
|
|
return rpyToMatrix(rpy[0], rpy[1], rpy[2], R); |
45 |
|
|
} |
46 |
|
|
|
47 |
|
|
void matrixToRpy(const Eigen::Matrix3d& M, Eigen::Vector3d& rpy) { |
48 |
|
|
double m = sqrt(M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2)); |
49 |
|
|
rpy(1) = atan2(-M(2, 0), m); |
50 |
|
|
if (fabs(fabs(rpy(1)) - 0.5 * M_PI) < 0.001) { |
51 |
|
|
rpy(0) = 0.0; |
52 |
|
|
rpy(2) = -atan2(M(0, 1), M(1, 1)); |
53 |
|
|
} else { |
54 |
|
|
rpy(2) = atan2(M(1, 0), M(0, 0)); // alpha |
55 |
|
|
rpy(0) = atan2(M(2, 1), M(2, 2)); // gamma |
56 |
|
|
} |
57 |
|
|
} |
58 |
|
|
|
59 |
|
|
void quanternionMult(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, |
60 |
|
|
Eigen::Vector4d& q12) { |
61 |
|
|
q12(0) = q2(0) * q1(0) - q2(1) * q1(1) - q2(2) * q1(2) - q2(3) * q1(3); |
62 |
|
|
q12(1) = q2(0) * q1(1) + q2(1) * q1(0) - q2(2) * q1(3) + q2(3) * q1(2); |
63 |
|
|
q12(2) = q2(0) * q1(2) + q2(1) * q1(3) + q2(2) * q1(0) - q2(3) * q1(1); |
64 |
|
|
q12(3) = q2(0) * q1(3) - q2(1) * q1(2) + q2(2) * q1(1) + q2(3) * q1(0); |
65 |
|
|
} |
66 |
|
|
|
67 |
|
|
void pointRotationByQuaternion(const Eigen::Vector3d& point, |
68 |
|
|
const Eigen::Vector4d& quat, |
69 |
|
|
Eigen::Vector3d& rotatedPoint) { |
70 |
|
|
const Eigen::Vector4d p4(0.0, point(0), point(1), point(2)); |
71 |
|
|
const Eigen::Vector4d quat_conj(quat(0), -quat(1), -quat(2), -quat(3)); |
72 |
|
|
Eigen::Vector4d q_tmp1, q_tmp2; |
73 |
|
|
quanternionMult(quat, p4, q_tmp1); |
74 |
|
|
quanternionMult(q_tmp1, quat_conj, q_tmp2); |
75 |
|
|
rotatedPoint(0) = q_tmp2(1); |
76 |
|
|
rotatedPoint(1) = q_tmp2(2); |
77 |
|
|
rotatedPoint(2) = q_tmp2(3); |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
#define PROFILE_BASE_POSITION_ESTIMATION "base-est position estimation" |
81 |
|
|
#define PROFILE_BASE_VELOCITY_ESTIMATION "base-est velocity estimation" |
82 |
|
|
#define PROFILE_BASE_KINEMATICS_COMPUTATION "base-est kinematics computation" |
83 |
|
|
|
84 |
|
|
#define INPUT_SIGNALS \ |
85 |
|
|
m_joint_positionsSIN << m_joint_velocitiesSIN << m_imu_quaternionSIN \ |
86 |
|
|
<< m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN \ |
87 |
|
|
<< m_dforceRLEGSIN << m_w_lf_inSIN << m_w_rf_inSIN \ |
88 |
|
|
<< m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN \ |
89 |
|
|
<< m_rf_ref_xyzquatSIN << m_accelerometerSIN \ |
90 |
|
|
<< m_gyroscopeSIN |
91 |
|
|
#define OUTPUT_SIGNALS \ |
92 |
|
|
m_qSOUT << m_vSOUT << m_v_kinSOUT << m_v_flexSOUT << m_v_imuSOUT \ |
93 |
|
|
<< m_v_gyrSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << m_a_acSOUT \ |
94 |
|
|
<< m_v_acSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT \ |
95 |
|
|
<< m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT \ |
96 |
|
|
<< m_w_rf_filteredSOUT |
97 |
|
|
|
98 |
|
|
/// Define EntityClassName here rather than in the header file |
99 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
100 |
|
|
typedef BaseEstimator EntityClassName; |
101 |
|
|
|
102 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
103 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(BaseEstimator, "BaseEstimator"); |
104 |
|
|
|
105 |
|
|
/* ------------------------------------------------------------------- */ |
106 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
107 |
|
|
/* ------------------------------------------------------------------- */ |
108 |
|
|
BaseEstimator::BaseEstimator(const std::string& name) |
109 |
|
|
: Entity(name), |
110 |
|
|
CONSTRUCT_SIGNAL_IN(joint_positions, dynamicgraph::Vector), |
111 |
|
|
CONSTRUCT_SIGNAL_IN(joint_velocities, dynamicgraph::Vector), |
112 |
|
|
CONSTRUCT_SIGNAL_IN(imu_quaternion, dynamicgraph::Vector), |
113 |
|
|
CONSTRUCT_SIGNAL_IN(forceLLEG, dynamicgraph::Vector), |
114 |
|
|
CONSTRUCT_SIGNAL_IN(forceRLEG, dynamicgraph::Vector), |
115 |
|
|
CONSTRUCT_SIGNAL_IN(dforceLLEG, dynamicgraph::Vector), |
116 |
|
|
CONSTRUCT_SIGNAL_IN(dforceRLEG, dynamicgraph::Vector), |
117 |
|
|
CONSTRUCT_SIGNAL_IN(w_lf_in, double), |
118 |
|
|
CONSTRUCT_SIGNAL_IN(w_rf_in, double), |
119 |
|
|
CONSTRUCT_SIGNAL_IN(K_fb_feet_poses, double), |
120 |
|
|
CONSTRUCT_SIGNAL_IN(lf_ref_xyzquat, dynamicgraph::Vector), |
121 |
|
|
CONSTRUCT_SIGNAL_IN(rf_ref_xyzquat, dynamicgraph::Vector), |
122 |
|
|
CONSTRUCT_SIGNAL_IN(accelerometer, dynamicgraph::Vector), |
123 |
|
|
CONSTRUCT_SIGNAL_IN(gyroscope, dynamicgraph::Vector), |
124 |
|
|
CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector, |
125 |
|
|
m_joint_positionsSIN << m_joint_velocitiesSIN), |
126 |
|
|
CONSTRUCT_SIGNAL_OUT(q, dynamicgraph::Vector, |
127 |
|
|
m_kinematics_computationsSINNER |
128 |
|
|
<< m_joint_positionsSIN << m_imu_quaternionSIN |
129 |
|
|
<< m_forceLLEGSIN << m_forceRLEGSIN |
130 |
|
|
<< m_w_lf_inSIN << m_w_rf_inSIN |
131 |
|
|
<< m_K_fb_feet_posesSIN << m_w_lf_filteredSOUT |
132 |
|
|
<< m_w_rf_filteredSOUT << m_lf_ref_xyzquatSIN |
133 |
|
|
<< m_rf_ref_xyzquatSIN), |
134 |
|
|
CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector, |
135 |
|
|
m_kinematics_computationsSINNER |
136 |
|
|
<< m_accelerometerSIN << m_gyroscopeSIN |
137 |
|
|
<< m_qSOUT << m_dforceLLEGSIN |
138 |
|
|
<< m_dforceRLEGSIN), |
139 |
|
|
CONSTRUCT_SIGNAL_OUT(v_kin, dynamicgraph::Vector, m_vSOUT), |
140 |
|
|
CONSTRUCT_SIGNAL_OUT(v_flex, dynamicgraph::Vector, m_vSOUT), |
141 |
|
|
CONSTRUCT_SIGNAL_OUT(v_imu, dynamicgraph::Vector, m_vSOUT), |
142 |
|
|
CONSTRUCT_SIGNAL_OUT(v_gyr, dynamicgraph::Vector, m_vSOUT), |
143 |
|
|
CONSTRUCT_SIGNAL_OUT(lf_xyzquat, dynamicgraph::Vector, m_qSOUT), |
144 |
|
|
CONSTRUCT_SIGNAL_OUT(rf_xyzquat, dynamicgraph::Vector, m_qSOUT), |
145 |
|
|
CONSTRUCT_SIGNAL_OUT(a_ac, dynamicgraph::Vector, m_vSOUT), |
146 |
|
|
CONSTRUCT_SIGNAL_OUT(v_ac, dynamicgraph::Vector, m_vSOUT), |
147 |
|
|
CONSTRUCT_SIGNAL_OUT(q_lf, dynamicgraph::Vector, m_qSOUT), |
148 |
|
|
CONSTRUCT_SIGNAL_OUT(q_rf, dynamicgraph::Vector, m_qSOUT), |
149 |
|
|
CONSTRUCT_SIGNAL_OUT(q_imu, dynamicgraph::Vector, |
150 |
|
|
m_qSOUT << m_imu_quaternionSIN), |
151 |
|
|
CONSTRUCT_SIGNAL_OUT(w_lf, double, m_forceLLEGSIN), |
152 |
|
|
CONSTRUCT_SIGNAL_OUT(w_rf, double, m_forceRLEGSIN), |
153 |
|
|
CONSTRUCT_SIGNAL_OUT(w_lf_filtered, double, m_w_lfSOUT), |
154 |
|
|
CONSTRUCT_SIGNAL_OUT(w_rf_filtered, double, m_w_rfSOUT), |
155 |
|
|
m_initSucceeded(false), |
156 |
|
|
m_reset_foot_pos(true), |
157 |
|
|
m_w_imu(0.0), |
158 |
|
|
m_zmp_std_dev_rf(1.0), |
159 |
|
|
m_zmp_std_dev_lf(1.0), |
160 |
|
|
m_fz_std_dev_rf(1.0), |
161 |
|
|
m_fz_std_dev_lf(1.0), |
162 |
|
|
m_zmp_margin_lf(0.0), |
163 |
|
|
m_zmp_margin_rf(0.0) { |
164 |
|
|
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
165 |
|
|
|
166 |
|
|
m_K_rf << 4034, 23770, 239018, 707, 502, 936; |
167 |
|
|
m_K_lf << 4034, 23770, 239018, 707, 502, 936; |
168 |
|
|
m_left_foot_sizes << 0.130, -0.100, 0.075, -0.056; |
169 |
|
|
m_right_foot_sizes << 0.130, -0.100, 0.056, -0.075; |
170 |
|
|
|
171 |
|
|
/* Commands. */ |
172 |
|
|
addCommand("init", |
173 |
|
|
makeCommandVoid2( |
174 |
|
|
*this, &BaseEstimator::init, |
175 |
|
|
docCommandVoid2("Initialize the entity.", "time step (double)", |
176 |
|
|
"URDF file path (string)"))); |
177 |
|
|
|
178 |
|
|
addCommand("set_fz_stable_windows_size", |
179 |
|
|
makeCommandVoid1( |
180 |
|
|
*this, &BaseEstimator::set_fz_stable_windows_size, |
181 |
|
|
docCommandVoid1( |
182 |
|
|
"Set the windows size used to detect that feet is stable", |
183 |
|
|
"int"))); |
184 |
|
|
addCommand("set_alpha_w_filter", |
185 |
|
|
makeCommandVoid1( |
186 |
|
|
*this, &BaseEstimator::set_alpha_w_filter, |
187 |
|
|
docCommandVoid1("Set the filter parameter to filter weights", |
188 |
|
|
"double"))); |
189 |
|
|
addCommand( |
190 |
|
|
"set_alpha_DC_acc", |
191 |
|
|
makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_acc, |
192 |
|
|
docCommandVoid1("Set the filter parameter for removing " |
193 |
|
|
"DC from accelerometer data", |
194 |
|
|
"double"))); |
195 |
|
|
addCommand("set_alpha_DC_vel", |
196 |
|
|
makeCommandVoid1( |
197 |
|
|
*this, &BaseEstimator::set_alpha_DC_vel, |
198 |
|
|
docCommandVoid1("Set the filter parameter for removing DC " |
199 |
|
|
"from velocity integrated from acceleration", |
200 |
|
|
"double"))); |
201 |
|
|
addCommand( |
202 |
|
|
"reset_foot_positions", |
203 |
|
|
makeCommandVoid0(*this, &BaseEstimator::reset_foot_positions, |
204 |
|
|
docCommandVoid0("Reset the position of the feet."))); |
205 |
|
|
addCommand( |
206 |
|
|
"get_imu_weight", |
207 |
|
|
makeDirectGetter( |
208 |
|
|
*this, &m_w_imu, |
209 |
|
|
docDirectGetter("Weight of imu-based orientation in sensor fusion", |
210 |
|
|
"double"))); |
211 |
|
|
addCommand("set_imu_weight", |
212 |
|
|
makeCommandVoid1( |
213 |
|
|
*this, &BaseEstimator::set_imu_weight, |
214 |
|
|
docCommandVoid1( |
215 |
|
|
"Set the weight of imu-based orientation in sensor fusion", |
216 |
|
|
"double"))); |
217 |
|
|
addCommand( |
218 |
|
|
"set_zmp_std_dev_right_foot", |
219 |
|
|
makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_right_foot, |
220 |
|
|
docCommandVoid1("Set the standard deviation of the ZMP " |
221 |
|
|
"measurement errors for the right foot", |
222 |
|
|
"double"))); |
223 |
|
|
addCommand( |
224 |
|
|
"set_zmp_std_dev_left_foot", |
225 |
|
|
makeCommandVoid1(*this, &BaseEstimator::set_zmp_std_dev_left_foot, |
226 |
|
|
docCommandVoid1("Set the standard deviation of the ZMP " |
227 |
|
|
"measurement errors for the left foot", |
228 |
|
|
"double"))); |
229 |
|
|
addCommand("set_normal_force_std_dev_right_foot", |
230 |
|
|
makeCommandVoid1( |
231 |
|
|
*this, &BaseEstimator::set_normal_force_std_dev_right_foot, |
232 |
|
|
docCommandVoid1("Set the standard deviation of the normal " |
233 |
|
|
"force measurement errors for the right foot", |
234 |
|
|
"double"))); |
235 |
|
|
addCommand("set_normal_force_std_dev_left_foot", |
236 |
|
|
makeCommandVoid1( |
237 |
|
|
*this, &BaseEstimator::set_normal_force_std_dev_left_foot, |
238 |
|
|
docCommandVoid1("Set the standard deviation of the normal " |
239 |
|
|
"force measurement errors for the left foot", |
240 |
|
|
"double"))); |
241 |
|
|
addCommand("set_stiffness_right_foot", |
242 |
|
|
makeCommandVoid1( |
243 |
|
|
*this, &BaseEstimator::set_stiffness_right_foot, |
244 |
|
|
docCommandVoid1( |
245 |
|
|
"Set the 6d stiffness of the spring at the right foot", |
246 |
|
|
"vector"))); |
247 |
|
|
addCommand( |
248 |
|
|
"set_stiffness_left_foot", |
249 |
|
|
makeCommandVoid1( |
250 |
|
|
*this, &BaseEstimator::set_stiffness_left_foot, |
251 |
|
|
docCommandVoid1("Set the 6d stiffness of the spring at the left foot", |
252 |
|
|
"vector"))); |
253 |
|
|
addCommand( |
254 |
|
|
"set_right_foot_sizes", |
255 |
|
|
makeCommandVoid1( |
256 |
|
|
*this, &BaseEstimator::set_right_foot_sizes, |
257 |
|
|
docCommandVoid1( |
258 |
|
|
"Set the size of the right foot (pos x, neg x, pos y, neg y)", |
259 |
|
|
"4d vector"))); |
260 |
|
|
addCommand( |
261 |
|
|
"set_left_foot_sizes", |
262 |
|
|
makeCommandVoid1( |
263 |
|
|
*this, &BaseEstimator::set_left_foot_sizes, |
264 |
|
|
docCommandVoid1( |
265 |
|
|
"Set the size of the left foot (pos x, neg x, pos y, neg y)", |
266 |
|
|
"4d vector"))); |
267 |
|
|
addCommand( |
268 |
|
|
"set_zmp_margin_right_foot", |
269 |
|
|
makeCommandVoid1( |
270 |
|
|
*this, &BaseEstimator::set_zmp_margin_right_foot, |
271 |
|
|
docCommandVoid1("Set the ZMP margin for the right foot", "double"))); |
272 |
|
|
addCommand( |
273 |
|
|
"set_zmp_margin_left_foot", |
274 |
|
|
makeCommandVoid1( |
275 |
|
|
*this, &BaseEstimator::set_zmp_margin_left_foot, |
276 |
|
|
docCommandVoid1("Set the ZMP margin for the left foot", "double"))); |
277 |
|
|
addCommand( |
278 |
|
|
"set_normal_force_margin_right_foot", |
279 |
|
|
makeCommandVoid1( |
280 |
|
|
*this, &BaseEstimator::set_normal_force_margin_right_foot, |
281 |
|
|
docCommandVoid1("Set the normal force margin for the right foot", |
282 |
|
|
"double"))); |
283 |
|
|
addCommand( |
284 |
|
|
"set_normal_force_margin_left_foot", |
285 |
|
|
makeCommandVoid1( |
286 |
|
|
*this, &BaseEstimator::set_normal_force_margin_left_foot, |
287 |
|
|
docCommandVoid1("Set the normal force margin for the left foot", |
288 |
|
|
"double"))); |
289 |
|
|
} |
290 |
|
|
|
291 |
|
|
void BaseEstimator::init(const double& dt, const std::string& robotRef) { |
292 |
|
|
m_dt = dt; |
293 |
|
|
try { |
294 |
|
|
/* Retrieve m_robot_util informations */ |
295 |
|
|
std::string localName(robotRef); |
296 |
|
|
if (isNameInRobotUtil(localName)) { |
297 |
|
|
m_robot_util = getRobotUtil(localName); |
298 |
|
|
std::cerr << "m_robot_util:" << m_robot_util << std::endl; |
299 |
|
|
} else { |
300 |
|
|
SEND_MSG( |
301 |
|
|
"You should have an entity controller manager initialized before", |
302 |
|
|
MSG_TYPE_ERROR); |
303 |
|
|
return; |
304 |
|
|
} |
305 |
|
|
|
306 |
|
|
pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename, |
307 |
|
|
pinocchio::JointModelFreeFlyer(), m_model); |
308 |
|
|
|
309 |
|
|
assert( |
310 |
|
|
m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name)); |
311 |
|
|
assert( |
312 |
|
|
m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
313 |
|
|
assert( |
314 |
|
|
m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name)); |
315 |
|
|
m_left_foot_id = |
316 |
|
|
m_model.getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); |
317 |
|
|
m_right_foot_id = |
318 |
|
|
m_model.getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); |
319 |
|
|
m_IMU_body_id = m_model.getFrameId(m_robot_util->m_imu_joint_name); |
320 |
|
|
|
321 |
|
|
m_q_pin.setZero(m_model.nq); |
322 |
|
|
m_q_pin[6] = 1.; // for quaternion |
323 |
|
|
m_q_sot.setZero(m_robot_util->m_nbJoints + 6); |
324 |
|
|
m_v_pin.setZero(m_robot_util->m_nbJoints + 6); |
325 |
|
|
m_v_sot.setZero(m_robot_util->m_nbJoints + 6); |
326 |
|
|
m_v_kin.setZero(m_robot_util->m_nbJoints + 6); |
327 |
|
|
m_v_flex.setZero(m_robot_util->m_nbJoints + 6); |
328 |
|
|
m_v_imu.setZero(m_robot_util->m_nbJoints + 6); |
329 |
|
|
m_v_gyr.setZero(m_robot_util->m_nbJoints + 6); |
330 |
|
|
m_sole_M_ftSens = |
331 |
|
|
SE3(Matrix3::Identity(), |
332 |
|
|
-Eigen::Map<const Vector3>( |
333 |
|
|
&m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0))); |
334 |
|
|
|
335 |
|
|
m_last_vel.setZero(); |
336 |
|
|
m_last_DCvel.setZero(); |
337 |
|
|
m_last_DCacc |
338 |
|
|
.setZero(); // this is to replace by acceleration at 1st iteration |
339 |
|
|
|
340 |
|
|
m_alpha_DC_acc = 0.9995; |
341 |
|
|
m_alpha_DC_vel = 0.9995; |
342 |
|
|
m_alpha_w_filter = 1.0; |
343 |
|
|
m_left_foot_is_stable = true; |
344 |
|
|
m_right_foot_is_stable = true; |
345 |
|
|
m_fz_stable_windows_size = 10; |
346 |
|
|
m_lf_fz_stable_cpt = m_fz_stable_windows_size; |
347 |
|
|
m_rf_fz_stable_cpt = m_fz_stable_windows_size; |
348 |
|
|
m_isFirstIterFlag = true; |
349 |
|
|
} catch (const std::exception& e) { |
350 |
|
|
std::cout << e.what(); |
351 |
|
|
SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, |
352 |
|
|
MSG_TYPE_ERROR); |
353 |
|
|
return; |
354 |
|
|
} |
355 |
|
|
m_data = new pinocchio::Data(m_model); |
356 |
|
|
m_initSucceeded = true; |
357 |
|
|
} |
358 |
|
|
|
359 |
|
|
void BaseEstimator::set_fz_stable_windows_size(const int& ws) { |
360 |
|
|
if (ws < 0.0) |
361 |
|
|
return SEND_MSG("windows_size should be a positive integer", |
362 |
|
|
MSG_TYPE_ERROR); |
363 |
|
|
m_fz_stable_windows_size = ws; |
364 |
|
|
} |
365 |
|
|
|
366 |
|
|
void BaseEstimator::set_alpha_w_filter(const double& a) { |
367 |
|
|
if (a < 0.0 || a > 1.0) |
368 |
|
|
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR); |
369 |
|
|
m_alpha_w_filter = a; |
370 |
|
|
} |
371 |
|
|
|
372 |
|
|
void BaseEstimator::set_alpha_DC_acc(const double& a) { |
373 |
|
|
if (a < 0.0 || a > 1.0) |
374 |
|
|
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR); |
375 |
|
|
m_alpha_DC_acc = a; |
376 |
|
|
} |
377 |
|
|
|
378 |
|
|
void BaseEstimator::set_alpha_DC_vel(const double& a) { |
379 |
|
|
if (a < 0.0 || a > 1.0) |
380 |
|
|
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR); |
381 |
|
|
m_alpha_DC_vel = a; |
382 |
|
|
} |
383 |
|
|
|
384 |
|
|
void BaseEstimator::reset_foot_positions() { m_reset_foot_pos = true; } |
385 |
|
|
|
386 |
|
|
void BaseEstimator::set_imu_weight(const double& w) { |
387 |
|
|
if (w < 0.0) |
388 |
|
|
return SEND_MSG("Imu weight must be nonnegative", MSG_TYPE_ERROR); |
389 |
|
|
m_w_imu = w; |
390 |
|
|
} |
391 |
|
|
|
392 |
|
|
void BaseEstimator::set_stiffness_right_foot(const dynamicgraph::Vector& k) { |
393 |
|
|
if (k.size() != 6) |
394 |
|
|
return SEND_MSG( |
395 |
|
|
"Stiffness vector should have size 6, not " + toString(k.size()), |
396 |
|
|
MSG_TYPE_ERROR); |
397 |
|
|
m_K_rf = k; |
398 |
|
|
} |
399 |
|
|
|
400 |
|
|
void BaseEstimator::set_stiffness_left_foot(const dynamicgraph::Vector& k) { |
401 |
|
|
if (k.size() != 6) |
402 |
|
|
return SEND_MSG( |
403 |
|
|
"Stiffness vector should have size 6, not " + toString(k.size()), |
404 |
|
|
MSG_TYPE_ERROR); |
405 |
|
|
m_K_lf = k; |
406 |
|
|
} |
407 |
|
|
|
408 |
|
|
void BaseEstimator::set_zmp_std_dev_right_foot(const double& std_dev) { |
409 |
|
|
if (std_dev <= 0.0) |
410 |
|
|
return SEND_MSG("Standard deviation must be a positive number", |
411 |
|
|
MSG_TYPE_ERROR); |
412 |
|
|
m_zmp_std_dev_rf = std_dev; |
413 |
|
|
} |
414 |
|
|
|
415 |
|
|
void BaseEstimator::set_zmp_std_dev_left_foot(const double& std_dev) { |
416 |
|
|
if (std_dev <= 0.0) |
417 |
|
|
return SEND_MSG("Standard deviation must be a positive number", |
418 |
|
|
MSG_TYPE_ERROR); |
419 |
|
|
m_zmp_std_dev_lf = std_dev; |
420 |
|
|
} |
421 |
|
|
|
422 |
|
|
void BaseEstimator::set_normal_force_std_dev_right_foot(const double& std_dev) { |
423 |
|
|
if (std_dev <= 0.0) |
424 |
|
|
return SEND_MSG("Standard deviation must be a positive number", |
425 |
|
|
MSG_TYPE_ERROR); |
426 |
|
|
m_fz_std_dev_rf = std_dev; |
427 |
|
|
} |
428 |
|
|
|
429 |
|
|
void BaseEstimator::set_normal_force_std_dev_left_foot(const double& std_dev) { |
430 |
|
|
if (std_dev <= 0.0) |
431 |
|
|
return SEND_MSG("Standard deviation must be a positive number", |
432 |
|
|
MSG_TYPE_ERROR); |
433 |
|
|
m_fz_std_dev_lf = std_dev; |
434 |
|
|
} |
435 |
|
|
|
436 |
|
|
void BaseEstimator::set_right_foot_sizes(const dynamicgraph::Vector& s) { |
437 |
|
|
if (s.size() != 4) |
438 |
|
|
return SEND_MSG( |
439 |
|
|
"Foot size vector should have size 4, not " + toString(s.size()), |
440 |
|
|
MSG_TYPE_ERROR); |
441 |
|
|
m_right_foot_sizes = s; |
442 |
|
|
} |
443 |
|
|
|
444 |
|
|
void BaseEstimator::set_left_foot_sizes(const dynamicgraph::Vector& s) { |
445 |
|
|
if (s.size() != 4) |
446 |
|
|
return SEND_MSG( |
447 |
|
|
"Foot size vector should have size 4, not " + toString(s.size()), |
448 |
|
|
MSG_TYPE_ERROR); |
449 |
|
|
m_left_foot_sizes = s; |
450 |
|
|
} |
451 |
|
|
|
452 |
|
|
void BaseEstimator::set_zmp_margin_right_foot(const double& margin) { |
453 |
|
|
m_zmp_margin_rf = margin; |
454 |
|
|
} |
455 |
|
|
|
456 |
|
|
void BaseEstimator::set_zmp_margin_left_foot(const double& margin) { |
457 |
|
|
m_zmp_margin_lf = margin; |
458 |
|
|
} |
459 |
|
|
|
460 |
|
|
void BaseEstimator::set_normal_force_margin_right_foot(const double& margin) { |
461 |
|
|
m_fz_margin_rf = margin; |
462 |
|
|
} |
463 |
|
|
|
464 |
|
|
void BaseEstimator::set_normal_force_margin_left_foot(const double& margin) { |
465 |
|
|
m_fz_margin_lf = margin; |
466 |
|
|
} |
467 |
|
|
|
468 |
|
|
void BaseEstimator::compute_zmp(const Vector6& w, Vector2& zmp) { |
469 |
|
|
pinocchio::Force f(w); |
470 |
|
|
f = m_sole_M_ftSens.act(f); |
471 |
|
|
if (f.linear()[2] == 0.0) return; |
472 |
|
|
zmp[0] = -f.angular()[1] / f.linear()[2]; |
473 |
|
|
zmp[1] = f.angular()[0] / f.linear()[2]; |
474 |
|
|
} |
475 |
|
|
|
476 |
|
|
double BaseEstimator::compute_zmp_weight(const Vector2& zmp, |
477 |
|
|
const Vector4& foot_sizes, |
478 |
|
|
double std_dev, double margin) { |
479 |
|
|
double fs0 = foot_sizes[0] - margin; |
480 |
|
|
double fs1 = foot_sizes[1] + margin; |
481 |
|
|
double fs2 = foot_sizes[2] - margin; |
482 |
|
|
double fs3 = foot_sizes[3] + margin; |
483 |
|
|
|
484 |
|
|
if (zmp[0] > fs0 || zmp[0] < fs1 || zmp[1] > fs2 || zmp[1] < fs3) return 0; |
485 |
|
|
|
486 |
|
|
double cdx = ((cdf(m_normal, (fs0 - zmp[0]) / std_dev) - |
487 |
|
|
cdf(m_normal, (fs1 - zmp[0]) / std_dev)) - |
488 |
|
|
0.5) * |
489 |
|
|
2.0; |
490 |
|
|
double cdy = ((cdf(m_normal, (fs2 - zmp[1]) / std_dev) - |
491 |
|
|
cdf(m_normal, (fs3 - zmp[1]) / std_dev)) - |
492 |
|
|
0.5) * |
493 |
|
|
2.0; |
494 |
|
|
return cdx * cdy; |
495 |
|
|
} |
496 |
|
|
|
497 |
|
|
double BaseEstimator::compute_force_weight(double fz, double std_dev, |
498 |
|
|
double margin) { |
499 |
|
|
if (fz < margin) return 0.0; |
500 |
|
|
return (cdf(m_normal, (fz - margin) / std_dev) - 0.5) * 2.0; |
501 |
|
|
} |
502 |
|
|
|
503 |
|
|
void BaseEstimator::reset_foot_positions_impl(const Vector6& ftlf, |
504 |
|
|
const Vector6& ftrf) { |
505 |
|
|
// compute the base position wrt each foot |
506 |
|
|
SE3 dummy, dummy1, lfMff, rfMff; |
507 |
|
|
m_oMrfs = SE3::Identity(); |
508 |
|
|
m_oMlfs = SE3::Identity(); |
509 |
|
|
kinematics_estimation(ftrf, m_K_rf, m_oMrfs, |
510 |
|
|
static_cast<int>(m_right_foot_id), rfMff, dummy, |
511 |
|
|
dummy1); // rfMff is obtain reading oMff becaused oMrfs |
512 |
|
|
// is here set to Identity |
513 |
|
|
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, static_cast<int>(m_left_foot_id), |
514 |
|
|
lfMff, dummy, dummy1); |
515 |
|
|
|
516 |
|
|
// distance from ankle to ground |
517 |
|
|
const Vector3& ankle_2_sole_xyz = |
518 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ; |
519 |
|
|
const SE3 groundMfoot(Matrix3::Identity(), -1.0 * ankle_2_sole_xyz); |
520 |
|
|
lfMff = groundMfoot * lfMff; |
521 |
|
|
rfMff = groundMfoot * rfMff; |
522 |
|
|
|
523 |
|
|
// set the world frame in between the feet |
524 |
|
|
const Vector3 w(0.5 * (pinocchio::log3(lfMff.rotation()) + |
525 |
|
|
pinocchio::log3(rfMff.rotation()))); |
526 |
|
|
SE3 oMff = SE3(pinocchio::exp3(w), |
527 |
|
|
0.5 * (lfMff.translation() + rfMff.translation())); |
528 |
|
|
// add a constant offset to make the world frame match the one in OpenHRP |
529 |
|
|
oMff.translation()(0) += 9.562e-03; |
530 |
|
|
|
531 |
|
|
m_oMlfs = oMff * lfMff.inverse() * groundMfoot; |
532 |
|
|
m_oMrfs = oMff * rfMff.inverse() * groundMfoot; |
533 |
|
|
|
534 |
|
|
m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation(); |
535 |
|
|
Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation()); |
536 |
|
|
m_oMlfs_xyzquat(3) = quat_lf.w(); |
537 |
|
|
m_oMlfs_xyzquat(4) = quat_lf.x(); |
538 |
|
|
m_oMlfs_xyzquat(5) = quat_lf.y(); |
539 |
|
|
m_oMlfs_xyzquat(6) = quat_lf.z(); |
540 |
|
|
|
541 |
|
|
m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation(); |
542 |
|
|
Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation()); |
543 |
|
|
m_oMrfs_xyzquat(3) = quat_rf.w(); |
544 |
|
|
m_oMrfs_xyzquat(4) = quat_rf.x(); |
545 |
|
|
m_oMrfs_xyzquat(5) = quat_rf.y(); |
546 |
|
|
m_oMrfs_xyzquat(6) = quat_rf.z(); |
547 |
|
|
|
548 |
|
|
// save this poses to use it if no ref is provided |
549 |
|
|
m_oMlfs_default_ref = m_oMlfs; |
550 |
|
|
m_oMrfs_default_ref = m_oMrfs; |
551 |
|
|
|
552 |
|
|
sendMsg("Reference pos of left foot:\n" + toString(m_oMlfs), MSG_TYPE_INFO); |
553 |
|
|
sendMsg("Reference pos of right foot:\n" + toString(m_oMrfs), MSG_TYPE_INFO); |
554 |
|
|
|
555 |
|
|
// kinematics_estimation(ftrf, m_K_rf, m_oMrfs, m_right_foot_id, |
556 |
|
|
// m_oMff_rf, dummy); kinematics_estimation(ftlf, m_K_lf, m_oMlfs, |
557 |
|
|
// m_left_foot_id, m_oMff_lf, dummy); sendMsg("Base estimation left |
558 |
|
|
// foot:\n"+toString(m_oMff_lf), MSG_TYPE_DEBUG); sendMsg("Base |
559 |
|
|
// estimation right foot:\n"+toString(m_oMff_rf), MSG_TYPE_DEBUG); |
560 |
|
|
// sendMsg("Difference base estimation left-right |
561 |
|
|
// foot:\n"+toString(m_oMff_rf.inverse()*m_oMff_lf), MSG_TYPE_DEBUG); |
562 |
|
|
|
563 |
|
|
m_reset_foot_pos = false; |
564 |
|
|
} |
565 |
|
|
|
566 |
|
|
void BaseEstimator::kinematics_estimation(const Vector6& ft, const Vector6& K, |
567 |
|
|
const SE3& oMfs, const int foot_id, |
568 |
|
|
SE3& oMff, SE3& oMfa, SE3& fsMff) { |
569 |
|
|
Vector3 xyz; |
570 |
|
|
xyz << -ft[0] / K(0), -ft[1] / K(1), -ft[2] / K(2); |
571 |
|
|
Matrix3 R; |
572 |
|
|
rpyToMatrix(-ft[3] / K(3), -ft[4] / K(4), -ft[5] / K(5), R); |
573 |
|
|
const SE3 fsMfa(R, xyz); // foot sole to foot ankle |
574 |
|
|
oMfa = oMfs * fsMfa; // world to foot ankle |
575 |
|
|
const SE3 faMff(m_data->oMf[foot_id].inverse()); // foot ankle to free flyer |
576 |
|
|
//~ sendMsg("faMff (foot_id="+toString(foot_id)+"):\n" + toString(faMff), |
577 |
|
|
// MSG_TYPE_INFO); ~ sendMsg("fsMfa (foot_id="+toString(foot_id)+"):\n" + |
578 |
|
|
// toString(fsMfa), MSG_TYPE_INFO); |
579 |
|
|
oMff = oMfa * faMff; // world to free flyer |
580 |
|
|
fsMff = fsMfa * faMff; // foot sole to free flyer |
581 |
|
|
} |
582 |
|
|
|
583 |
|
|
/* ------------------------------------------------------------------- */ |
584 |
|
|
/* --- SIGNALS ------------------------------------------------------- */ |
585 |
|
|
/* ------------------------------------------------------------------- */ |
586 |
|
|
|
587 |
|
|
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector) { |
588 |
|
|
if (!m_initSucceeded) { |
589 |
|
|
SEND_WARNING_STREAM_MSG( |
590 |
|
|
"Cannot compute signal kinematics_computations before initialization!"); |
591 |
|
|
return s; |
592 |
|
|
} |
593 |
|
|
|
594 |
|
|
const Eigen::VectorXd& qj = m_joint_positionsSIN(iter); // n+6 |
595 |
|
|
const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter); |
596 |
|
|
assert(qj.size() == |
597 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && |
598 |
|
|
"Unexpected size of signal joint_positions"); |
599 |
|
|
assert(dq.size() == |
600 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && |
601 |
|
|
"Unexpected size of signal joint_velocities"); |
602 |
|
|
|
603 |
|
|
/* convert sot to pinocchio joint order */ |
604 |
|
|
m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints)); |
605 |
|
|
m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints)); |
606 |
|
|
|
607 |
|
|
getProfiler().start(PROFILE_BASE_KINEMATICS_COMPUTATION); |
608 |
|
|
|
609 |
|
|
/* Compute kinematics assuming world is at free-flyer frame */ |
610 |
|
|
m_q_pin.head<6>().setZero(); |
611 |
|
|
m_q_pin(6) = 1.0; |
612 |
|
|
m_v_pin.head<6>().setZero(); |
613 |
|
|
pinocchio::forwardKinematics(m_model, *m_data, m_q_pin, m_v_pin); |
614 |
|
|
pinocchio::framesForwardKinematics(m_model, *m_data); |
615 |
|
|
|
616 |
|
|
getProfiler().stop(PROFILE_BASE_KINEMATICS_COMPUTATION); |
617 |
|
|
|
618 |
|
|
return s; |
619 |
|
|
} |
620 |
|
|
|
621 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(q, dynamicgraph::Vector) { |
622 |
|
|
if (!m_initSucceeded) { |
623 |
|
|
SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!"); |
624 |
|
|
return s; |
625 |
|
|
} |
626 |
|
|
if (s.size() != |
627 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) |
628 |
|
|
s.resize(m_robot_util->m_nbJoints + 6); |
629 |
|
|
|
630 |
|
|
const Eigen::VectorXd& qj = m_joint_positionsSIN(iter); // n+6 |
631 |
|
|
const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter); |
632 |
|
|
const Vector6& ftrf = m_forceRLEGSIN(iter); |
633 |
|
|
const Vector6& ftlf = m_forceLLEGSIN(iter); |
634 |
|
|
|
635 |
|
|
// if the weights are not specified by the user through the input signals |
636 |
|
|
// w_lf, w_rf then compute them if one feet is not stable, force weight to 0.0 |
637 |
|
|
double wL, wR; |
638 |
|
|
if (m_w_lf_inSIN.isPlugged()) |
639 |
|
|
wL = m_w_lf_inSIN(iter); |
640 |
|
|
else { |
641 |
|
|
wL = m_w_lf_filteredSOUT(iter); |
642 |
|
|
if (m_left_foot_is_stable == false) wL = 0.0; |
643 |
|
|
} |
644 |
|
|
if (m_w_rf_inSIN.isPlugged()) |
645 |
|
|
wR = m_w_rf_inSIN(iter); |
646 |
|
|
else { |
647 |
|
|
wR = m_w_rf_filteredSOUT(iter); |
648 |
|
|
if (m_right_foot_is_stable == false) wR = 0.0; |
649 |
|
|
} |
650 |
|
|
|
651 |
|
|
assert(qj.size() == |
652 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && |
653 |
|
|
"Unexpected size of signal joint_positions"); |
654 |
|
|
|
655 |
|
|
// if both weights are zero set them to a small positive value to avoid |
656 |
|
|
// division by zero |
657 |
|
|
if (wR == 0.0 && wL == 0.0) { |
658 |
|
|
SEND_WARNING_STREAM_MSG( |
659 |
|
|
"The robot is flying!" + |
660 |
|
|
("- forceRLEG: " + toString(ftrf.transpose())) + |
661 |
|
|
"- forceLLEG: " + toString(ftlf.transpose()) + |
662 |
|
|
"- m_right_foot_is_stable: " + toString(m_right_foot_is_stable) + |
663 |
|
|
"- m_left_foot_is_stable: " + toString(m_left_foot_is_stable)); |
664 |
|
|
wR = 1e-3; |
665 |
|
|
wL = 1e-3; |
666 |
|
|
} |
667 |
|
|
|
668 |
|
|
m_kinematics_computationsSINNER(iter); |
669 |
|
|
|
670 |
|
|
if (m_reset_foot_pos) reset_foot_positions_impl(ftlf, ftrf); |
671 |
|
|
|
672 |
|
|
getProfiler().start(PROFILE_BASE_POSITION_ESTIMATION); |
673 |
|
|
{ |
674 |
|
|
SE3 oMlfa, oMrfa, lfsMff, rfsMff; |
675 |
|
|
kinematics_estimation(ftrf, m_K_rf, m_oMrfs, |
676 |
|
|
static_cast<int>(m_right_foot_id), m_oMff_rf, oMrfa, |
677 |
|
|
rfsMff); |
678 |
|
|
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, |
679 |
|
|
static_cast<int>(m_left_foot_id), m_oMff_lf, oMlfa, |
680 |
|
|
lfsMff); |
681 |
|
|
|
682 |
|
|
// get rpy |
683 |
|
|
const SE3 ffMchest( |
684 |
|
|
m_data->oMf[m_IMU_body_id]); // transform between freeflyer and body |
685 |
|
|
// attached to IMU sensor |
686 |
|
|
const SE3 chestMff(ffMchest.inverse()); // transform between body attached |
687 |
|
|
// to IMU sensor and freeflyer |
688 |
|
|
|
689 |
|
|
Vector3 rpy_chest, rpy_chest_lf, rpy_chest_rf, |
690 |
|
|
rpy_chest_imu; // orientation of the body which imu is attached to. |
691 |
|
|
// (fusion, from left kine, from right kine, from imu) |
692 |
|
|
|
693 |
|
|
matrixToRpy((m_oMff_lf * ffMchest).rotation(), rpy_chest_lf); |
694 |
|
|
matrixToRpy((m_oMff_rf * ffMchest).rotation(), rpy_chest_rf); |
695 |
|
|
Eigen::Quaternion<double> quatIMU(quatIMU_vec[0], quatIMU_vec[1], |
696 |
|
|
quatIMU_vec[2], quatIMU_vec[3]); |
697 |
|
|
matrixToRpy(quatIMU.toRotationMatrix(), rpy_chest_imu); |
698 |
|
|
|
699 |
|
|
// average (we do not take into account the IMU yaw) |
700 |
|
|
double wSum = wL + wR + m_w_imu; |
701 |
|
|
rpy_chest(0) = (rpy_chest_lf[0] * wL + rpy_chest_rf[0] * wR + |
702 |
|
|
rpy_chest_imu[0] * m_w_imu) / |
703 |
|
|
wSum; |
704 |
|
|
rpy_chest(1) = (rpy_chest_lf[1] * wL + rpy_chest_rf[1] * wR + |
705 |
|
|
rpy_chest_imu[1] * m_w_imu) / |
706 |
|
|
wSum; |
707 |
|
|
rpy_chest(2) = (rpy_chest_lf[2] * wL + rpy_chest_rf[2] * wR) / (wL + wR); |
708 |
|
|
|
709 |
|
|
rpyToMatrix(rpy_chest, m_oRchest); |
710 |
|
|
m_oRff = m_oRchest * chestMff.rotation(); |
711 |
|
|
|
712 |
|
|
// translation to get foot at the right position |
713 |
|
|
// evaluate Mrl Mlf for q with the good orientation and zero translation for |
714 |
|
|
// freeflyer |
715 |
|
|
const Vector3 pos_lf_ff = |
716 |
|
|
m_oRff * m_data->oMf[m_left_foot_id].translation(); |
717 |
|
|
const Vector3 pos_rf_ff = |
718 |
|
|
m_oRff * m_data->oMf[m_right_foot_id].translation(); |
719 |
|
|
// get average translation |
720 |
|
|
m_q_pin.head<3>() = ((oMlfa.translation() - pos_lf_ff) * wL + |
721 |
|
|
(oMrfa.translation() - pos_rf_ff) * wR) / |
722 |
|
|
(wL + wR); |
723 |
|
|
|
724 |
|
|
m_q_sot.tail(m_robot_util->m_nbJoints) = qj; |
725 |
|
|
base_se3_to_sot(m_q_pin.head<3>(), m_oRff, m_q_sot.head<6>()); |
726 |
|
|
|
727 |
|
|
s = m_q_sot; |
728 |
|
|
|
729 |
|
|
// store estimation of the base pose in SE3 format |
730 |
|
|
const SE3 oMff_est(m_oRff, m_q_pin.head<3>()); |
731 |
|
|
|
732 |
|
|
// feedback on feet poses |
733 |
|
|
if (m_K_fb_feet_posesSIN.isPlugged()) { |
734 |
|
|
const double K_fb = m_K_fb_feet_posesSIN(iter); |
735 |
|
|
if (K_fb > 0.0 && m_w_imu > 0.0) { |
736 |
|
|
assert(m_w_imu > 0.0 && |
737 |
|
|
"Update of the feet 6d poses should not be done if wIMU = 0.0"); |
738 |
|
|
assert(K_fb < 1.0 && |
739 |
|
|
"Feedback gain on foot correction should be less than 1.0 " |
740 |
|
|
"(K_fb_feet_poses>1.0)"); |
741 |
|
|
// feet positions in the world according to current base estimation |
742 |
|
|
const SE3 oMlfs_est(oMff_est * (lfsMff.inverse())); |
743 |
|
|
const SE3 oMrfs_est(oMff_est * (rfsMff.inverse())); |
744 |
|
|
// error in current foot position |
745 |
|
|
SE3 leftDrift = m_oMlfs.inverse() * oMlfs_est; |
746 |
|
|
SE3 rightDrift = m_oMrfs.inverse() * oMrfs_est; |
747 |
|
|
|
748 |
|
|
/// apply feedback correction |
749 |
|
|
SE3 leftDrift_delta; |
750 |
|
|
SE3 rightDrift_delta; |
751 |
|
|
se3Interp(leftDrift, SE3::Identity(), K_fb * wR, leftDrift_delta); |
752 |
|
|
se3Interp(rightDrift, SE3::Identity(), K_fb * wL, rightDrift_delta); |
753 |
|
|
// if a feet is not stable on the ground (aka flying), fully update his |
754 |
|
|
// position |
755 |
|
|
if (m_right_foot_is_stable == false) rightDrift_delta = rightDrift; |
756 |
|
|
if (m_left_foot_is_stable == false) leftDrift_delta = leftDrift; |
757 |
|
|
if (m_right_foot_is_stable == false && m_left_foot_is_stable == false) { |
758 |
|
|
// robot is jumping, do not update any feet position |
759 |
|
|
rightDrift_delta = SE3::Identity(); |
760 |
|
|
leftDrift_delta = SE3::Identity(); |
761 |
|
|
} |
762 |
|
|
m_oMlfs = m_oMlfs * leftDrift_delta; |
763 |
|
|
m_oMrfs = m_oMrfs * rightDrift_delta; |
764 |
|
|
// dedrift (x, y, z, yaw) using feet pose references |
765 |
|
|
SE3 oMlfs_ref, oMrfs_ref; |
766 |
|
|
if (m_lf_ref_xyzquatSIN.isPlugged() and |
767 |
|
|
m_rf_ref_xyzquatSIN.isPlugged()) { |
768 |
|
|
/// convert from xyzquat to se3 |
769 |
|
|
const Vector7& lf_ref_xyzquat_vec = m_lf_ref_xyzquatSIN(iter); |
770 |
|
|
const Vector7& rf_ref_xyzquat_vec = m_rf_ref_xyzquatSIN(iter); |
771 |
|
|
const Eigen::Quaterniond ql( |
772 |
|
|
m_lf_ref_xyzquatSIN(iter)(3), m_lf_ref_xyzquatSIN(iter)(4), |
773 |
|
|
m_lf_ref_xyzquatSIN(iter)(5), m_lf_ref_xyzquatSIN(iter)(6)); |
774 |
|
|
const Eigen::Quaterniond qr( |
775 |
|
|
m_rf_ref_xyzquatSIN(iter)(3), m_rf_ref_xyzquatSIN(iter)(4), |
776 |
|
|
m_rf_ref_xyzquatSIN(iter)(5), m_rf_ref_xyzquatSIN(iter)(6)); |
777 |
|
|
oMlfs_ref = SE3(ql.toRotationMatrix(), lf_ref_xyzquat_vec.head<3>()); |
778 |
|
|
oMrfs_ref = SE3(qr.toRotationMatrix(), rf_ref_xyzquat_vec.head<3>()); |
779 |
|
|
} else { |
780 |
|
|
oMlfs_ref = m_oMlfs_default_ref; |
781 |
|
|
oMrfs_ref = m_oMrfs_default_ref; |
782 |
|
|
} |
783 |
|
|
/// find translation to apply to both feet to minimise distances to |
784 |
|
|
/// reference positions |
785 |
|
|
const Vector3 translation_feet_drift = |
786 |
|
|
0.5 * ((oMlfs_ref.translation() - m_oMlfs.translation()) + |
787 |
|
|
(oMrfs_ref.translation() - m_oMrfs.translation())); |
788 |
|
|
/// two vectors define by left to right feet translation |
789 |
|
|
const Vector3 V_ref = oMrfs_ref.translation() - oMlfs_ref.translation(); |
790 |
|
|
const Vector3 V_est = m_oMrfs.translation() - m_oMlfs.translation(); |
791 |
|
|
/// angle betwin this two vectors projected in horizontal plane is the |
792 |
|
|
/// yaw drift |
793 |
|
|
const double yaw_drift = |
794 |
|
|
(atan2(V_ref(1), V_ref(0)) - atan2(V_est(1), V_est(0))); |
795 |
|
|
//~ printf("yaw_drift=%lf\r\n",yaw_drift); |
796 |
|
|
/// apply correction to cancel this drift |
797 |
|
|
const Vector3 rpy_feet_drift(0., 0., yaw_drift); |
798 |
|
|
Matrix3 rotation_feet_drift; |
799 |
|
|
rpyToMatrix(rpy_feet_drift, rotation_feet_drift); |
800 |
|
|
const SE3 drift_to_ref(rotation_feet_drift, translation_feet_drift); |
801 |
|
|
m_oMlfs = m_oMlfs * drift_to_ref; |
802 |
|
|
m_oMrfs = m_oMrfs * drift_to_ref; |
803 |
|
|
} |
804 |
|
|
} |
805 |
|
|
// convert to xyz+quaternion format //Rq: this convertions could be done in |
806 |
|
|
// outupt signals function? |
807 |
|
|
m_oMlfs_xyzquat.head<3>() = m_oMlfs.translation(); |
808 |
|
|
Eigen::Quaternion<double> quat_lf(m_oMlfs.rotation()); |
809 |
|
|
m_oMlfs_xyzquat(3) = quat_lf.w(); |
810 |
|
|
m_oMlfs_xyzquat(4) = quat_lf.x(); |
811 |
|
|
m_oMlfs_xyzquat(5) = quat_lf.y(); |
812 |
|
|
m_oMlfs_xyzquat(6) = quat_lf.z(); |
813 |
|
|
|
814 |
|
|
m_oMrfs_xyzquat.head<3>() = m_oMrfs.translation(); |
815 |
|
|
Eigen::Quaternion<double> quat_rf(m_oMrfs.rotation()); |
816 |
|
|
m_oMrfs_xyzquat(3) = quat_rf.w(); |
817 |
|
|
m_oMrfs_xyzquat(4) = quat_rf.x(); |
818 |
|
|
m_oMrfs_xyzquat(5) = quat_rf.y(); |
819 |
|
|
m_oMrfs_xyzquat(6) = quat_rf.z(); |
820 |
|
|
} |
821 |
|
|
getProfiler().stop(PROFILE_BASE_POSITION_ESTIMATION); |
822 |
|
|
return s; |
823 |
|
|
} |
824 |
|
|
|
825 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(lf_xyzquat, dynamicgraph::Vector) { |
826 |
|
|
if (!m_initSucceeded) { |
827 |
|
|
SEND_WARNING_STREAM_MSG( |
828 |
|
|
"Cannot compute signal lf_xyzquat before initialization! iter" + |
829 |
|
|
toString(iter)); |
830 |
|
|
return s; |
831 |
|
|
} |
832 |
|
|
if (s.size() != 7) s.resize(7); |
833 |
|
|
s = m_oMlfs_xyzquat; |
834 |
|
|
return s; |
835 |
|
|
} |
836 |
|
|
|
837 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(rf_xyzquat, dynamicgraph::Vector) { |
838 |
|
|
if (!m_initSucceeded) { |
839 |
|
|
SEND_WARNING_STREAM_MSG( |
840 |
|
|
"Cannot compute signal rf_xyzquat before initialization! iter" + |
841 |
|
|
toString(iter)); |
842 |
|
|
return s; |
843 |
|
|
} |
844 |
|
|
if (s.size() != 7) s.resize(7); |
845 |
|
|
s = m_oMrfs_xyzquat; |
846 |
|
|
return s; |
847 |
|
|
} |
848 |
|
|
|
849 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(q_lf, dynamicgraph::Vector) { |
850 |
|
|
if (!m_initSucceeded) { |
851 |
|
|
SEND_WARNING_STREAM_MSG( |
852 |
|
|
"Cannot compute signal q_lf before initialization!"); |
853 |
|
|
return s; |
854 |
|
|
} |
855 |
|
|
if (s.size() != |
856 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) |
857 |
|
|
s.resize(m_robot_util->m_nbJoints + 6); |
858 |
|
|
|
859 |
|
|
const Eigen::VectorXd& q = m_qSOUT(iter); |
860 |
|
|
s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints); |
861 |
|
|
base_se3_to_sot(m_oMff_lf.translation(), m_oMff_lf.rotation(), s.head<6>()); |
862 |
|
|
|
863 |
|
|
return s; |
864 |
|
|
} |
865 |
|
|
|
866 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(q_rf, dynamicgraph::Vector) { |
867 |
|
|
if (!m_initSucceeded) { |
868 |
|
|
SEND_WARNING_STREAM_MSG( |
869 |
|
|
"Cannot compute signal q_rf before initialization!"); |
870 |
|
|
return s; |
871 |
|
|
} |
872 |
|
|
if (s.size() != |
873 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) |
874 |
|
|
s.resize(m_robot_util->m_nbJoints + 6); |
875 |
|
|
|
876 |
|
|
const Eigen::VectorXd& q = m_qSOUT(iter); |
877 |
|
|
s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints); |
878 |
|
|
base_se3_to_sot(m_oMff_rf.translation(), m_oMff_rf.rotation(), s.head<6>()); |
879 |
|
|
|
880 |
|
|
return s; |
881 |
|
|
} |
882 |
|
|
|
883 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(q_imu, dynamicgraph::Vector) { |
884 |
|
|
if (!m_initSucceeded) { |
885 |
|
|
SEND_WARNING_STREAM_MSG( |
886 |
|
|
"Cannot compute signal q_imu before initialization!"); |
887 |
|
|
return s; |
888 |
|
|
} |
889 |
|
|
if (s.size() != |
890 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) |
891 |
|
|
s.resize(m_robot_util->m_nbJoints + 6); |
892 |
|
|
|
893 |
|
|
const Eigen::VectorXd& q = m_qSOUT(iter); |
894 |
|
|
s.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints); |
895 |
|
|
|
896 |
|
|
const Eigen::Vector4d& quatIMU_vec = m_imu_quaternionSIN(iter); |
897 |
|
|
Eigen::Quaternion<double> quatIMU(quatIMU_vec); |
898 |
|
|
base_se3_to_sot(q.head<3>(), quatIMU.toRotationMatrix(), s.head<6>()); |
899 |
|
|
|
900 |
|
|
return s; |
901 |
|
|
} |
902 |
|
|
|
903 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(w_lf, double) { |
904 |
|
|
if (!m_initSucceeded) { |
905 |
|
|
SEND_WARNING_STREAM_MSG( |
906 |
|
|
"Cannot compute signal w_lf before initialization!"); |
907 |
|
|
return s; |
908 |
|
|
} |
909 |
|
|
|
910 |
|
|
const Vector6& wrench = m_forceLLEGSIN(iter); |
911 |
|
|
Vector2 zmp; |
912 |
|
|
zmp.setZero(); |
913 |
|
|
compute_zmp(wrench, zmp); |
914 |
|
|
double w_zmp = compute_zmp_weight(zmp, m_left_foot_sizes, m_zmp_std_dev_lf, |
915 |
|
|
m_zmp_margin_lf); |
916 |
|
|
double w_fz = |
917 |
|
|
compute_force_weight(wrench(2), m_fz_std_dev_lf, m_fz_margin_lf); |
918 |
|
|
// check that foot is sensing a force greater than the margin treshold for |
919 |
|
|
// more than 'm_fz_stable_windows_size' samples |
920 |
|
|
if (wrench(2) > m_fz_margin_lf) |
921 |
|
|
m_lf_fz_stable_cpt++; |
922 |
|
|
else |
923 |
|
|
m_lf_fz_stable_cpt = 0; |
924 |
|
|
|
925 |
|
|
if (m_lf_fz_stable_cpt >= m_fz_stable_windows_size) { |
926 |
|
|
m_lf_fz_stable_cpt = m_fz_stable_windows_size; |
927 |
|
|
m_left_foot_is_stable = true; |
928 |
|
|
} else { |
929 |
|
|
m_left_foot_is_stable = false; |
930 |
|
|
} |
931 |
|
|
s = w_zmp * w_fz; |
932 |
|
|
return s; |
933 |
|
|
} |
934 |
|
|
|
935 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(w_rf, double) { |
936 |
|
|
if (!m_initSucceeded) { |
937 |
|
|
SEND_WARNING_STREAM_MSG( |
938 |
|
|
"Cannot compute signal w_rf before initialization!"); |
939 |
|
|
return s; |
940 |
|
|
} |
941 |
|
|
|
942 |
|
|
const Vector6& wrench = m_forceRLEGSIN(iter); |
943 |
|
|
Vector2 zmp; |
944 |
|
|
zmp.setZero(); |
945 |
|
|
compute_zmp(wrench, zmp); |
946 |
|
|
double w_zmp = compute_zmp_weight(zmp, m_right_foot_sizes, m_zmp_std_dev_rf, |
947 |
|
|
m_zmp_margin_rf); |
948 |
|
|
double w_fz = |
949 |
|
|
compute_force_weight(wrench(2), m_fz_std_dev_rf, m_fz_margin_rf); |
950 |
|
|
// check that foot is sensing a force greater than the margin treshold for |
951 |
|
|
// more than 'm_fz_stable_windows_size' samples |
952 |
|
|
if (wrench(2) > m_fz_margin_rf) |
953 |
|
|
m_rf_fz_stable_cpt++; |
954 |
|
|
else |
955 |
|
|
m_rf_fz_stable_cpt = 0; |
956 |
|
|
|
957 |
|
|
if (m_rf_fz_stable_cpt >= m_fz_stable_windows_size) { |
958 |
|
|
m_rf_fz_stable_cpt = m_fz_stable_windows_size; |
959 |
|
|
m_right_foot_is_stable = true; |
960 |
|
|
} else { |
961 |
|
|
m_right_foot_is_stable = false; |
962 |
|
|
} |
963 |
|
|
s = w_zmp * w_fz; |
964 |
|
|
return s; |
965 |
|
|
} |
966 |
|
|
|
967 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(w_rf_filtered, double) { |
968 |
|
|
if (!m_initSucceeded) { |
969 |
|
|
SEND_WARNING_STREAM_MSG( |
970 |
|
|
"Cannot compute signal w_rf_filtered before initialization!"); |
971 |
|
|
return s; |
972 |
|
|
} |
973 |
|
|
double w_rf = m_w_rfSOUT(iter); |
974 |
|
|
m_w_rf_filtered = |
975 |
|
|
m_alpha_w_filter * w_rf + |
976 |
|
|
(1 - m_alpha_w_filter) * m_w_rf_filtered; // low pass filter |
977 |
|
|
s = m_w_rf_filtered; |
978 |
|
|
return s; |
979 |
|
|
} |
980 |
|
|
|
981 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(w_lf_filtered, double) { |
982 |
|
|
if (!m_initSucceeded) { |
983 |
|
|
SEND_WARNING_STREAM_MSG( |
984 |
|
|
"Cannot compute signal w_lf_filtered before initialization!"); |
985 |
|
|
return s; |
986 |
|
|
} |
987 |
|
|
double w_lf = m_w_lfSOUT(iter); |
988 |
|
|
m_w_lf_filtered = |
989 |
|
|
m_alpha_w_filter * w_lf + |
990 |
|
|
(1 - m_alpha_w_filter) * m_w_lf_filtered; // low pass filter |
991 |
|
|
s = m_w_lf_filtered; |
992 |
|
|
return s; |
993 |
|
|
} |
994 |
|
|
|
995 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v, dynamicgraph::Vector) { |
996 |
|
|
if (!m_initSucceeded) { |
997 |
|
|
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!"); |
998 |
|
|
return s; |
999 |
|
|
} |
1000 |
|
|
if (s.size() != |
1001 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6)) |
1002 |
|
|
s.resize(m_robot_util->m_nbJoints + 6); |
1003 |
|
|
|
1004 |
|
|
m_kinematics_computationsSINNER(iter); |
1005 |
|
|
m_qSOUT(iter); |
1006 |
|
|
|
1007 |
|
|
getProfiler().start(PROFILE_BASE_VELOCITY_ESTIMATION); |
1008 |
|
|
{ |
1009 |
|
|
const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter); |
1010 |
|
|
const Eigen::Vector3d& acc_imu = m_accelerometerSIN(iter); |
1011 |
|
|
const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter); |
1012 |
|
|
const Vector6& dftrf = m_dforceRLEGSIN(iter); |
1013 |
|
|
const Vector6& dftlf = m_dforceLLEGSIN(iter); |
1014 |
|
|
assert(dq.size() == |
1015 |
|
|
static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && |
1016 |
|
|
"Unexpected size of signal joint_velocities"); |
1017 |
|
|
|
1018 |
|
|
// if the weights are not specified by the user through the input signals |
1019 |
|
|
// w_lf, w_rf then compute them if one feet is not stable, force weight to |
1020 |
|
|
// 0.0 |
1021 |
|
|
double wL, wR; |
1022 |
|
|
if (m_w_lf_inSIN.isPlugged()) |
1023 |
|
|
wL = m_w_lf_inSIN(iter); |
1024 |
|
|
else { |
1025 |
|
|
wL = m_w_lf_filteredSOUT(iter); |
1026 |
|
|
if (m_left_foot_is_stable == false) wL = 0.0; |
1027 |
|
|
} |
1028 |
|
|
if (m_w_rf_inSIN.isPlugged()) |
1029 |
|
|
wR = m_w_rf_inSIN(iter); |
1030 |
|
|
else { |
1031 |
|
|
wR = m_w_rf_filteredSOUT(iter); |
1032 |
|
|
if (m_right_foot_is_stable == false) wR = 0.0; |
1033 |
|
|
} |
1034 |
|
|
// if both weights are zero set them to a small positive value to avoid |
1035 |
|
|
// division by zero |
1036 |
|
|
if (wR == 0.0 && wL == 0.0) { |
1037 |
|
|
wR = 1e-3; |
1038 |
|
|
wL = 1e-3; |
1039 |
|
|
} |
1040 |
|
|
|
1041 |
|
|
/* Compute foot velocities */ |
1042 |
|
|
const Frame& f_lf = m_model.frames[m_left_foot_id]; |
1043 |
|
|
const Motion v_lf_local = m_data->v[f_lf.parent]; |
1044 |
|
|
const SE3 ffMlf = m_data->oMi[f_lf.parent]; |
1045 |
|
|
Vector6 v_kin_l = |
1046 |
|
|
-ffMlf.act(v_lf_local).toVector(); // this is the velocity of the base |
1047 |
|
|
// in the frame of the base. |
1048 |
|
|
v_kin_l.head<3>() = m_oRff * v_kin_l.head<3>(); |
1049 |
|
|
v_kin_l.segment<3>(3) = m_oRff * v_kin_l.segment<3>(3); |
1050 |
|
|
|
1051 |
|
|
const Frame& f_rf = m_model.frames[m_right_foot_id]; |
1052 |
|
|
const Motion v_rf_local = m_data->v[f_rf.parent]; |
1053 |
|
|
const SE3 ffMrf = m_data->oMi[f_rf.parent]; |
1054 |
|
|
Vector6 v_kin_r = |
1055 |
|
|
-ffMrf.act(v_rf_local).toVector(); // this is the velocity of the base |
1056 |
|
|
// in the frame of the base. |
1057 |
|
|
v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>(); |
1058 |
|
|
v_kin_r.segment<3>(3) = m_oRff * v_kin_r.segment<3>(3); |
1059 |
|
|
|
1060 |
|
|
m_v_kin.head<6>() = (wR * v_kin_r + wL * v_kin_l) / (wL + wR); |
1061 |
|
|
|
1062 |
|
|
/* Compute velocity induced by the flexibility */ |
1063 |
|
|
Vector6 v_flex_l; |
1064 |
|
|
Vector6 v_flex_r; |
1065 |
|
|
v_flex_l << -dftlf[0] / m_K_lf(0), -dftlf[1] / m_K_lf(1), |
1066 |
|
|
-dftlf[2] / m_K_lf(2), -dftlf[3] / m_K_lf(3), -dftlf[4] / m_K_lf(4), |
1067 |
|
|
-dftlf[5] / m_K_lf(5); |
1068 |
|
|
v_flex_r << -dftrf[0] / m_K_rf(0), -dftrf[1] / m_K_rf(1), |
1069 |
|
|
-dftrf[2] / m_K_rf(2), -dftrf[3] / m_K_rf(3), -dftrf[4] / m_K_rf(4), |
1070 |
|
|
-dftrf[5] / m_K_rf(5); |
1071 |
|
|
const Eigen::Matrix<double, 6, 6> lfAff = ffMlf.inverse().toActionMatrix(); |
1072 |
|
|
const Eigen::Matrix<double, 6, 6> rfAff = ffMrf.inverse().toActionMatrix(); |
1073 |
|
|
Eigen::Matrix<double, 12, 6> A; |
1074 |
|
|
A << lfAff, rfAff; |
1075 |
|
|
Eigen::Matrix<double, 12, 1> b; |
1076 |
|
|
b << v_flex_l, v_flex_r; |
1077 |
|
|
//~ m_v_flex.head<6>() = A.jacobiSvd(Eigen::ComputeThinU | |
1078 |
|
|
// Eigen::ComputeThinV).solve(b); |
1079 |
|
|
m_v_flex.head<6>() = (A.transpose() * A).ldlt().solve(A.transpose() * b); |
1080 |
|
|
m_v_flex.head<3>() = m_oRff * m_v_flex.head<3>(); |
1081 |
|
|
|
1082 |
|
|
/* Get an estimate of linear velocities from gyroscope only*/ |
1083 |
|
|
// we make the asumtion than we are 'turning around the foot' with pure |
1084 |
|
|
// angular velocity in the ankle measured by the gyro |
1085 |
|
|
const Matrix3 ffRimu = (m_data->oMf[m_IMU_body_id]).rotation(); |
1086 |
|
|
const Matrix3 lfRimu = ffMlf.rotation().transpose() * ffRimu; |
1087 |
|
|
const Matrix3 rfRimu = ffMrf.rotation().transpose() * ffRimu; |
1088 |
|
|
|
1089 |
|
|
const SE3 chestMimu( |
1090 |
|
|
Matrix3::Identity(), |
1091 |
|
|
Vector3(-0.13, 0.0, |
1092 |
|
|
0.118)); /// TODO Read this transform from setable parameter |
1093 |
|
|
/// /// TODO chesk the sign of the translation |
1094 |
|
|
const SE3 ffMchest(m_data->oMf[m_IMU_body_id]); |
1095 |
|
|
const SE3 imuMff = (ffMchest * chestMimu).inverse(); |
1096 |
|
|
// gVw_a = gVo_g + gHa.act(aVb_a)-gVb_g //angular velocity in the ankle |
1097 |
|
|
// from gyro and d_enc |
1098 |
|
|
const Frame& f_imu = m_model.frames[m_IMU_body_id]; |
1099 |
|
|
Vector3 gVo_a_l = Vector3(gyr_imu(0), gyr_imu(1), gyr_imu(2)) + |
1100 |
|
|
(imuMff * ffMlf).act(v_lf_local).angular() - |
1101 |
|
|
m_data->v[f_imu.parent].angular(); |
1102 |
|
|
Vector3 gVo_a_r = Vector3(gyr_imu(0), gyr_imu(1), gyr_imu(2)) + |
1103 |
|
|
(imuMff * ffMrf).act(v_rf_local).angular() - |
1104 |
|
|
m_data->v[f_imu.parent].angular(); |
1105 |
|
|
Motion v_gyr_ankle_l(Vector3(0., 0., 0.), lfRimu * gVo_a_l); |
1106 |
|
|
Motion v_gyr_ankle_r(Vector3(0., 0., 0.), rfRimu * gVo_a_r); |
1107 |
|
|
Vector6 v_gyr_l = -ffMlf.inverse().act(v_gyr_ankle_l).toVector(); |
1108 |
|
|
Vector6 v_gyr_r = -ffMrf.inverse().act(v_gyr_ankle_r).toVector(); |
1109 |
|
|
m_v_gyr.head<6>() = (wL * v_gyr_l + wR * v_gyr_r) / (wL + wR); |
1110 |
|
|
|
1111 |
|
|
/* Get an estimate of linear velocities from filtered accelerometer |
1112 |
|
|
* integration */ |
1113 |
|
|
|
1114 |
|
|
/* rotate acceleration to express it in the world frame */ |
1115 |
|
|
//~ pointRotationByQuaternion(acc_imu,quatIMU_vec,acc_world); //this is |
1116 |
|
|
// false because yaw from imuquat is drifting |
1117 |
|
|
const Vector3 acc_world = m_oRchest * acc_imu; |
1118 |
|
|
|
1119 |
|
|
/* now, the acceleration is expressed in the world frame, |
1120 |
|
|
* so it can be written as the sum of the proper acceleration and the |
1121 |
|
|
* constant gravity vector. We could remove this assuming a [0,0,9.81] |
1122 |
|
|
* but we prefer to filter this signal with low frequency high pass |
1123 |
|
|
* filter since it is robust to gravity norm error, and we know that |
1124 |
|
|
* globaly the robot can not accelerate continuously. */ |
1125 |
|
|
|
1126 |
|
|
///* Extract DC component by low pass filter and remove it*/ |
1127 |
|
|
if (m_isFirstIterFlag) { |
1128 |
|
|
m_last_DCacc = acc_world; // Copy the first acceleration data |
1129 |
|
|
m_isFirstIterFlag = false; |
1130 |
|
|
sendMsg("iter:" + toString(iter) + "\n", MSG_TYPE_INFO); |
1131 |
|
|
} |
1132 |
|
|
const Vector3 DCacc = |
1133 |
|
|
acc_world * (1 - m_alpha_DC_acc) + m_last_DCacc * (m_alpha_DC_acc); |
1134 |
|
|
//~ sendMsg("acc_world :"+toString(acc_world)+"\n", |
1135 |
|
|
// MSG_TYPE_INFO); |
1136 |
|
|
m_last_DCacc = DCacc; |
1137 |
|
|
const Vector3 ACacc = acc_world - DCacc; |
1138 |
|
|
m_v_ac = ACacc; |
1139 |
|
|
/* Then this acceleration is integrated. */ |
1140 |
|
|
const Vector3 vel = m_last_vel + ACacc * m_dt; |
1141 |
|
|
m_last_vel = vel; |
1142 |
|
|
/* To stabilise the integrated velocity, we add the |
1143 |
|
|
* asumption that globaly, velocity is zero. So we remove DC again */ |
1144 |
|
|
const Vector3 DCvel = |
1145 |
|
|
vel * (1 - m_alpha_DC_vel) + m_last_DCvel * (m_alpha_DC_vel); |
1146 |
|
|
m_last_DCvel = DCvel; |
1147 |
|
|
const Vector3 ACvel = vel - DCvel; |
1148 |
|
|
m_v_ac = ACvel; |
1149 |
|
|
|
1150 |
|
|
/* Express back velocity in the imu frame to get a full 6d velocity with the |
1151 |
|
|
* gyro*/ |
1152 |
|
|
const Vector3 imuVimu = m_oRchest.transpose() * ACvel; |
1153 |
|
|
/* Here we could remove dc from gyrometer to remove bias*/ /// TODO |
1154 |
|
|
const Motion imuWimu(imuVimu, gyr_imu); |
1155 |
|
|
// const Frame & f_imu = m_model.frames[m_IMU_body_id]; |
1156 |
|
|
// const SE3 ffMchest(m_data->oMf[m_IMU_body_id]); |
1157 |
|
|
// const SE3 chestMimu(Matrix3::Identity(), +1.0*Vector3(-0.13, 0.0, |
1158 |
|
|
// 0.118)); ///TODO Read this transform from setable parameter /// TODO |
1159 |
|
|
// chesk the sign of the translation |
1160 |
|
|
const SE3 ffMimu = ffMchest * chestMimu; |
1161 |
|
|
const Motion v = ffMimu.act(imuWimu); //- ffWchest; |
1162 |
|
|
m_v_imu.head<6>() = v.toVector(); |
1163 |
|
|
m_v_imu.head<3>() = m_oRff * m_v_imu.head<3>(); |
1164 |
|
|
|
1165 |
|
|
//~ m_v_sot.head<6>() = m_v_kin.head<6>(); |
1166 |
|
|
//~ m_v_sot.head<6>() = m_v_flex.head<6>() + m_v_kin.head<6>(); |
1167 |
|
|
m_v_sot.head<6>() = m_v_gyr.head<6>() + m_v_kin.head<6>(); |
1168 |
|
|
// m_v_sot.head<6>() = m_v_gyr.head<6>(); |
1169 |
|
|
//~ m_v_sot.head<6>() = m_v_imu.head<6>(); |
1170 |
|
|
|
1171 |
|
|
m_v_sot.tail(m_robot_util->m_nbJoints) = dq; |
1172 |
|
|
m_v_kin.tail(m_robot_util->m_nbJoints) = dq; |
1173 |
|
|
m_v_flex.tail(m_robot_util->m_nbJoints) = dq; |
1174 |
|
|
m_v_gyr.tail(m_robot_util->m_nbJoints) = dq; |
1175 |
|
|
m_v_imu.tail(m_robot_util->m_nbJoints) = dq; |
1176 |
|
|
s = m_v_sot; |
1177 |
|
|
} |
1178 |
|
|
getProfiler().stop(PROFILE_BASE_VELOCITY_ESTIMATION); |
1179 |
|
|
return s; |
1180 |
|
|
} |
1181 |
|
|
|
1182 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v_kin, dynamicgraph::Vector) { |
1183 |
|
|
if (!m_initSucceeded) { |
1184 |
|
|
SEND_WARNING_STREAM_MSG( |
1185 |
|
|
"Cannot compute signal v_kin before initialization!"); |
1186 |
|
|
return s; |
1187 |
|
|
} |
1188 |
|
|
m_vSOUT(iter); |
1189 |
|
|
s = m_v_kin; |
1190 |
|
|
return s; |
1191 |
|
|
} |
1192 |
|
|
|
1193 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v_flex, dynamicgraph::Vector) { |
1194 |
|
|
if (!m_initSucceeded) { |
1195 |
|
|
SEND_WARNING_STREAM_MSG( |
1196 |
|
|
"Cannot compute signal v_flex before initialization!"); |
1197 |
|
|
return s; |
1198 |
|
|
} |
1199 |
|
|
m_vSOUT(iter); |
1200 |
|
|
s = m_v_flex + m_v_kin; |
1201 |
|
|
return s; |
1202 |
|
|
} |
1203 |
|
|
|
1204 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v_imu, dynamicgraph::Vector) { |
1205 |
|
|
if (!m_initSucceeded) { |
1206 |
|
|
SEND_WARNING_STREAM_MSG( |
1207 |
|
|
"Cannot compute signal v_imu before initialization!"); |
1208 |
|
|
return s; |
1209 |
|
|
} |
1210 |
|
|
m_vSOUT(iter); |
1211 |
|
|
s = m_v_imu; |
1212 |
|
|
return s; |
1213 |
|
|
} |
1214 |
|
|
|
1215 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v_gyr, dynamicgraph::Vector) { |
1216 |
|
|
if (!m_initSucceeded) { |
1217 |
|
|
SEND_WARNING_STREAM_MSG( |
1218 |
|
|
"Cannot compute signal v_gyr before initialization!"); |
1219 |
|
|
return s; |
1220 |
|
|
} |
1221 |
|
|
m_vSOUT(iter); |
1222 |
|
|
s = m_v_gyr; |
1223 |
|
|
return s; |
1224 |
|
|
} |
1225 |
|
|
|
1226 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(v_ac, dynamicgraph::Vector) { |
1227 |
|
|
if (!m_initSucceeded) { |
1228 |
|
|
SEND_WARNING_STREAM_MSG( |
1229 |
|
|
"Cannot compute signal v_ac before initialization!"); |
1230 |
|
|
return s; |
1231 |
|
|
} |
1232 |
|
|
m_vSOUT(iter); |
1233 |
|
|
s = m_v_ac; |
1234 |
|
|
return s; |
1235 |
|
|
} |
1236 |
|
|
|
1237 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(a_ac, dynamicgraph::Vector) { |
1238 |
|
|
if (!m_initSucceeded) { |
1239 |
|
|
SEND_WARNING_STREAM_MSG( |
1240 |
|
|
"Cannot compute signal a_ac before initialization!"); |
1241 |
|
|
return s; |
1242 |
|
|
} |
1243 |
|
|
m_vSOUT(iter); |
1244 |
|
|
s = m_a_ac; |
1245 |
|
|
return s; |
1246 |
|
|
} |
1247 |
|
|
|
1248 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
1249 |
|
|
|
1250 |
|
|
/* ------------------------------------------------------------------- */ |
1251 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
1252 |
|
|
/* ------------------------------------------------------------------- */ |
1253 |
|
|
|
1254 |
|
|
void BaseEstimator::display(std::ostream& os) const { |
1255 |
|
|
os << "BaseEstimator " << getName(); |
1256 |
|
|
try { |
1257 |
|
|
getProfiler().report_all(3, os); |
1258 |
|
|
} catch (ExceptionSignal e) { |
1259 |
|
|
} |
1260 |
|
|
} |
1261 |
|
|
} // namespace torque_control |
1262 |
|
|
} // namespace sot |
1263 |
|
|
} // namespace dynamicgraph |