sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
torque-offset-estimator.hh
Go to the documentation of this file.
1/*
2 * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
3 *
4 */
5
6#ifndef __sot_torque_control_TorqueOffsetEstimator_H__
7#define __sot_torque_control_TorqueOffsetEstimator_H__
8/* --------------------------------------------------------------------- */
9/* --- API ------------------------------------------------------------- */
10/* --------------------------------------------------------------------- */
11
12#if defined(WIN32)
13#if defined(torque_offset_estimator_EXPORTS)
14#define TORQUEOFFSETESTIMATOR_EXPORT __declspec(dllexport)
15#else
16#define TORQUEOFFSETESTIMATOR_EXPORT __declspec(dllimport)
17#endif
18#else
19#define TORQUEOFFSETESTIMATOR_EXPORT
20#endif
21
22// #define VP_DEBUG 1 /// enable debug output
23// #define VP_DEBUG_MODE 20
24
25/* --------------------------------------------------------------------- */
26/* --- INCLUDE --------------------------------------------------------- */
27/* --------------------------------------------------------------------- */
28
29#include <Eigen/StdVector>
30#include <boost/circular_buffer.hpp>
31
32/*Motor model*/
33#include <pinocchio/algorithm/kinematics.hpp>
34#include <pinocchio/algorithm/rnea.hpp>
35#include <pinocchio/multibody/model.hpp>
36#include <pinocchio/parsers/urdf.hpp>
37
38/* HELPER */
39#include <dynamic-graph/signal-helper.h>
40
41#include <sot/core/matrix-geometry.hh>
42#include <sot/core/robot-utils.hh>
43#include <sot/core/stop-watch.hh>
44
45/*Motor model*/
47
48namespace dynamicgraph {
49namespace sot {
50namespace torque_control {
51
53 : public ::dynamicgraph::Entity {
54 DYNAMIC_GRAPH_ENTITY_DECL();
55
56 public: /* --- SIGNALS --- */
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
58 typedef int dummy;
59 typedef std::vector<Eigen::VectorXd,
60 Eigen::aligned_allocator<Eigen::VectorXd> >
62
64 TorqueOffsetEstimator(const std::string& name);
65 void init(const std::string& urdfFile, const Eigen::Matrix4d& _m_torso_X_imu,
66 const double& gyro_epsilon, const std::string& ffJointName,
67 const std::string& torsoJointName);
68 void computeOffset(const int& nIterations, const double& epsilon);
69
70 virtual void display(std::ostream& os) const;
71
72 DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
73 DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
74 DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
75 DECLARE_SIGNAL_IN(jointTorques, dynamicgraph::Vector);
76 DECLARE_SIGNAL_INNER(collectSensorData, dummy);
77 DECLARE_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector);
78
79 protected:
80 RobotUtilShrPtr m_robot_util;
81 pinocchio::Model m_model;
82 pinocchio::Data* m_data;
83 int n_iterations; // Number of iterations to consider
84 double epsilon;
86
87 pinocchio::JointIndex ffIndex,
88 torsoIndex; // Index of the free-flyer and torso frames
89 Eigen::VectorXd jointTorqueOffsets;
90 pinocchio::SE3 m_torso_X_imu; // Definition of the imu in the chest frame.
91
92 // stdAlignedVector encSignals;
93 // stdAlignedVector accSignals;
94 // stdAlignedVector gyrSignals;
95 // stdAlignedVector tauSignals;
96
97 // stdAlignedVector stdVecJointTorqueOffsets;
98
99 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
100 const char* = "", int = 0) {
101 logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
102 }
103
104 private:
105 enum { PRECOMPUTATION, INPROGRESS, COMPUTED } sensor_offset_status;
106
107 // void calculateSensorOffsets();
108 int current_progress;
109}; // class TorqueOffsetEstimator
110
111} // namespace torque_control
112} // namespace sot
113} // namespace dynamicgraph
114
115#endif // #ifndef __sot_torque_control_TorqueOffsetEstimator_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(jointTorques, dynamicgraph::Vector)
std::vector< Eigen::VectorXd, Eigen::aligned_allocator< Eigen::VectorXd > > stdAlignedVector
DECLARE_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector)
to read text file
Definition: treeview.dox:22
#define TORQUEOFFSETESTIMATOR_EXPORT