sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
admittance-controller.hh
Go to the documentation of this file.
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>
39
40namespace dynamicgraph {
41namespace sot {
42namespace torque_control {
43/* --------------------------------------------------------------------- */
44/* --- CLASS ----------------------------------------------------------- */
45/* --------------------------------------------------------------------- */
46
48 : public ::dynamicgraph::Entity {
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);
71 DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector);
72 DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector);
73 DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector);
74 DECLARE_SIGNAL_IN(fRightFootFiltered,
75 dynamicgraph::Vector);
76 DECLARE_SIGNAL_IN(fLeftFootFiltered,
77 dynamicgraph::Vector);
79 controlledJoints,
80 dynamicgraph::Vector);
82 damping,
83 dynamicgraph::Vector);
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);
93 // DEBUG SIGNALS
95 dynamicgraph::Vector);
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;
114 bool
118 double m_dt;
119 int m_nj;
120
124
126 tsid::robots::RobotWrapper* m_robot;
127 pinocchio::Data* m_data;
128
129 tsid::math::Vector6 m_f_RF;
130 tsid::math::Vector6 m_f_LF;
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;
136 // tsid::math::Vector m_dqDesIntegral; /// integral of the desired
137 // joint velocities
138 tsid::math::Vector
140 tsid::math::Vector m_qPrev;
141
142 typedef pinocchio::Data::Matrix6x Matrix6x;
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__
#define SOTADMITTANCECONTROLLER_EXPORT
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
tsid::math::Vector m_dq_fd
integral of the velocity error
DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector)
dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector)
damping factors used for the 4 end-effectors
bool m_useJacobianTranspose
true if the entity has been successfully initialized
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector)
mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector)
6d estimated force filtered
tsid::math::Vector6 m_f_LF
desired 6d wrench right foot
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector)
tsid::robots::RobotWrapper * m_robot
frame id of left foot
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector)
6d reference force
DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector)
tsid::math::Vector m_qPrev
joint velocities computed with finite differences
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector)
control
tsid::math::Vector m_q_urdf
desired 6d wrench left foot
DECLARE_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector)
6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector)
6d estimated force filtered
pinocchio::Data::Matrix6x Matrix6x
previous value of encoders
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector)
6d estimated force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector)
6d reference force
to read text file
Definition treeview.dox:22