| Line |
Branch |
Exec |
Source |
| 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 |
|
|
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__ |
| 128 |
|
|
|