sot-torque-control  1.6.2
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
free-flyer-locator.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, Thomas Flayols, LAAS-CNRS
3  *
4  */
5 
6 #ifndef __sot_torque_control_free_flyer_locator_H__
7 #define __sot_torque_control_free_flyer_locator_H__
8 
9 /* --------------------------------------------------------------------- */
10 /* --- API ------------------------------------------------------------- */
11 /* --------------------------------------------------------------------- */
12 
13 #if defined(WIN32)
14 #if defined(free_flyer_locator_EXPORTS)
15 #define SOTFREEFLYERLOCATOR_EXPORT __declspec(dllexport)
16 #else
17 #define SOTFREEFLYERLOCATOR_EXPORT __declspec(dllimport)
18 #endif
19 #else
20 #define SOTFREEFLYERLOCATOR_EXPORT
21 #endif
22 
23 /* --------------------------------------------------------------------- */
24 /* --- INCLUDE --------------------------------------------------------- */
25 /* --------------------------------------------------------------------- */
26 #include <map>
27 #include "boost/assign.hpp"
28 
29 /* Pinocchio */
30 #include <pinocchio/multibody/model.hpp>
31 #include <pinocchio/parsers/urdf.hpp>
32 #include <pinocchio/algorithm/kinematics.hpp>
33 //~ #include <pinocchio/algorithm/rnea.hpp>
34 //~ #include "pinocchio/algorithm/crba.hpp"
35 
36 #include <dynamic-graph/linear-algebra.h>
37 /* HELPER */
38 #include <dynamic-graph/signal-helper.h>
39 #include <sot/core/matrix-geometry.hh>
40 #include <sot/core/robot-utils.hh>
42 
43 namespace dynamicgraph {
44 namespace sot {
45 namespace torque_control {
46 
47 /* --------------------------------------------------------------------- */
48 /* --- CLASS ----------------------------------------------------------- */
49 /* --------------------------------------------------------------------- */
50 
51 class SOTFREEFLYERLOCATOR_EXPORT FreeFlyerLocator : public ::dynamicgraph::Entity {
53  DYNAMIC_GRAPH_ENTITY_DECL();
54 
55  public:
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  /* --- CONSTRUCTOR ---- */
59  FreeFlyerLocator(const std::string& name);
61 
62  void init(const std::string& robotRef);
63 
64  /* --- SIGNALS --- */
65  DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
66  DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
67  DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
69  DECLARE_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector);
71  DECLARE_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector);
73  DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
74 
75  /* --- COMMANDS --- */
76  void displayRobotUtil();
77 
78  /* --- ENTITY INHERITANCE --- */
79  virtual void display(std::ostream& os) const;
80 
81  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
82  logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n';
83  }
84 
85  protected:
87  pinocchio::Model* m_model;
88  pinocchio::Data* m_data;
89  pinocchio::SE3 m_Mff;
90  pinocchio::SE3 m_w_M_lf;
91  pinocchio::SE3 m_w_M_rf;
92  long unsigned int m_right_foot_id;
93  long unsigned int m_left_foot_id;
94  Eigen::VectorXd m_q_pin;
95  Eigen::VectorXd m_q_sot;
96  Eigen::VectorXd m_v_pin;
97  Eigen::VectorXd m_v_sot;
98 
99  RobotUtilShrPtr m_robot_util;
100 
101 }; // class FreeFlyerLocator
102 
103 } // namespace torque_control
104 } // namespace sot
105 } // namespace dynamicgraph
106 
107 #endif // #ifndef __sot_torque_control_free_flyer_locator_H__
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_v_sot
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Definition: free-flyer-locator.hh:97
dynamicgraph
to read text file
Definition: treeview.dox:22
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_q_pin
Eigen::VectorXd m_q_pin
Definition: free-flyer-locator.hh:94
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_w_M_rf
pinocchio::SE3 m_w_M_rf
Definition: free-flyer-locator.hh:91
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_v_pin
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
Definition: free-flyer-locator.hh:96
vector-conversions.hh
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_model
pinocchio::Model * m_model
true if the entity has been successfully initialized
Definition: free-flyer-locator.hh:87
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_data
pinocchio::Data * m_data
Pinocchio robot model.
Definition: free-flyer-locator.hh:88
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_w_M_lf
pinocchio::SE3 m_w_M_lf
SE3 Transform from center of feet to base.
Definition: free-flyer-locator.hh:90
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_robot_util
RobotUtilShrPtr m_robot_util
robot velocities according to SoT convention
Definition: free-flyer-locator.hh:99
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_Mff
pinocchio::SE3 m_Mff
Pinocchio robot data.
Definition: free-flyer-locator.hh:89
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_right_foot_id
long unsigned int m_right_foot_id
Definition: free-flyer-locator.hh:92
torque_control
Definition: __init__.py:1
dynamicgraph::sot::torque_control::FreeFlyerLocator::sendMsg
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Definition: free-flyer-locator.hh:81
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_left_foot_id
long unsigned int m_left_foot_id
Definition: free-flyer-locator.hh:93
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_q_sot
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
Definition: free-flyer-locator.hh:95
dynamicgraph::sot::torque_control::EntityClassName
AdmittanceController EntityClassName
Definition: admittance-controller.cpp:44
dynamicgraph::sot::torque_control::FreeFlyerLocator
Definition: free-flyer-locator.hh:51
SOTFREEFLYERLOCATOR_EXPORT
#define SOTFREEFLYERLOCATOR_EXPORT
Definition: free-flyer-locator.hh:20
dynamicgraph::sot::torque_control::FreeFlyerLocator::m_initSucceeded
bool m_initSucceeded
Definition: free-flyer-locator.hh:86