roscontrol-sot-controller.hh 9.45 KB
Newer Older
Olivier Stasse's avatar
Olivier Stasse committed
1
2
3
4
5
6
7
8
9
10
/*
 * Author: Olivier STASSE
 */

#ifndef RC_SOT_CONTROLLER_H
#define RC_SOT_CONTROLLER_H

#include <string>
#include <map>

11
#pragma GCC diagnostic push
12
#pragma GCC system_header
Olivier Stasse's avatar
Olivier Stasse committed
13
14
15
16
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/imu_sensor_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
17
#include <pal_hardware_interfaces/actuator_temperature_interface.h>
18
#include <pluginlib/class_list_macros.h>
19
#pragma GCC diagnostic pop
Olivier Stasse's avatar
Olivier Stasse committed
20
21

#include <dynamic_graph_bridge/sot_loader_basic.hh>
22
23
#include <ros/ros.h>
#include <control_toolbox/pid.h>
Olivier Stasse's avatar
Olivier Stasse committed
24

25
26
27
28
/** URDF DOM*/
#include <urdf_parser/urdf_parser.h>

/* Local header */
Olivier Stasse's avatar
Olivier Stasse committed
29
30
#include "log.hh"

Guilhem Saurel's avatar
Guilhem Saurel committed
31
namespace sot_controller
Olivier Stasse's avatar
Olivier Stasse committed
32
{
33
34
35
  enum ControlMode { POSITION, VELOCITY, EFFORT};
  namespace lhi = hardware_interface;
  namespace lci = controller_interface;
36

37
38
39
40
41
42
43
  class XmlrpcHelperException : public ros::Exception
  {
  public:
    XmlrpcHelperException(const std::string& what)
      : ros::Exception(what) {}
  };

Guilhem Saurel's avatar
Guilhem Saurel committed
44

45
  struct ControlPDMotorControlData
46
47
48
  {
    control_toolbox::Pid pid_controller;

49
    ControlPDMotorControlData();
50
51
52
53
    //    void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
    void read_from_xmlrpc_value(const std::string &prefix);
  };

54
55
56
57
  struct JointSotHandle
  {
    lhi::JointHandle joint;
    double desired_init_pose;
Joseph Mirabel's avatar
Joseph Mirabel committed
58
59
60
61
    // This should not be handled in roscontrol_sot package. The control type
    // should be handled in SoT directly, by externalizing the integration from
    // the Device.
    //ControlMode sot_control_mode;
62
63
64
65
    ControlMode ros_control_mode;
  };

  typedef std::map<std::string,JointSotHandle>::iterator it_joint_sot_h;
66
67
#ifndef CONTROLLER_INTERFACE_KINETIC
  typedef std::set<std::string> ClaimedResources;
Guilhem Saurel's avatar
Guilhem Saurel committed
68
#endif
69
70
  /**
     This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
Guilhem Saurel's avatar
Guilhem Saurel committed
71

72
   */
73
  class RCSotController : public lci::ControllerBase,
Olivier Stasse's avatar
Olivier Stasse committed
74
75
			       SotLoaderBasic
  {
Guilhem Saurel's avatar
Guilhem Saurel committed
76

Olivier Stasse's avatar
Olivier Stasse committed
77
78
79
  protected:
    /// Robot nb dofs.
    size_t nbDofs_;
Guilhem Saurel's avatar
Guilhem Saurel committed
80

Olivier Stasse's avatar
Olivier Stasse committed
81
82
83
    /// Data log.
    rc_sot_system::DataToLog DataOneIter_;
  private:
Guilhem Saurel's avatar
Guilhem Saurel committed
84

Olivier Stasse's avatar
Olivier Stasse committed
85
    /// @{ \name Ros-control related fields
Guilhem Saurel's avatar
Guilhem Saurel committed
86

Olivier Stasse's avatar
Olivier Stasse committed
87
    /// \brief Vector of joint handles.
88
    std::map<std::string,JointSotHandle> joints_;
Olivier Stasse's avatar
Olivier Stasse committed
89
90
    std::vector<std::string> joints_name_;

91
    /// \brief Vector towards the IMU.
92
    std::vector<lhi::ImuSensorHandle> imu_sensor_;
Olivier Stasse's avatar
Olivier Stasse committed
93

94
    /// \brief Vector of 6D force sensor.
95
    std::vector<lhi::ForceTorqueSensorHandle> ft_sensors_;
Guilhem Saurel's avatar
Guilhem Saurel committed
96

97
#ifdef TEMPERATURE_SENSOR_CONTROLLER
98
    /// \brief Vector of temperature sensors for the actuators.
Guilhem Saurel's avatar
Guilhem Saurel committed
99
    std::vector<lhi::ActuatorTemperatureSensorHandle>
100
    act_temp_sensors_;
101
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
102

Olivier Stasse's avatar
Olivier Stasse committed
103
    /// \brief Interface to the joints controlled in position.
104
    lhi::PositionJointInterface * pos_iface_;
Olivier Stasse's avatar
Olivier Stasse committed
105

Joseph Mirabel's avatar
Joseph Mirabel committed
106
107
108
    /// \brief Interface to the joints controlled in position.
    lhi::VelocityJointInterface * vel_iface_;

Olivier Stasse's avatar
Olivier Stasse committed
109
    /// \brief Interface to the joints controlled in force.
110
    lhi::EffortJointInterface * effort_iface_;
Guilhem Saurel's avatar
Guilhem Saurel committed
111

112
    /// \brief Interface to the sensors (IMU).
113
    lhi::ImuSensorInterface* imu_iface_;
Olivier Stasse's avatar
Olivier Stasse committed
114

115
    /// \brief Interface to the sensors (Force).
116
    lhi::ForceTorqueSensorInterface* ft_iface_;
Guilhem Saurel's avatar
Guilhem Saurel committed
117
118

#ifdef TEMPERATURE_SENSOR_CONTROLLER
119
120
    /// \brief Interface to the actuator temperature sensor.
    lhi::ActuatorTemperatureSensorInterface  * act_temp_iface_;
121
#endif
Olivier Stasse's avatar
Olivier Stasse committed
122
123
124
    /// @}

    /// \brief Log
125
    rc_sot_system::Log RcSotLog_;
Olivier Stasse's avatar
Olivier Stasse committed
126
    /// @}
Guilhem Saurel's avatar
Guilhem Saurel committed
127

Olivier Stasse's avatar
Olivier Stasse committed
128
129
130
131
132
    const std::string type_name_;

    /// \brief Adapt the interface to Gazebo simulation
    bool simulation_mode_;

133
134
    /// \brief Implement a PD controller for the robot when the dynamic graph
    /// is not on.
135
    std::map<std::string, ControlPDMotorControlData> effort_mode_pd_motors_;
136

Joseph Mirabel's avatar
Joseph Mirabel committed
137
138
139
140
    /// \brief Implement a PD controller for the robot when the dynamic graph
    /// is not on.
    std::map<std::string, ControlPDMotorControlData> velocity_mode_pd_motors_;

Olivier Stasse's avatar
Olivier Stasse committed
141
142
143
144
145
146
147
148
149
    /// \brief Map from ros-control quantities to robot device
    /// ros-control quantities are for the sensors:
    /// * motor-angles
    /// * joint-angles
    /// * velocities
    /// * torques
    /// ros-control quantities for control are:
    /// * joints
    /// * torques
150
    std::map<std::string,std::string> mapFromRCToSotDevice_;
Olivier Stasse's avatar
Olivier Stasse committed
151

152
153
    /// To be able to subsample control period.
    double accumulated_time_;
154
155
156

    /// Jitter for the subsampling.
    double jitter_;
157

158
159
    /// \brief Verbosity level for ROS messages during initRequest/initialization phase.
    /// 0: no messages or error 1: info 2: debug
160
    int verbosity_level_;
161

162
    /// URDF model of the robot.
Olivier Stasse's avatar
Olivier Stasse committed
163
164
165
166
    urdf::ModelInterfaceSharedPtr modelURDF_;

    /// Profile log
    rc_sot_system::ProfileLog profileLog_;
167

Olivier Stasse's avatar
Olivier Stasse committed
168
169
170
  public :

    RCSotController ();
171

Guilhem Saurel's avatar
Guilhem Saurel committed
172
    /// \brief Read the configuration files,
173
    /// claims the request to the robot and initialize the Stack-Of-Tasks.
Guilhem Saurel's avatar
Guilhem Saurel committed
174
    bool initRequest (lhi::RobotHW * robot_hw,
Olivier Stasse's avatar
Olivier Stasse committed
175
176
		      ros::NodeHandle &robot_nh,
		      ros::NodeHandle &controller_nh,
177
		      ClaimedResources & claimed_resources);
178

179
    /// \brief Display claimed resources
180
    void displayClaimedResources(ClaimedResources & claimed_resources);
181

182
    /// \brief Claims
Olivier Stasse's avatar
Olivier Stasse committed
183
    bool init();
184
185

    /// \brief Read the sensor values, calls the control graph, and apply the control.
Guilhem Saurel's avatar
Guilhem Saurel committed
186
    ///
Olivier Stasse's avatar
Olivier Stasse committed
187
    void update(const ros::Time&, const ros::Duration& );
188
    /// \brief Starting by filling the sensors.
Olivier Stasse's avatar
Olivier Stasse committed
189
    void starting(const ros::Time&);
190
    /// \brief Stopping the control
Olivier Stasse's avatar
Olivier Stasse committed
191
192
193
194
    void stopping(const ros::Time&);

  protected:
    /// Initialize the roscontrol interfaces
195
    bool initInterfaces(lhi::RobotHW * robot_hw,
Olivier Stasse's avatar
Olivier Stasse committed
196
197
			ros::NodeHandle &,
			ros::NodeHandle &,
198
			ClaimedResources & claimed_resources);
199
200

    /// Initialize the hardware interface using the joints.
Olivier Stasse's avatar
Olivier Stasse committed
201
    bool initJoints();
202
    /// Initialize the hardware interface accessing the IMU.
Olivier Stasse's avatar
Olivier Stasse committed
203
    bool initIMU();
204
    /// Initialize the hardware interface accessing the force sensors.
Olivier Stasse's avatar
Olivier Stasse committed
205
    bool initForceSensors();
206
207
    /// Initialize the hardware interface accessing the temperature sensors.
    bool initTemperatureSensors();
Guilhem Saurel's avatar
Guilhem Saurel committed
208

209
210
211
    /// Initialize internal structure for the logs based on nbDofs
    /// number of force sensors and size of the buffer.
    void initLogs();
Guilhem Saurel's avatar
Guilhem Saurel committed
212

Olivier Stasse's avatar
Olivier Stasse committed
213
214
215
216
217
218
219
220
221
222
223
224
225
226
    ///@{ \name Read the parameter server
    /// \brief Entry point
    bool readParams(ros::NodeHandle &robot_nh);

    /// \brief Creates the list of joint names.
    bool readParamsJointNames(ros::NodeHandle &robot_nh);

    /// \brief Set the SoT library name.
    bool readParamsSotLibName(ros::NodeHandle &robot_nh);

    /// \Brief Set the mapping between ros-control and the robot device
    /// For instance the yaml file should have a line with map_rc_to_sot_device:
    ///   map_rc_to_sot_device: [ ]
    bool readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh);
Guilhem Saurel's avatar
Guilhem Saurel committed
227

Olivier Stasse's avatar
Olivier Stasse committed
228
229
230
    /// \brief Read the control mode.
    bool readParamsControlMode(ros::NodeHandle & robot_nh);

Joseph Mirabel's avatar
Joseph Mirabel committed
231
232
233
    /// \brief Read the PID information of the robot in velocity mode.
    bool readParamsVelocityControlPDMotorControlData(ros::NodeHandle &robot_nh);

234
235
236
    /// \brief Read the PID information of the robot in effort mode.
    bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);

237
238
239
    /// \brief Read the desired initial pose of the robot in position mode.
    bool readParamsPositionControlData(ros::NodeHandle &robot_nh);

240
241
    /// \brief Read the control period.
    bool readParamsdt(ros::NodeHandle & robot_nh);
242
243
244

    /// \brief Read verbosity level to display messages mostly during initialization
    void readParamsVerbosityLevel(ros::NodeHandle &robot_nh);
Olivier Stasse's avatar
Olivier Stasse committed
245
246
247
248
249
250
251
    ///@}

    /// \brief Fill the SoT map structures
    void fillSensorsIn(std::string &title, std::vector<double> & data);

    /// \brief Get the information from the low level and calls fillSensorsIn.
    void fillJoints();
Guilhem Saurel's avatar
Guilhem Saurel committed
252

Olivier Stasse's avatar
Olivier Stasse committed
253
254
255
256
257
258
    /// In the map sensorsIn_ creates the key "name_IMUNb"
    /// and associate to this key the vector data.
    void setSensorsImu(std::string &name,
		       int IMUNb,
		       std::vector<double> &data);

Guilhem Saurel's avatar
Guilhem Saurel committed
259
    /// @{ \name Fill the sensors
Olivier Stasse's avatar
Olivier Stasse committed
260
261
262
263
    /// Read the imus and set the interface to the SoT.
    void fillImu();
    /// Read the force sensors
    void fillForceSensors();
264
265
    /// Read the temperature sensors
    void fillTempSensors();
Olivier Stasse's avatar
Olivier Stasse committed
266
267
268
    /// Entry point for reading all the sensors .
    void fillSensors();
    ///@}
Guilhem Saurel's avatar
Guilhem Saurel committed
269

270
271
272
    ///@{ Control the robot while waiting for the SoT
    /// Default control in effort.
    void localStandbyEffortControlMode(const ros::Duration& period);
Joseph Mirabel's avatar
Joseph Mirabel committed
273
274
    /// Default control in velocity.
    void localStandbyVelocityControlMode(const ros::Duration& period);
275
276
    /// Default control in position.
    void localStandbyPositionControlMode();
Guilhem Saurel's avatar
Guilhem Saurel committed
277

278
    ///@}
Olivier Stasse's avatar
Olivier Stasse committed
279
280
281
282
283
284
285
286
287
    /// Extract control values to send to the simulator.
    void readControl(std::map<std::string,dgs::ControlValues> &controlValues);

    /// Map of sensor readings
    std::map <std::string,dgs::SensorValues> sensorsIn_;

    /// Map of control values
    std::map<std::string,dgs::ControlValues> controlValues_;

288
289
    /// Control period
    double dt_;
Guilhem Saurel's avatar
Guilhem Saurel committed
290

Olivier Stasse's avatar
Olivier Stasse committed
291
292
293
294
    /// \brief Command send to motors
    /// Depending on control_mode it can be either
    /// position control or torque control.
    std::vector<double> command_;
Guilhem Saurel's avatar
Guilhem Saurel committed
295

Olivier Stasse's avatar
Olivier Stasse committed
296
297
298
    /// One iteration: read sensor, compute the control law, apply control.
    void one_iteration();

299
300
    /// Read URDF model from /robot_description parameter.
    bool readUrdf(ros::NodeHandle &robot_nh);
301
302
303
304
305
306
307

    /// Returns control mode by reading rosparam.
    /// It reads /sot_controller/control_mode/joint_name
    /// and check 
    bool
    getJointControlMode(std::string &joint_name,
			JointSotHandle &aJointSotHandle);
Olivier Stasse's avatar
Olivier Stasse committed
308
309
310
311
  };
}

#endif /* RC_SOT_CONTROLLER_H */