8 #include <dynamic-graph/factory.h>
9 #include <sot/core/debug.hh>
18 #define ALL_INPUT_SIGNALS m_base6d_encodersSIN << m_accelerometerSIN << m_jointTorquesSIN << m_gyroscopeSIN
20 #define ALL_OUTPUT_SIGNALS m_jointTorquesEstimatedSOUT
25 using namespace Eigen;
40 CONSTRUCT_SIGNAL_IN(base6d_encoders,
dynamicgraph::Vector),
41 CONSTRUCT_SIGNAL_IN(accelerometer,
dynamicgraph::Vector),
45 CONSTRUCT_SIGNAL_OUT(jointTorquesEstimated,
dynamicgraph::Vector,
47 sensor_offset_status(PRECOMPUTATION),
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",
57 docCommandVoid2(
"Compute the offset for sensor calibration",
58 "Number of iteration to average over.",
"Maximum allowed offset")));
60 docDirectGetter(
"Return the computed sensor offsets",
"vector")));
73 const double& gyro_epsilon_,
const std::string& ffJointName,
74 const std::string& torsoJointName) {
77 std::string localName(robotRef);
78 if (isNameInRobotUtil(localName)) {
80 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
82 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
86 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(),
m_model);
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);
101 m_torso_X_imu.translation() = m_torso_X_imu_.block<3, 1>(0, 3);
106 if (sensor_offset_status == PRECOMPUTATION) {
107 SEND_MSG(
"Starting offset computation with no. iterations:" + nIterations, MSG_TYPE_DEBUG);
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);
114 SEND_MSG(
"Recomputing offset with no. iterations:" + nIterations, MSG_TYPE_DEBUG);
117 current_progress = 0;
122 sensor_offset_status = INPROGRESS;
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);
135 int i = current_progress;
137 if (i < n_iterations) {
138 SEND_MSG(
"Collecting signals for iteration no:" + i, MSG_TYPE_DEBUG_STREAM);
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);
144 Eigen::VectorXd enc(m_model.nq);
146 enc.tail(m_model.nv - 6) = sot_enc.tail(m_model.nv - 6);
148 enc.head<6>().setZero();
153 pinocchio::forwardKinematics(m_model, *m_data, enc);
155 pinocchio::SE3 oMimu = m_data->oMi[torsoIndex] * m_torso_X_imu;
159 const dynamicgraph::Vector acc = oMimu.rotation() * IMU_acc;
162 m_model.gravity.linear() = acc;
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);
177 jointTorqueOffsets += current_offset;
179 }
else if (i == n_iterations) {
181 jointTorqueOffsets /= n_iterations;
182 sensor_offset_status = COMPUTED;
185 assert(
false &&
"Error in collectSensorData. ");
194 m_collectSensorDataSINNER(iter);
196 if (s.size() != m_model.nv - 6) s.resize(m_model.nv - 6);
198 if (sensor_offset_status == PRECOMPUTATION || sensor_offset_status == INPROGRESS) {
199 s = m_jointTorquesSIN(iter);
201 const dynamicgraph::Vector& inputTorques = m_jointTorquesSIN(iter);
202 s = inputTorques - jointTorqueOffsets;
208 os <<
"TorqueOffsetEstimator" << getName() <<
":\n";
210 getProfiler().report_all(3, os);
211 }
catch (ExceptionSignal e) {
pinocchio::JointIndex torsoIndex
void computeOffset(const int &nIterations, const double &epsilon)
Eigen::VectorXd jointTorqueOffsets
RobotUtilShrPtr m_robot_util
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)
pinocchio::Data * m_data
Pinocchio robot model.
virtual void display(std::ostream &os) const
int n_iterations
Pinocchio robot data.
pinocchio::JointIndex ffIndex
pinocchio::SE3 m_torso_X_imu
TorqueOffsetEstimator(const std::string &name)
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)
AdmittanceController EntityClassName
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
#define ALL_OUTPUT_SIGNALS
#define ALL_INPUT_SIGNALS