sot-torque-control  1.5.2
admittance-controller.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2014, Andrea Del Prete, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_admittance_controller_H__
7 #define __sot_torque_control_admittance_controller_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(sot_admittance_controller_EXPORTS)
15 #define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
16 #else
17 #define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTADMITTANCECONTROLLER_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 
27 #include <map>
28 #include "boost/assign.hpp"
29 
30 #include <tsid/robots/robot-wrapper.hpp>
31 #include <tsid/tasks/task-se3-equality.hpp>
32 
33 /* HELPER */
34 #include <dynamic-graph/signal-helper.h>
35 #include <sot/core/matrix-geometry.hh>
36 #include <sot/core/robot-utils.hh>
38 
39 namespace dynamicgraph {
40 namespace sot {
41 namespace torque_control {
42 /* --------------------------------------------------------------------- */
43 /* --- CLASS ----------------------------------------------------------- */
44 /* --------------------------------------------------------------------- */
45 
46 class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController : public ::dynamicgraph::Entity {
48  DYNAMIC_GRAPH_ENTITY_DECL();
49 
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  /* --- CONSTRUCTOR ---- */
54  AdmittanceController(const std::string& name);
55 
56  void init(const double& dt, const std::string& robotRef);
57 
58  /* --- SIGNALS --- */
59  DECLARE_SIGNAL_IN(encoders, dynamicgraph::Vector);
60  DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
61  DECLARE_SIGNAL_IN(kp_force, dynamicgraph::Vector);
62  DECLARE_SIGNAL_IN(ki_force, dynamicgraph::Vector);
63  DECLARE_SIGNAL_IN(kp_vel, dynamicgraph::Vector);
64  DECLARE_SIGNAL_IN(ki_vel, dynamicgraph::Vector);
65  DECLARE_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector);
66  DECLARE_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector);
67  DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector);
68  DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector);
69  DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector);
70  DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector);
71  DECLARE_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector);
72  DECLARE_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector);
73  DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector);
74  DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector);
75 
76  // DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
77  // DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
78  // DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d estimated force
79  // DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// 6d estimated force
80 
81  DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
82  // DEBUG SIGNALS
83  DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector);
84  DECLARE_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector);
85  DECLARE_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector);
86  // DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
87  // DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
88 
89  /* --- COMMANDS --- */
90  /* --- ENTITY INHERITANCE --- */
91  virtual void display(std::ostream& os) const;
92 
93  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
94  logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
95  }
96 
97  protected:
98  Eigen::VectorXd m_u;
102  double m_dt;
103  int m_nj;
104 
108 
110  tsid::robots::RobotWrapper* m_robot;
111  pinocchio::Data* m_data;
112 
115  tsid::math::Vector m_q_urdf;
116  tsid::math::Vector m_v_urdf;
117 
118  tsid::math::Vector m_dq_des_urdf;
119  tsid::math::Vector m_dqErrIntegral;
120  // tsid::math::Vector m_dqDesIntegral; /// integral of the desired joint velocities
121  tsid::math::Vector m_dq_fd;
122  tsid::math::Vector m_qPrev;
123 
124  typedef pinocchio::Data::Matrix6x Matrix6x;
127  Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
128  Eigen::ColPivHouseholderQR<Matrix6x> m_J_LF_QR;
131 
132  RobotUtilShrPtr m_robot_util;
133 
134  // tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
135  // tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
136  // tsid::math::Vector3 m_zmp_des_LF_local; /// 3d desired zmp left foot expressed in local frame
137  // tsid::math::Vector3 m_zmp_des_RF_local; /// 3d desired zmp left foot expressed in local frame
138  // tsid::math::Vector3 m_zmp_des; /// 3d desired global zmp
139  // tsid::math::Vector3 m_zmp_LF; /// 3d zmp left foot
140  // tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot
141  // tsid::math::Vector3 m_zmp; /// 3d global zmp
142 }; // class AdmittanceController
143 
144 } // namespace torque_control
145 } // namespace sot
146 } // namespace dynamicgraph
147 
148 #endif // #ifndef __sot_torque_control_admittance_controller_H__
dynamicgraph::sot::torque_control::AdmittanceController::m_qPrev
tsid::math::Vector m_qPrev
joint velocities computed with finite differences
Definition: admittance-controller.hh:122
dynamicgraph::sot::torque_control::AdmittanceController::m_J_RF_QR
Eigen::ColPivHouseholderQR< Matrix6x > m_J_RF_QR
Definition: admittance-controller.hh:127
dynamicgraph::sot::torque_control::AdmittanceController::m_f_RF
tsid::math::Vector6 m_f_RF
Definition: admittance-controller.hh:113
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::AdmittanceController::m_q_urdf
tsid::math::Vector m_q_urdf
desired 6d wrench left foot
Definition: admittance-controller.hh:115
dynamicgraph::sot::torque_control::AdmittanceController::m_initSucceeded
bool m_initSucceeded
Definition: admittance-controller.hh:100
vector-conversions.hh
dynamicgraph::sot::torque_control::AdmittanceController::m_nj
int m_nj
control loop time period
Definition: admittance-controller.hh:103
dynamicgraph::sot::torque_control::AdmittanceController::m_robot_util
RobotUtilShrPtr m_robot_util
Definition: admittance-controller.hh:132
dynamicgraph::sot::torque_control::AdmittanceController::m_J_LF
Matrix6x m_J_LF
Definition: admittance-controller.hh:126
dynamicgraph::sot::torque_control::AdmittanceController::m_dq_fd
tsid::math::Vector m_dq_fd
integral of the velocity error
Definition: admittance-controller.hh:121
dynamicgraph::sot::torque_control::AdmittanceController::m_dqErrIntegral
tsid::math::Vector m_dqErrIntegral
Definition: admittance-controller.hh:119
dynamicgraph::sot::torque_control::AdmittanceController::Matrix6x
pinocchio::Data::Matrix6x Matrix6x
previous value of encoders
Definition: admittance-controller.hh:124
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::AdmittanceController::m_J_LF_QR
Eigen::ColPivHouseholderQR< Matrix6x > m_J_LF_QR
Definition: admittance-controller.hh:128
dynamicgraph::sot::torque_control::AdmittanceController::m_dq_des_urdf
tsid::math::Vector m_dq_des_urdf
Definition: admittance-controller.hh:118
dynamicgraph::sot::torque_control::AdmittanceController::m_J_RF
Matrix6x m_J_RF
Definition: admittance-controller.hh:125
dynamicgraph::sot::torque_control::AdmittanceController::m_firstIter
bool m_firstIter
control (i.e. motor currents)
Definition: admittance-controller.hh:99
dynamicgraph::sot::torque_control::AdmittanceController
Definition: admittance-controller.hh:46
dynamicgraph::sot::torque_control::AdmittanceController::m_data
pinocchio::Data * m_data
Definition: admittance-controller.hh:111
dynamicgraph::sot::torque_control::AdmittanceController::m_useJacobianTranspose
bool m_useJacobianTranspose
true if the entity has been successfully initialized
Definition: admittance-controller.hh:101
dynamicgraph::sot::torque_control::AdmittanceController::m_frame_id_rf
int m_frame_id_rf
robot geometric/inertial data
Definition: admittance-controller.hh:106
dynamicgraph::sot::torque_control::AdmittanceController::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: admittance-controller.hh:93
dynamicgraph::sot::torque_control::Vector6
Eigen::Matrix< double, 6, 1 > Vector6
Definition: admittance-controller.cpp:47
dynamicgraph::sot::torque_control::AdmittanceController::m_frame_id_lf
int m_frame_id_lf
frame id of right foot
Definition: admittance-controller.hh:107
dynamicgraph::sot::torque_control::AdmittanceController::m_v_RF_int
tsid::math::Vector6 m_v_RF_int
Definition: admittance-controller.hh:129
dynamicgraph::sot::torque_control::AdmittanceController::m_f_LF
tsid::math::Vector6 m_f_LF
desired 6d wrench right foot
Definition: admittance-controller.hh:114
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::AdmittanceController::m_v_urdf
tsid::math::Vector m_v_urdf
Definition: admittance-controller.hh:116
dynamicgraph::sot::torque_control::AdmittanceController::m_dt
double m_dt
if true it uses the Jacobian transpose rather than the pseudoinverse
Definition: admittance-controller.hh:102
dynamicgraph::sot::torque_control::AdmittanceController::m_robot
tsid::robots::RobotWrapper * m_robot
frame id of left foot
Definition: admittance-controller.hh:110
SOTADMITTANCECONTROLLER_EXPORT
#define SOTADMITTANCECONTROLLER_EXPORT
Definition: admittance-controller.hh:20
dynamicgraph::sot::torque_control::AdmittanceController::m_u
Eigen::VectorXd m_u
Definition: admittance-controller.hh:98
dynamicgraph::sot::torque_control::AdmittanceController::m_v_LF_int
tsid::math::Vector6 m_v_LF_int
Definition: admittance-controller.hh:130