From 63089b9f72a0b3d4d2da9cdd737772573baca125 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Wed, 11 Apr 2018 16:47:04 +0200
Subject: [PATCH] Specify time dt inside roscontrol-sot for subsampling.
---
.../src/roscontrol-sot-controller.cpp | 38 +++++++++++++++++--
.../src/roscontrol-sot-controller.hh | 8 ++++
.../config/sot_talos_params.yaml | 1 +
.../config/sot_talos_params_effort.yaml | 1 +
.../config/sot_talos_params_gazebo.yaml | 3 +-
.../sot_talos_params_gazebo_effort.yaml | 3 +-
6 files changed, 49 insertions(+), 5 deletions(-)
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
index 97d2cd0..72600af 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
@@ -69,7 +69,8 @@ namespace talos_sot_controller
// -> 124 Mo of data.
type_name_("RCSotController"),
simulation_mode_(false),
- control_mode_(POSITION)
+ control_mode_(POSITION),
+ accumulated_time_(0.0)
{
RESETDEBUG4();
}
@@ -406,6 +407,20 @@ namespace talos_sot_controller
return true;
}
+ bool RCSotController::
+ readParamsdt(ros::NodeHandle &robot_nh)
+ {
+ /// Read /sot_controller/dt to know what is the control period
+ if (robot_nh.hasParam("/sot_controller/dt"))
+ {
+ robot_nh.getParam("/sot_controller/dt",dt_);
+ ROS_INFO_STREAM("dt: " << dt_);
+ return true;
+ }
+ ROS_ERROR("You need to define a control period in param /sot_controller/dt");
+ return false;
+ }
+
bool RCSotController::
readParams(ros::NodeHandle &robot_nh)
{
@@ -433,6 +448,10 @@ namespace talos_sot_controller
// Mapping from ros-controll to sot device
readParamsFromRCToSotDevice(robot_nh);
+ /// Get control perioud
+ if (!readParamsdt(robot_nh))
+ return false;
+
if (control_mode_==EFFORT)
readParamsEffortControlPDMotorControlData(robot_nh);
@@ -713,6 +732,9 @@ namespace talos_sot_controller
void RCSotController::one_iteration()
{
+ // Chrono start
+ RcSotLog.start_it();
+
/// Update the sensors.
fillSensors();
@@ -726,7 +748,10 @@ namespace talos_sot_controller
/// Read the control values
readControl(controlValues_);
-
+
+ // Chrono stop.
+ RcSotLog.stop_it();
+
/// Store everything in Log.
RcSotLog.record(DataOneIter_);
}
@@ -739,7 +764,14 @@ namespace talos_sot_controller
{
try
{
- one_iteration();
+ double periodInSec = period.toSec();
+ if (periodInSec+accumulated_time_>dt_)
+ {
+ one_iteration();
+ accumulated_time_ = 0.0;
+ }
+ else
+ accumulated_time_ += periodInSec;
}
catch (std::exception const &exc)
{
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
index 1104645..96cc4b0 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
@@ -167,6 +167,9 @@ namespace talos_sot_controller
/// * torques
std::map<std::string,std::string> mapFromRCToSotDevice_;
+ /// To be able to subsample control period.
+ double accumulated_time_;
+
public :
RCSotController ();
@@ -231,6 +234,8 @@ namespace talos_sot_controller
/// \brief Read the PID information of the robot in effort mode.
bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
+ /// \brief Read the control period.
+ bool readParamsdt(ros::NodeHandle & robot_nh);
///@}
/// \brief Fill the SoT map structures
@@ -266,6 +271,9 @@ namespace talos_sot_controller
/// Map of control values
std::map<std::string,dgs::ControlValues> controlValues_;
+ /// Control period
+ double dt_;
+
/// \brief Command send to motors
/// Depending on control_mode it can be either
/// position control or torque control.
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params.yaml
index d9894fb..cdd3697 100644
--- a/talos_roscontrol_sot_talos/config/sot_talos_params.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params.yaml
@@ -10,3 +10,4 @@ sot_controller:
joint-angles: joint-angles, velocities: velocities,
torques: torques, cmd-positions: control, cmd-effort: control }
control_mode: POSITION
+ dt: 0.001
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
index 4eaa7e5..afe826b 100644
--- a/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
@@ -139,3 +139,4 @@ sot_controller:
d_gain: 0.0
des_pos:: 0.0
control_mode: EFFORT
+ dt: 0.001
\ No newline at end of file
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml
index bb65ebb..48cb96d 100644
--- a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml
@@ -10,4 +10,5 @@ sot_controller:
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities,
torques: torques, cmd-joints: control, cmd-effort: control }
- control_mode: POSITION
\ No newline at end of file
+ control_mode: POSITION
+ dt: 0.001
\ No newline at end of file
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
index ec5ebe2..d51614b 100644
--- a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
@@ -10,4 +10,5 @@ sot_controller:
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
- control_mode: EFFORT
+ control_mode: EFFORT
+ dt: 0.001
--
GitLab