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__
DECLARE_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector)
base6d_encoders with base6d in RPY
void sendMsg(const std::string &msg, MsgType t=MSG_TYPE_INFO, const char *="", int=0)
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
RobotUtilShrPtr m_robot_util
robot velocities according to SoT convention
pinocchio::Data * m_data
Pinocchio robot model.
pinocchio::Model * m_model
true if the entity has been successfully initialized
pinocchio::SE3 m_w_M_lf
SE3 Transform from center of feet to base.
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector)
n+6 robot velocities
DECLARE_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector)
freeflyer position with angle axis format
DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector)
DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector)
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector)
#define SOTFREEFLYERLOCATOR_EXPORT
to read text file
Definition: treeview.dox:22