sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
joint-torque-controller.hh
Go to the documentation of this file.
1/*
2 * Copyright 2015, Andrea Del Prete, LAAS-CNRS
3 *
4 */
5
6#ifndef __sot_torque_control_JointTorqueController_H__
7#define __sot_torque_control_JointTorqueController_H__
8/* --------------------------------------------------------------------- */
9/* --- API ------------------------------------------------------------- */
10/* --------------------------------------------------------------------- */
11
12#if defined(WIN32)
13#if defined(joint_torque_controller_EXPORTS)
14#define SOTJOINTTORQUECONTROLLER_EXPORT __declspec(dllexport)
15#else
16#define SOTJOINTTORQUECONTROLLER_EXPORT __declspec(dllimport)
17#endif
18#else
19#define SOTJOINTTORQUECONTROLLER_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 <pinocchio/fwd.hpp>
30#include <tsid/utils/stop-watch.hpp>
31/* HELPER */
32#include <dynamic-graph/signal-helper.h>
33
34#include <sot/core/matrix-geometry.hh>
35#include <sot/core/robot-utils.hh>
37
38/*Motor model*/
40
41namespace dynamicgraph {
42namespace sot {
43namespace torque_control {
44
68 : public ::dynamicgraph::Entity {
69 DYNAMIC_GRAPH_ENTITY_DECL();
70
71 public: /* --- SIGNALS --- */
72 DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector);
73 DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
74 DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector);
75 DECLARE_SIGNAL_IN(jointsTorques,
76 dynamicgraph::Vector);
78 jointsTorquesDerivative,
79 dynamicgraph::Vector);
80 DECLARE_SIGNAL_IN(jointsTorquesDesired,
81 dynamicgraph::Vector);
82 // DECLARE_SIGNAL_IN(jointsTorquesDesiredDerivative,
83 // dynamicgraph::Vector);/// desired joints torques derivative dtauDes
84 DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector);
86 dynamicgraph::Vector);
89 KiTorque,
90 dynamicgraph::Vector);
92 KdTorque,
93 dynamicgraph::Vector);
95 KdVel, dynamicgraph::Vector);
97 KiVel, dynamicgraph::Vector);
98 DECLARE_SIGNAL_IN(torque_integral_saturation,
99 dynamicgraph::Vector);
100
101 // DECLARE_SIGNAL_IN(dq_threshold, dynamicgraph::Vector); ///
102 // velocity sign threshold DECLARE_SIGNAL_IN(ddq_threshold,
103 // dynamicgraph::Vector); /// acceleration sign threshold
104
106 DECLARE_SIGNAL_IN(coulomb_friction_compensation_percentage,
107 dynamicgraph::Vector);
108 DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector);
109 DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector);
110 DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector);
111 DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector);
112 DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector);
113 DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector);
114 DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector);
115 DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector);
116 DECLARE_SIGNAL_IN(polySignDq, dynamicgraph::Vector);
117
118 DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
120 smoothSignDq, dynamicgraph::Vector);
122 torque_error_integral,
123 dynamicgraph::Vector);
124
125 protected:
127 double m_dt;
128 Eigen::VectorXd m_tau_star;
129 Eigen::VectorXd m_current_des;
130 Eigen::VectorXd m_tauErrIntegral;
131 Eigen::VectorXd m_currentErrIntegral;
132 // Eigen::VectorXd m_dqDesIntegral; /// integral of the desired
133 // velocity
134 Eigen::VectorXd m_dqErrIntegral;
135
136 RobotUtilShrPtr m_robot_util;
137
138 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
139 const char* = "", int = 0) {
140 logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
141 }
142
143 public:
144 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145
147 JointTorqueController(const std::string& name);
148
153 void init(const double& timestep, const std::string& robotRef);
154
155 void reset_integral();
156
157 public: /* --- ENTITY INHERITANCE --- */
158 virtual void display(std::ostream& os) const;
159
160}; // class JointTorqueController
161
162} // namespace torque_control
163} // namespace sot
164} // namespace dynamicgraph
165
166#endif // #ifndef __sot_torque_control_JointTorqueController_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(KpTorque, dynamicgraph::Vector)
desired joint velocities
DECLARE_SIGNAL_IN(coulomb_friction_compensation_percentage, dynamicgraph::Vector)
integral error saturation
DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector)
RobotUtilShrPtr m_robot_util
integral of the velocity error
Eigen::VectorXd m_currentErrIntegral
integral of the torque error
MotorModel motorModel
integral of the torque tracking error
DECLARE_SIGNAL_IN(KdTorque, dynamicgraph::Vector)
integral gain for torque feedback controller
DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector)
q
DECLARE_SIGNAL_OUT(torque_error_integral, dynamicgraph::Vector)
smooth approximation of sign(dq)
DECLARE_SIGNAL_IN(jointsTorquesDesired, dynamicgraph::Vector)
estimated joints torques derivative dtau
DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(jointsTorques, dynamicgraph::Vector)
ddq
DECLARE_SIGNAL_IN(jointsTorquesDerivative, dynamicgraph::Vector)
estimated joints torques tau
DECLARE_SIGNAL_IN(jointsAccelerations, dynamicgraph::Vector)
dq
DECLARE_SIGNAL_IN(KdVel, dynamicgraph::Vector)
derivative gain for torque feedback controller
Eigen::VectorXd m_dqErrIntegral
integral of the current error
DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(KiVel, dynamicgraph::Vector)
derivative gain for velocity feedback
DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(dq_des, dynamicgraph::Vector)
desired joints torques tauDes
DECLARE_SIGNAL_IN(torque_integral_saturation, dynamicgraph::Vector)
integral gain for velocity feedback
DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(polySignDq, dynamicgraph::Vector)
DECLARE_SIGNAL_OUT(smoothSignDq, dynamicgraph::Vector)
Desired current.
DECLARE_SIGNAL_IN(jointsPositions, dynamicgraph::Vector)
#define SOTJOINTTORQUECONTROLLER_EXPORT
to read text file
Definition treeview.dox:22