sot-talos-device.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2016,
3  *
4  * Olivier Stasse
5  *
6  * LAAS, CNRS
7  *
8  * This file is part of TalosController.
9  * TalosController is not a free software,
10  * it contains information related to Talos which involves
11  * that you either purchased the proper license to havec access to
12  * those informations, or that you signed the appropriate
13  * Non-Disclosure agreement.
14  *
15  *
16  */
17 
18 #ifndef _SOT_TalosDevice_H_
19 #define _SOT_TalosDevice_H_
20 
21 #include <dynamic-graph/entity.h>
22 #include <dynamic-graph/linear-algebra.h>
23 #include <dynamic-graph/signal-ptr.h>
24 #include <dynamic-graph/signal.h>
25 
26 #include <sot/core/abstract-sot-external-interface.hh>
27 #include <sot/core/device.hh>
28 #include <sot/core/matrix-geometry.hh>
29 
30 namespace dgsot = dynamicgraph::sot;
31 namespace dg = dynamicgraph;
32 
33 class SoTTalosDevice : public dgsot::Device {
34  public:
35  static const std::string CLASS_NAME;
36  static const double TIMESTEP_DEFAULT;
37 
38  virtual const std::string &getClassName() const { return CLASS_NAME; }
39 
40  SoTTalosDevice(std::string RobotName);
41  virtual ~SoTTalosDevice();
42 
43  void setSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
44 
45  void setupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
46 
47  void nominalSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
48 
49  void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
50 
51  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
52 
53  void timeStep(double ts) { timestep_ = ts; }
54 
55  protected:
57  dg::Vector previousState_;
58 
60  std::vector<double> baseff_;
61 
63  dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
65  dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
67  dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
69  dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
71  dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
72 
74  dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
75 
76  dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
77 
79  void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn,
80  int t);
81  void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn,
82  int t);
83  void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn,
84  int t);
86  std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
88  std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
89  void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn,
90  int t);
91 
93  dg::Vector dgforces_;
94  dg::Vector dgRobotState_; // motor-angles
95  dg::Vector joint_angles_; // joint-angles
96  dg::Vector motor_angles_; // motor-angles
97  dg::Vector dgRobotVelocity_; // motor velocities
98  dg::Vector velocities_; // motor velocities
99  dgsot::MatrixRotation pose;
100  dg::Vector accelerometer_;
101  dg::Vector gyrometer_;
102  dg::Vector torques_;
103  dg::Vector currents_;
104  dg::Vector p_gains_;
105  dg::Vector d_gains_;
106 };
107 #endif /* _SOT_TalosDevice_H_*/
Definition: sot-talos-device.hh:33
dynamicgraph::Signal< dg::Vector, int > d_gainsSOUT_
Definition: sot-talos-device.hh:76
void setSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:269
dg::Vector gyrometer_
Definition: sot-talos-device.hh:101
dgsot::MatrixRotation pose
Definition: sot-talos-device.hh:99
dg::Vector dgforces_
Intermediate variables to avoid allocation during control.
Definition: sot-talos-device.hh:93
void setSensorsForce(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Protected methods for internal variables filling.
Definition: sot-talos-device.cpp:122
static const std::string CLASS_NAME
Definition: sot-talos-device.hh:35
void timeStep(double ts)
Definition: sot-talos-device.hh:53
virtual ~SoTTalosDevice()
Definition: sot-talos-device.cpp:120
dg::Vector dgRobotVelocity_
Definition: sot-talos-device.hh:97
void setSensorsIMU(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:145
dg::Vector dgRobotState_
Definition: sot-talos-device.hh:94
void setSensorsEncoders(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:176
dynamicgraph::Signal< dg::Vector, int > currentsSOUT_
motor currents
Definition: sot-talos-device.hh:67
void cleanupSetSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:297
virtual const std::string & getClassName() const
Definition: sot-talos-device.hh:38
dg::Vector d_gains_
Definition: sot-talos-device.hh:105
dg::Vector previousState_
Previous robot configuration.
Definition: sot-talos-device.hh:57
dynamicgraph::Signal< dg::Vector, int > motor_anglesSOUT_
motor angles
Definition: sot-talos-device.hh:71
void setSensorsVelocities(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:207
void setSensorsTorquesCurrents(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:224
SoTTalosDevice(std::string RobotName)
Definition: sot-talos-device.cpp:69
dg::Vector velocities_
Definition: sot-talos-device.hh:98
dynamicgraph::Signal< dg::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
Definition: sot-talos-device.hh:65
void nominalSetSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:292
dg::Vector p_gains_
Definition: sot-talos-device.hh:104
dg::Vector torques_
Definition: sot-talos-device.hh:102
dynamicgraph::Signal< dg::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
Definition: sot-talos-device.hh:63
dg::Vector accelerometer_
Definition: sot-talos-device.hh:100
void setSensorsGains(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:247
dg::Vector motor_angles_
Definition: sot-talos-device.hh:96
dg::Vector joint_angles_
Definition: sot-talos-device.hh:95
void getControl(std::map< std::string, dgsot::ControlValues > &anglesOut)
Definition: sot-talos-device.cpp:302
dynamicgraph::Signal< dg::Vector, int > joint_anglesSOUT_
joint angles
Definition: sot-talos-device.hh:69
std::vector< double > baseff_
Intermediate variables to avoid allocation during control.
Definition: sot-talos-device.hh:60
dynamicgraph::Signal< dg::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
Definition: sot-talos-device.hh:74
static const double TIMESTEP_DEFAULT
Definition: sot-talos-device.hh:36
dg::Vector currents_
Definition: sot-talos-device.hh:103
void setupSetSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:284
Definition: sot-talos-device.cpp:367
Definition: sot-talos-device.cpp:366