sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
position-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_position_controller_H__
7#define __sot_torque_control_position_controller_H__
8
9/* --------------------------------------------------------------------- */
10/* --- API ------------------------------------------------------------- */
11/* --------------------------------------------------------------------- */
12
13#if defined(WIN32)
14#if defined(position_controller_EXPORTS)
15#define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
16#else
17#define SOTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
18#endif
19#else
20#define SOTPOSITIONCONTROLLER_EXPORT
21#endif
22
23/* --------------------------------------------------------------------- */
24/* --- INCLUDE --------------------------------------------------------- */
25/* --------------------------------------------------------------------- */
26
27#include <pinocchio/fwd.hpp>
28
29// include pinocchio first
30
31#include <dynamic-graph/signal-helper.h>
32
33#include <boost/assign.hpp>
34#include <map>
35#include <sot/core/matrix-geometry.hh>
36#include <sot/core/robot-utils.hh>
38
39namespace dynamicgraph {
40namespace sot {
41namespace torque_control {
42
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 PositionController(const std::string& name);
57
58 void init(const double& dt, const std::string& robotRef);
59
60 void resetIntegral();
61
62 /* --- SIGNALS --- */
63 DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
64 DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
65 DECLARE_SIGNAL_IN(qRef, dynamicgraph::Vector);
66 DECLARE_SIGNAL_IN(dqRef, dynamicgraph::Vector);
67 DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
68 DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
69 DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
70
72 dynamicgraph::Vector);
73 // DEBUG SIGNALS
74 DECLARE_SIGNAL_OUT(qError, dynamicgraph::Vector);
75
76 /* --- COMMANDS --- */
77 /* --- ENTITY INHERITANCE --- */
78 virtual void display(std::ostream& os) const;
79
80 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
81 const char* = "", int = 0) {
82 logger_.stream(t) << ("[PositionController-" + name + "] " + msg) << '\n';
83 }
84
85 protected:
86 RobotUtilShrPtr m_robot_util;
87 Eigen::VectorXd m_pwmDes;
88 bool
90 double m_dt;
91
93 Eigen::VectorXd m_e_integral;
94
95 Eigen::VectorXd m_q, m_dq;
96
97}; // class PositionController
98
99} // namespace torque_control
100} // namespace sot
101} // namespace dynamicgraph
102
103#endif // #ifndef __sot_torque_control_position_controller_H__
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Eigen::VectorXd m_e_integral
control loop time period
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector)
double m_dt
true if the entity has been successfully initialized
DECLARE_SIGNAL_OUT(pwmDes, dynamicgraph::Vector)
joint integral gains
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector)
joint proportional gains
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector)
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector)
joint derivative gains
DECLARE_SIGNAL_OUT(qError, dynamicgraph::Vector)
Kp*e_q + Kd*de_q + Ki*int(e_q)
to read text file
Definition: treeview.dox:22
#define SOTPOSITIONCONTROLLER_EXPORT