1 |
|
|
/* |
2 |
|
|
* Copyright 2019 |
3 |
|
|
* |
4 |
|
|
* LAAS-CNRS |
5 |
|
|
* |
6 |
|
|
* Noëlie Ramuzat |
7 |
|
|
* This file is part of sot-core. |
8 |
|
|
* See license file. |
9 |
|
|
*/ |
10 |
|
|
|
11 |
|
|
#ifndef __sot_core_admittance_control_op_point_H__ |
12 |
|
|
#define __sot_core_admittance_control_op_point_H__ |
13 |
|
|
|
14 |
|
|
/* --------------------------------------------------------------------- */ |
15 |
|
|
/* --- API ------------------------------------------------------------- */ |
16 |
|
|
/* --------------------------------------------------------------------- */ |
17 |
|
|
|
18 |
|
|
#if defined(WIN32) |
19 |
|
|
#if defined(admittance_control_op_point_EXPORTS) |
20 |
|
|
#define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllexport) |
21 |
|
|
#else |
22 |
|
|
#define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllimport) |
23 |
|
|
#endif |
24 |
|
|
#else |
25 |
|
|
#define ADMITTANCECONTROLOPPOINT_EXPORT |
26 |
|
|
#endif |
27 |
|
|
|
28 |
|
|
/* --------------------------------------------------------------------- */ |
29 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
30 |
|
|
/* --------------------------------------------------------------------- */ |
31 |
|
|
|
32 |
|
|
#include <dynamic-graph/signal-helper.h> |
33 |
|
|
|
34 |
|
|
#include <sot/core/matrix-geometry.hh> |
35 |
|
|
|
36 |
|
|
#include "pinocchio/spatial/force.hpp" |
37 |
|
|
#include "pinocchio/spatial/motion.hpp" |
38 |
|
|
#include "pinocchio/spatial/se3.hpp" |
39 |
|
|
|
40 |
|
|
namespace dynamicgraph { |
41 |
|
|
namespace sot { |
42 |
|
|
namespace core { |
43 |
|
|
|
44 |
|
|
/* --------------------------------------------------------------------- */ |
45 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
46 |
|
|
/* --------------------------------------------------------------------- */ |
47 |
|
|
|
48 |
|
|
/** |
49 |
|
|
* @brief Admittance controller for an operational point wrt to a force sensor. |
50 |
|
|
* It can be a point of the model (hand) or not (created operational |
51 |
|
|
* point: an object in the hand of the robot) Which is closed to a force sensor |
52 |
|
|
* (for instance the right or left wrist ft) |
53 |
|
|
* |
54 |
|
|
* This entity computes a velocity reference for an operational point based |
55 |
|
|
* on the force error in the world frame : |
56 |
|
|
* w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq) |
57 |
|
|
* |
58 |
|
|
*/ |
59 |
|
|
class ADMITTANCECONTROLOPPOINT_EXPORT AdmittanceControlOpPoint |
60 |
|
|
: public ::dynamicgraph::Entity { |
61 |
|
10 |
DYNAMIC_GRAPH_ENTITY_DECL(); |
62 |
|
|
|
63 |
|
|
public: |
64 |
|
2 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
65 |
|
|
|
66 |
|
|
/* --- CONSTRUCTOR ---- */ |
67 |
|
|
AdmittanceControlOpPoint(const std::string &name); |
68 |
|
|
/** |
69 |
|
|
* @brief Initialize the entity |
70 |
|
|
* |
71 |
|
|
* @param[in] dt Time step of the control |
72 |
|
|
*/ |
73 |
|
|
void init(const double &dt); |
74 |
|
|
|
75 |
|
|
/* --- SIGNALS --- */ |
76 |
|
|
/// \brief Gain (6d) for the integration of the error on the force |
77 |
|
|
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector); |
78 |
|
|
/// \brief Derivative gain (6d) for the error on the force |
79 |
|
|
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector); |
80 |
|
|
/// \brief Value of the saturation to apply on the velocity output |
81 |
|
|
DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector); |
82 |
|
|
/// \brief 6d force given by the sensor in its local frame |
83 |
|
|
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector); |
84 |
|
|
/// \brief 6d desired force of the end-effector in the world frame |
85 |
|
|
DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector); |
86 |
|
|
/// \brief Current position (matrixHomogeneous) of the given operational |
87 |
|
|
/// point |
88 |
|
|
DECLARE_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous); |
89 |
|
|
/// \brief Current position (matrixHomogeneous) of the given force sensor |
90 |
|
|
DECLARE_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous); |
91 |
|
|
|
92 |
|
|
/// \brief 6d force given by the sensor in the world frame |
93 |
|
|
DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector); |
94 |
|
|
/// \brief Internal intergration computed in the world frame |
95 |
|
|
DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector); |
96 |
|
|
|
97 |
|
|
/// \brief Velocity reference for the end-effector in the local frame |
98 |
|
|
DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector); |
99 |
|
|
|
100 |
|
|
/* --- COMMANDS --- */ |
101 |
|
|
/** |
102 |
|
|
* @brief Reset the velocity |
103 |
|
|
*/ |
104 |
|
|
void resetDq(); |
105 |
|
|
|
106 |
|
|
/* --- ENTITY INHERITANCE --- */ |
107 |
|
|
virtual void display(std::ostream &os) const; |
108 |
|
|
|
109 |
|
|
protected: |
110 |
|
|
/// Dimension of the force signals and of the output |
111 |
|
|
int m_n; |
112 |
|
|
/// True if the entity has been successfully initialized |
113 |
|
|
bool m_initSucceeded; |
114 |
|
|
/// Internal state |
115 |
|
|
dynamicgraph::Vector m_w_dq; |
116 |
|
|
/// Time step of the control |
117 |
|
|
double m_dt; |
118 |
|
|
// Weight of the end-effector |
119 |
|
|
double m_mass; |
120 |
|
|
|
121 |
|
|
}; // class AdmittanceControlOpPoint |
122 |
|
|
|
123 |
|
|
} // namespace core |
124 |
|
|
} // namespace sot |
125 |
|
|
} // namespace dynamicgraph |
126 |
|
|
|
127 |
|
|
#endif // #ifndef __sot_core_admittance_control_op_point_H__ |