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/signal.h>
23 #include <dynamic-graph/signal-ptr.h>
24 #include <dynamic-graph/linear-algebra.h>
25 #include <sot/core/device.hh>
26 #include <sot/core/abstract-sot-external-interface.hh>
27 #include <sot/core/matrix-geometry.hh>
28 
29 namespace dgsot = dynamicgraph::sot;
30 namespace dg = dynamicgraph;
31 
32 class SoTTalosDevice : public dgsot::Device {
33  public:
34  static const std::string CLASS_NAME;
35  static const double TIMESTEP_DEFAULT;
36 
37  virtual const std::string &getClassName() const { return CLASS_NAME; }
38 
39  SoTTalosDevice(std::string RobotName);
40  virtual ~SoTTalosDevice();
41 
42  void setSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
43 
44  void setupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
45 
46  void nominalSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
47 
48  void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
49 
50  void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
51 
52  void timeStep(double ts) { timestep_ = ts; }
53 
54  protected:
56  dg::Vector previousState_;
57 
59  std::vector<double> baseff_;
60 
62  dynamicgraph::Signal<dg::Vector, int> accelerometerSOUT_;
64  dynamicgraph::Signal<dg::Vector, int> gyrometerSOUT_;
66  dynamicgraph::Signal<dg::Vector, int> currentsSOUT_;
68  dynamicgraph::Signal<dg::Vector, int> joint_anglesSOUT_;
70  dynamicgraph::Signal<dg::Vector, int> motor_anglesSOUT_;
71 
73  dynamicgraph::Signal<dg::Vector, int> p_gainsSOUT_;
74 
75  dynamicgraph::Signal<dg::Vector, int> d_gainsSOUT_;
76 
78  void setSensorsForce(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
79  void setSensorsIMU(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
80  void setSensorsEncoders(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
81  void setSensorsVelocities(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
82  void setSensorsTorquesCurrents(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
83  void setSensorsGains(std::map<std::string, dgsot::SensorValues> &SensorsIn, int t);
84 
86  dg::Vector dgforces_;
87  dg::Vector dgRobotState_; // motor-angles
88  dg::Vector joint_angles_; // joint-angles
89  dg::Vector motor_angles_; // motor-angles
90  dg::Vector dgRobotVelocity_; // motor velocities
91  dg::Vector velocities_; // motor velocities
92  dgsot::MatrixRotation pose;
93  dg::Vector accelerometer_;
94  dg::Vector gyrometer_;
95  dg::Vector torques_;
96  dg::Vector currents_;
97  dg::Vector p_gains_;
98  dg::Vector d_gains_;
99 };
100 #endif /* _SOT_TalosDevice_H_*/
Definition: sot-talos-device.hh:32
dynamicgraph::Signal< dg::Vector, int > d_gainsSOUT_
Definition: sot-talos-device.hh:75
void setSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:253
dg::Vector gyrometer_
Definition: sot-talos-device.hh:94
dgsot::MatrixRotation pose
Definition: sot-talos-device.hh:92
dg::Vector dgforces_
Intermediate variables to avoid allocation during control.
Definition: sot-talos-device.hh:86
void setSensorsForce(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Protected methods for internal variables filling.
Definition: sot-talos-device.cpp:115
static const std::string CLASS_NAME
Definition: sot-talos-device.hh:34
void timeStep(double ts)
Definition: sot-talos-device.hh:52
virtual ~SoTTalosDevice()
Definition: sot-talos-device.cpp:113
dg::Vector dgRobotVelocity_
Definition: sot-talos-device.hh:90
void setSensorsIMU(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:137
dg::Vector dgRobotState_
Definition: sot-talos-device.hh:87
void setSensorsEncoders(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:166
dynamicgraph::Signal< dg::Vector, int > currentsSOUT_
motor currents
Definition: sot-talos-device.hh:66
void cleanupSetSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:272
virtual const std::string & getClassName() const
Definition: sot-talos-device.hh:37
dg::Vector d_gains_
Definition: sot-talos-device.hh:98
dg::Vector previousState_
Previous robot configuration.
Definition: sot-talos-device.hh:56
dynamicgraph::Signal< dg::Vector, int > motor_anglesSOUT_
motor angles
Definition: sot-talos-device.hh:70
void setSensorsVelocities(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:195
void setSensorsTorquesCurrents(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:211
SoTTalosDevice(std::string RobotName)
Definition: sot-talos-device.cpp:67
dg::Vector velocities_
Definition: sot-talos-device.hh:91
dynamicgraph::Signal< dg::Vector, int > gyrometerSOUT_
Rotation velocity measured by gyrometers.
Definition: sot-talos-device.hh:64
void nominalSetSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:270
dg::Vector p_gains_
Definition: sot-talos-device.hh:97
dg::Vector torques_
Definition: sot-talos-device.hh:95
dynamicgraph::Signal< dg::Vector, int > accelerometerSOUT_
Accelerations measured by accelerometers.
Definition: sot-talos-device.hh:62
dg::Vector accelerometer_
Definition: sot-talos-device.hh:93
void setSensorsGains(std::map< std::string, dgsot::SensorValues > &SensorsIn, int t)
Definition: sot-talos-device.cpp:232
dg::Vector motor_angles_
Definition: sot-talos-device.hh:89
dg::Vector joint_angles_
Definition: sot-talos-device.hh:88
void getControl(std::map< std::string, dgsot::ControlValues > &anglesOut)
Definition: sot-talos-device.cpp:274
dynamicgraph::Signal< dg::Vector, int > joint_anglesSOUT_
joint angles
Definition: sot-talos-device.hh:68
std::vector< double > baseff_
Intermediate variables to avoid allocation during control.
Definition: sot-talos-device.hh:59
dynamicgraph::Signal< dg::Vector, int > p_gainsSOUT_
proportional and derivative position-control gains
Definition: sot-talos-device.hh:73
static const double TIMESTEP_DEFAULT
Definition: sot-talos-device.hh:35
dg::Vector currents_
Definition: sot-talos-device.hh:96
void setupSetSensors(std::map< std::string, dgsot::SensorValues > &sensorsIn)
Definition: sot-talos-device.cpp:268
Definition: sot-talos-device.cpp:333
Definition: sot-talos-device.cpp:332