sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
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
28#include "boost/assign.hpp"
29
30/* Pinocchio */
31#include <pinocchio/algorithm/kinematics.hpp>
32#include <pinocchio/multibody/model.hpp>
33#include <pinocchio/parsers/urdf.hpp>
34//~ #include <pinocchio/algorithm/rnea.hpp>
35//~ #include "pinocchio/algorithm/crba.hpp"
36
37#include <dynamic-graph/linear-algebra.h>
38/* HELPER */
39#include <dynamic-graph/signal-helper.h>
40
41#include <sot/core/matrix-geometry.hh>
42#include <sot/core/robot-utils.hh>
44
45namespace dynamicgraph {
46namespace sot {
47namespace torque_control {
48
49/* --------------------------------------------------------------------- */
50/* --- CLASS ----------------------------------------------------------- */
51/* --------------------------------------------------------------------- */
52
54 : public ::dynamicgraph::Entity {
56 DYNAMIC_GRAPH_ENTITY_DECL();
57
58 public:
59 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60
61 /* --- CONSTRUCTOR ---- */
62 FreeFlyerLocator(const std::string& name);
64
65 void init(const std::string& robotRef);
66
67 /* --- SIGNALS --- */
68 DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
69 DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
70 DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
72 DECLARE_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector);
74 DECLARE_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector);
76 DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
77
78 /* --- COMMANDS --- */
79 void displayRobotUtil();
80
81 /* --- ENTITY INHERITANCE --- */
82 virtual void display(std::ostream& os) const;
83
84 void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO,
85 const char* = "", int = 0) {
86 logger_.stream(t) << ("[FreeFlyerLocator-" + name + "] " + msg) << '\n';
87 }
88
89 protected:
90 bool
92 pinocchio::Model* m_model;
93 pinocchio::Data* m_data;
94 pinocchio::SE3 m_Mff;
95 pinocchio::SE3 m_w_M_lf;
96 pinocchio::SE3 m_w_M_rf;
97 long unsigned int m_right_foot_id;
98 long unsigned int m_left_foot_id;
99 Eigen::VectorXd
101 Eigen::VectorXd m_q_sot;
102 Eigen::VectorXd
104 Eigen::VectorXd m_v_sot;
105
106 RobotUtilShrPtr m_robot_util;
107
108}; // class FreeFlyerLocator
109
110} // namespace torque_control
111} // namespace sot
112} // namespace dynamicgraph
113
114#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