Commit f75ef1ab authored by Olivier Stasse's avatar Olivier Stasse

Specify time dt inside roscontrol-sot for subsampling.

parent e430bcc7
......@@ -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)
{
......
......@@ -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.
......
......@@ -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
......@@ -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
......@@ -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
......@@ -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
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment