sot-talos-balance  1.7.0
admittance-controller-end-effector.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  *
4  * LAAS-CNRS
5  *
6  * Fanny Risbourg
7  * This file is part of sot-talos-balance.
8  * See license file.
9  */
10 
11 #ifndef __sot_talos_balance_admittance_controller_end_effector_H__
12 #define __sot_talos_balance_admittance_controller_end_effector_H__
13 
14 /* --------------------------------------------------------------------- */
15 /* --- API ------------------------------------------------------------- */
16 /* --------------------------------------------------------------------- */
17 
18 #if defined(WIN32)
19 # if defined(admittance_controller_end_effector_EXPORTS)
20 # define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllexport)
21 # else
22 # define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT __declspec(dllimport)
23 # endif
24 #else
25 # define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT
26 #endif
27 
28 /* --------------------------------------------------------------------- */
29 /* --- INCLUDE --------------------------------------------------------- */
30 /* --------------------------------------------------------------------- */
31 
32 #include <pinocchio/fwd.hpp>
33 #include <dynamic-graph/signal-helper.h>
34 #include <map>
35 #include "boost/assign.hpp"
36 
37 #include <pinocchio/parsers/urdf.hpp>
38 #include <pinocchio/multibody/model.hpp>
39 #include "pinocchio/spatial/se3.hpp"
40 #include "pinocchio/spatial/motion.hpp"
41 #include <sot/core/robot-utils.hh>
42 #include <pinocchio/algorithm/kinematics.hpp>
43 #include <pinocchio/algorithm/frames.hpp>
44 #include <pinocchio/algorithm/center-of-mass.hpp>
45 
46 namespace dynamicgraph
47 {
48 namespace sot
49 {
50 namespace talos_balance
51 {
52 
53 /* --------------------------------------------------------------------- */
54 /* --- CLASS ----------------------------------------------------------- */
55 /* --------------------------------------------------------------------- */
56 
67  : public ::dynamicgraph::Entity
68 {
69  DYNAMIC_GRAPH_ENTITY_DECL();
70 
71 public:
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
74  /* --- CONSTRUCTOR ---- */
75  AdmittanceControllerEndEffector(const std::string &name);
76 
77  /* --- SIGNALS --- */
79  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
81  DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
83  DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
85  DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
87  DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
89  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
90 
92  DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
94  DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
95 
97  DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
98 
99  /* --- COMMANDS --- */
108  void init(const double &dt, const std::string &sensorFrameName,
109  const std::string &endeffectorName);
110 
114  void resetDq();
115 
116  /* --- ENTITY INHERITANCE --- */
117  virtual void display(std::ostream &os) const;
118 
119 protected:
121  int m_n;
127  double m_dt;
128  // Weight of the end-effector
129  double m_mass;
130 
132  RobotUtilShrPtr m_robot_util;
134  pinocchio::Model m_model;
136  pinocchio::Data *m_data;
138  pinocchio::FrameIndex m_sensorFrameId;
140  pinocchio::JointIndex m_endEffectorId;
141 
142 }; // class AdmittanceControllerEndEffector
143 
144 } // namespace talos_balance
145 } // namespace sot
146 } // namespace dynamicgraph
147 
148 #endif // #ifndef __sot_talos_balance_admittance_controller_end_effector_H__
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
RobotUtilShrPtr m_robot_util
Robot Util instance to get the sensor frame.
#define ADMITTANCECONTROLLERENDEFFECTOR_EXPORT
bool m_initSucceeded
True if the entity has been successfully initialized.
Admittance controller for an upper body end-effector (right or left wrist)
pinocchio::JointIndex m_endEffectorId
Id of the joint of the end-effector.