sot-talos-balance  1.5.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 <dynamic-graph/signal-helper.h>
33 #include <map>
34 #include "boost/assign.hpp"
35 
36 #include <pinocchio/parsers/urdf.hpp>
37 #include <pinocchio/multibody/model.hpp>
38 #include "pinocchio/spatial/se3.hpp"
39 #include "pinocchio/spatial/motion.hpp"
40 #include <sot/core/robot-utils.hh>
41 #include <pinocchio/algorithm/kinematics.hpp>
42 #include <pinocchio/algorithm/frames.hpp>
43 #include <pinocchio/algorithm/center-of-mass.hpp>
44 
45 namespace dynamicgraph
46 {
47 namespace sot
48 {
49 namespace talos_balance
50 {
51 
52 /* --------------------------------------------------------------------- */
53 /* --- CLASS ----------------------------------------------------------- */
54 /* --------------------------------------------------------------------- */
55 
66  : public ::dynamicgraph::Entity
67 {
68  DYNAMIC_GRAPH_ENTITY_DECL();
69 
70 public:
71  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 
73  /* --- CONSTRUCTOR ---- */
74  AdmittanceControllerEndEffector(const std::string &name);
75 
76  /* --- SIGNALS --- */
78  DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
80  DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
82  DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
84  DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
86  DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
88  DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
89 
91  DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
93  DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
94 
96  DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
97 
98  /* --- COMMANDS --- */
107  void init(const double &dt, const std::string &sensorFrameName,
108  const std::string &endeffectorName);
109 
113  void resetDq();
114 
115  /* --- ENTITY INHERITANCE --- */
116  virtual void display(std::ostream &os) const;
117 
118 protected:
120  int m_n;
126  double m_dt;
127  // Weight of the end-effector
128  double m_mass;
129 
131  RobotUtilShrPtr m_robot_util;
133  pinocchio::Model m_model;
135  pinocchio::Data *m_data;
137  pinocchio::FrameIndex m_sensorFrameId;
139  pinocchio::JointIndex m_endEffectorId;
140 
141 }; // class AdmittanceControllerEndEffector
142 
143 } // namespace talos_balance
144 } // namespace sot
145 } // namespace dynamicgraph
146 
147 #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.