1 |
|
|
/* |
2 |
|
|
* Copyright 2014, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#ifndef __sot_torque_control_admittance_controller_H__ |
7 |
|
|
#define __sot_torque_control_admittance_controller_H__ |
8 |
|
|
|
9 |
|
|
/* --------------------------------------------------------------------- */ |
10 |
|
|
/* --- API ------------------------------------------------------------- */ |
11 |
|
|
/* --------------------------------------------------------------------- */ |
12 |
|
|
|
13 |
|
|
#if defined(WIN32) |
14 |
|
|
#if defined(sot_admittance_controller_EXPORTS) |
15 |
|
|
#define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport) |
16 |
|
|
#else |
17 |
|
|
#define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport) |
18 |
|
|
#endif |
19 |
|
|
#else |
20 |
|
|
#define SOTADMITTANCECONTROLLER_EXPORT |
21 |
|
|
#endif |
22 |
|
|
|
23 |
|
|
/* --------------------------------------------------------------------- */ |
24 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
25 |
|
|
/* --------------------------------------------------------------------- */ |
26 |
|
|
|
27 |
|
|
#include <map> |
28 |
|
|
#include <tsid/robots/robot-wrapper.hpp> |
29 |
|
|
#include <tsid/tasks/task-se3-equality.hpp> |
30 |
|
|
|
31 |
|
|
#include "boost/assign.hpp" |
32 |
|
|
|
33 |
|
|
/* HELPER */ |
34 |
|
|
#include <dynamic-graph/signal-helper.h> |
35 |
|
|
|
36 |
|
|
#include <sot/core/matrix-geometry.hh> |
37 |
|
|
#include <sot/core/robot-utils.hh> |
38 |
|
|
#include <sot/torque_control/utils/vector-conversions.hh> |
39 |
|
|
|
40 |
|
|
namespace dynamicgraph { |
41 |
|
|
namespace sot { |
42 |
|
|
namespace torque_control { |
43 |
|
|
/* --------------------------------------------------------------------- */ |
44 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
45 |
|
|
/* --------------------------------------------------------------------- */ |
46 |
|
|
|
47 |
|
|
class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController |
48 |
|
|
: public ::dynamicgraph::Entity { |
49 |
|
|
typedef AdmittanceController EntityClassName; |
50 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
51 |
|
|
|
52 |
|
|
public: |
53 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
54 |
|
|
|
55 |
|
|
/* --- CONSTRUCTOR ---- */ |
56 |
|
|
AdmittanceController(const std::string& name); |
57 |
|
|
|
58 |
|
|
void init(const double& dt, const std::string& robotRef); |
59 |
|
|
|
60 |
|
|
/* --- SIGNALS --- */ |
61 |
|
|
DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector); |
62 |
|
|
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector); |
63 |
|
|
DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector); |
64 |
|
|
DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector); |
65 |
|
|
DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector); |
66 |
|
|
DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector); |
67 |
|
|
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector); |
68 |
|
|
DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector); |
69 |
|
|
DECLARE_SIGNAL_IN(fRightFootRef, |
70 |
|
|
dynamicgraph::Vector); /// 6d reference force |
71 |
|
|
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force |
72 |
|
|
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force |
73 |
|
|
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force |
74 |
|
|
DECLARE_SIGNAL_IN(fRightFootFiltered, |
75 |
|
|
dynamicgraph::Vector); /// 6d estimated force filtered |
76 |
|
|
DECLARE_SIGNAL_IN(fLeftFootFiltered, |
77 |
|
|
dynamicgraph::Vector); /// 6d estimated force filtered |
78 |
|
|
DECLARE_SIGNAL_IN( |
79 |
|
|
controlledJoints, |
80 |
|
|
dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise |
81 |
|
|
DECLARE_SIGNAL_IN( |
82 |
|
|
damping, |
83 |
|
|
dynamicgraph::Vector); /// damping factors used for the 4 end-effectors |
84 |
|
|
|
85 |
|
|
// DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d |
86 |
|
|
// reference force DECLARE_SIGNAL_IN(fLeftHandRef, |
87 |
|
|
// dynamicgraph::Vector); /// 6d reference force |
88 |
|
|
// DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d |
89 |
|
|
// estimated force DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); |
90 |
|
|
// /// 6d estimated force |
91 |
|
|
|
92 |
|
|
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector); /// control |
93 |
|
|
// DEBUG SIGNALS |
94 |
|
|
DECLARE_SIGNAL_OUT(dqDes, |
95 |
|
|
dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f) |
96 |
|
|
DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector); /// |
97 |
|
|
DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector); /// |
98 |
|
|
// DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// |
99 |
|
|
// fRef-f DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); |
100 |
|
|
// /// fRef-f |
101 |
|
|
|
102 |
|
|
/* --- COMMANDS --- */ |
103 |
|
|
/* --- ENTITY INHERITANCE --- */ |
104 |
|
|
virtual void display(std::ostream& os) const; |
105 |
|
|
|
106 |
|
|
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, |
107 |
|
|
const char* = "", int = 0) { |
108 |
|
|
logger_.stream(t) << ("[" + name + "] " + msg) << '\n'; |
109 |
|
|
} |
110 |
|
|
|
111 |
|
|
protected: |
112 |
|
|
Eigen::VectorXd m_u; /// control (i.e. motor currents) |
113 |
|
|
bool m_firstIter; |
114 |
|
|
bool |
115 |
|
|
m_initSucceeded; /// true if the entity has been successfully initialized |
116 |
|
|
bool m_useJacobianTranspose; /// if true it uses the Jacobian transpose |
117 |
|
|
/// rather than the pseudoinverse |
118 |
|
|
double m_dt; /// control loop time period |
119 |
|
|
int m_nj; |
120 |
|
|
|
121 |
|
|
/// robot geometric/inertial data |
122 |
|
|
int m_frame_id_rf; /// frame id of right foot |
123 |
|
|
int m_frame_id_lf; /// frame id of left foot |
124 |
|
|
|
125 |
|
|
/// tsid |
126 |
|
|
tsid::robots::RobotWrapper* m_robot; |
127 |
|
|
pinocchio::Data* m_data; |
128 |
|
|
|
129 |
|
|
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot |
130 |
|
|
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot |
131 |
|
|
tsid::math::Vector m_q_urdf; |
132 |
|
|
tsid::math::Vector m_v_urdf; |
133 |
|
|
|
134 |
|
|
tsid::math::Vector m_dq_des_urdf; |
135 |
|
|
tsid::math::Vector m_dqErrIntegral; /// integral of the velocity error |
136 |
|
|
// tsid::math::Vector m_dqDesIntegral; /// integral of the desired |
137 |
|
|
// joint velocities |
138 |
|
|
tsid::math::Vector |
139 |
|
|
m_dq_fd; /// joint velocities computed with finite differences |
140 |
|
|
tsid::math::Vector m_qPrev; /// previous value of encoders |
141 |
|
|
|
142 |
|
|
typedef pinocchio::Data::Matrix6x Matrix6x; |
143 |
|
|
Matrix6x m_J_RF; |
144 |
|
|
Matrix6x m_J_LF; |
145 |
|
|
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR; |
146 |
|
|
Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR; |
147 |
|
|
tsid::math::Vector6 m_v_RF_int; |
148 |
|
|
tsid::math::Vector6 m_v_LF_int; |
149 |
|
|
|
150 |
|
|
RobotUtilShrPtr m_robot_util; |
151 |
|
|
|
152 |
|
|
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left |
153 |
|
|
// foot tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp |
154 |
|
|
// left foot tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired |
155 |
|
|
// zmp left foot expressed in local frame tsid::math::Vector3 |
156 |
|
|
// m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in |
157 |
|
|
// local frame tsid::math::Vector3 m_zmp_des; /// 3d |
158 |
|
|
// desired global zmp tsid::math::Vector3 m_zmp_LF; /// 3d |
159 |
|
|
// zmp left foot tsid::math::Vector3 m_zmp_RF; /// 3d zmp |
160 |
|
|
// left foot tsid::math::Vector3 m_zmp; /// 3d global |
161 |
|
|
// zmp |
162 |
|
|
}; // class AdmittanceController |
163 |
|
|
|
164 |
|
|
} // namespace torque_control |
165 |
|
|
} // namespace sot |
166 |
|
|
} // namespace dynamicgraph |
167 |
|
|
|
168 |
|
|
#endif // #ifndef __sot_torque_control_admittance_controller_H__ |