From 6b5b10eda44b4077baa7398850fa28cab92e72ba Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Wed, 2 Jun 2021 18:49:55 +0200 Subject: [PATCH 1/4] Add flipper torque limit control. --- .../src/flipper_control_plugin.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp index 85a9f747..05ec78e3 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp @@ -6,6 +6,7 @@ #include #include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointEffortLimitsCmd.hh" #include "ignition/gazebo/components/JointVelocityCmd.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/Model.hh" @@ -207,14 +208,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys protected: void UpdateMaxTorque(const double maxTorqueCmd, EntityComponentManager& _ecm) { const auto torque = math::clamp(maxTorqueCmd, 0.0, this->maxTorque); - // TODO max effort cannot be changed during run time, waiting for resolution of - // https://github.com/ignitionrobotics/ign-physics/issues/96 - static bool informed{false}; - if (!informed) - { - ignwarn << "FlipperControlPlugin: Max torque commands are not yet supported." << std::endl; - informed = true; - } + _ecm.SetComponentData(this->joint, {{-torque, torque}}); } public: void OnCmdTorque(const msgs::Double &_msg) From e0b49d65a21ae6a7c4684c46661af6a5e3ee5dd0 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 15 Nov 2021 22:38:36 +0100 Subject: [PATCH 2/4] Improve topic parsing logic. --- .../src/flipper_control_plugin.cpp | 33 ++++++++----------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp index 05ec78e3..1e3e0c40 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp @@ -115,25 +115,20 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys _sdf->Get( "position_correction_tolerance", this->positionCorrectionTolerance.Radian()).first); - std::string topicTorque {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_max_torque"}; - if (_sdf->HasElement("topic_max_torque")) - topicTorque = _sdf->Get("topic_max_torque"); - this->node.Subscribe(topicTorque, &FlipperControlPlugin::OnCmdTorque, this); - - std::string topicVel {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_vel"}; - if (_sdf->HasElement("topic_vel")) - topicVel = _sdf->Get("topic_vel"); - this->node.Subscribe(topicVel, &FlipperControlPlugin::OnCmdVel, this); - - std::string topicPosAbs {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos"}; - if (_sdf->HasElement("topic_pos_abs")) - topicPosAbs = _sdf->Get("topic_pos_abs"); - this->node.Subscribe(topicPosAbs, &FlipperControlPlugin::OnCmdPosAbs, this); - - std::string topicPosRel {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos_rel"}; - if (_sdf->HasElement("topic_pos_rel")) - topicPosRel = _sdf->Get("topic_pos_rel"); - this->node.Subscribe(topicPosRel, &FlipperControlPlugin::OnCmdPosRel, this); + const auto topicPrefix = "/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName; + + std::string topicTorque = _sdf->Get("topic_max_torque", topicPrefix + "/cmd_max_torque").first; + this->node.Subscribe(gazebo::validTopic({topicTorque}), &FlipperControlPlugin::OnCmdTorque, this); + + std::string topicVel = _sdf->Get("topic_vel", topicPrefix + "/cmd_vel").first; + this->node.Subscribe(gazebo::validTopic({topicVel}), &FlipperControlPlugin::OnCmdVel, this); + + std::string topicPosAbs = _sdf->Get("topic_pos_abs", topicPrefix + "/cmd_pos").first; + this->node.Subscribe(gazebo::validTopic({topicPosAbs}), &FlipperControlPlugin::OnCmdPosAbs, this); + + std::string topicPosRel = _sdf->Get("topic_pos_rel", topicPrefix + "/cmd_pos_rel").first; + this->node.Subscribe(gazebo::validTopic({topicPosRel}), &FlipperControlPlugin::OnCmdPosRel, this); + // cached command for flipper joint velocity; the joint has 1 axis, so this vector needs to hold 1 item this->velocityCommand.push_back(0.0); From 4372b784ec377b450a0a472a08c96fd67bf27c38 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 15 Nov 2021 22:39:02 +0100 Subject: [PATCH 3/4] Add runtime-configurable max velocity used for position control of flippers. --- .../launch/flipper_control.launch | 5 +++++ .../src/flipper_control_plugin.cpp | 22 +++++++++++++++++-- .../launch/flipper_control.launch | 5 +++++ 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/flipper_control.launch b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/flipper_control.launch index ea6935fc..e0e9848f 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/flipper_control.launch +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/flipper_control.launch @@ -25,4 +25,9 @@ args="$(arg joint_namespace)/cmd_max_torque@std_msgs/Float64]ignition.msgs.Double"> + + + diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp index 1e3e0c40..745e7583 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp @@ -10,6 +10,7 @@ #include "ignition/gazebo/components/JointVelocityCmd.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" #include @@ -37,6 +38,10 @@ namespace cras /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/joints/{joint_name}/cmd_pos_rel`. /// +/// ``: Custom topic that this system will subscribe to in order to receive max velocity to be used +/// for position control commands. The value is additionally limited by the velocity limit of the joint. This element +/// is optional, and the default value is `/model/{name_of_model}/joints/{joint_name}/cmd_pos_max_vel`. +/// /// ``: Custom topic that this system will subscribe to in order to receive torque limit command /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/joints/{joint_name}/cmd_max_torque`. This feature is not yet implemented. @@ -49,8 +54,9 @@ namespace cras /// setting. Default is 30 Nm. This feature is not yet implemented. /// /// ``: The gain used for positional control. The correcting velocity is computed as -/// gain * (current_position - setpoint) and limited by . The higher this gain is, the faster will the -/// flipper reach its positional control setpoint. Default is 20.0. +/// gain * (current_position - setpoint) and limited by and possibly also by position control max +/// velocity. The higher this gain is, the faster will the flipper reach its positional control setpoint. +/// Default is 20.0. /// /// ``: Angular tolerance of positional control (in radians). When the positional error /// is lower than this threshold, the controller will stop the flipper and try to keep it at the given position. Default @@ -61,6 +67,7 @@ namespace cras /// `{topic_vel}` (`ignition::msgs::Double`): The desired rotation velocity of the flipper. /// `{topic_pos_abs}` (`ignition::msgs::Double`): The positional setpoint of the flipper. /// `{topic_pos_rel}` (`ignition::msgs::Double`): Relative positional setpoint of the flipper. +/// `{topic_pos_max_vel}` (`ignition::msgs::Double`): Maximum velocity used for position control. /// `{topic_max_torque}` (`ignition::msgs::Double`): Maximum torque allowed for the flipper. class FlipperControlPlugin : public System, public ISystemConfigure, public ISystemPreUpdate { @@ -129,6 +136,8 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys std::string topicPosRel = _sdf->Get("topic_pos_rel", topicPrefix + "/cmd_pos_rel").first; this->node.Subscribe(gazebo::validTopic({topicPosRel}), &FlipperControlPlugin::OnCmdPosRel, this); + std::string topicPosMaxVel = _sdf->Get("topic_pos_max_vel", topicPrefix + "/cmd_pos_max_vel").first; + this->node.Subscribe(gazebo::validTopic({topicPosMaxVel}), &FlipperControlPlugin::OnCmdPosMaxVel, this); // cached command for flipper joint velocity; the joint has 1 axis, so this vector needs to hold 1 item this->velocityCommand.push_back(0.0); @@ -195,6 +204,8 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys if (fabs((currentPos - staticPos).Radian()) > this->positionCorrectionTolerance.Radian()) { const auto correctingVelocity = this->positionCorrectionGain * (staticPos - currentPos).Radian(); + if (this->positionCorrectionMaxVelocity.has_value()) + return math::clamp(correctingVelocity, -this->positionCorrectionMaxVelocity.value(), this->positionCorrectionMaxVelocity.value()); return correctingVelocity; } return 0.0; @@ -226,10 +237,16 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys this->cmdPosRel = _msg.data(); } + public: void OnCmdPosMaxVel(const msgs::Double &_msg) + { + this->positionCorrectionMaxVelocity = _msg.data(); + } + public: void Reset(EntityComponentManager& _ecm) { this->angularSpeed = 0; this->staticAngle.reset(); + this->positionCorrectionMaxVelocity.reset(); if (this->joint != kNullEntity) { @@ -252,6 +269,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys protected: double angularSpeed{0.0}; protected: double maxTorque{30.0}; protected: double positionCorrectionGain{20.0}; + protected: std::optional positionCorrectionMaxVelocity; protected: math::Angle positionCorrectionTolerance{math::Angle::Pi / 180.0}; // 1 degree protected: double maxAngularVelocity{0.5}; protected: std::vector velocityCommand; diff --git a/submitted_models/ctu_cras_norlab_marv_sensor_config_1/launch/flipper_control.launch b/submitted_models/ctu_cras_norlab_marv_sensor_config_1/launch/flipper_control.launch index ea6935fc..e0e9848f 100644 --- a/submitted_models/ctu_cras_norlab_marv_sensor_config_1/launch/flipper_control.launch +++ b/submitted_models/ctu_cras_norlab_marv_sensor_config_1/launch/flipper_control.launch @@ -25,4 +25,9 @@ args="$(arg joint_namespace)/cmd_max_torque@std_msgs/Float64]ignition.msgs.Double"> + + + From 65b6f2e4e34d0b16ae1da45a2ec1becdd164f227 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 16 Nov 2021 18:35:28 +0100 Subject: [PATCH 4/4] Added mutex protecting flipper commands. --- .../src/flipper_control_plugin.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp index 745e7583..09d4ef8c 100644 --- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp +++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -158,6 +159,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys return; } + std::lock_guard lock(this->cmdMutex); const auto& angle = _ecm.ComponentDefault(this->joint)->Data()[0]; if (this->cmdVel.has_value()) @@ -219,31 +221,37 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys public: void OnCmdTorque(const msgs::Double &_msg) { + std::lock_guard lock(this->cmdMutex); this->cmdTorque = _msg.data(); } public: void OnCmdVel(const msgs::Double &_msg) { + std::lock_guard lock(this->cmdMutex); this->cmdVel = _msg.data(); } public: void OnCmdPosAbs(const msgs::Double &_msg) { + std::lock_guard lock(this->cmdMutex); this->cmdPosAbs = _msg.data(); } public: void OnCmdPosRel(const msgs::Double &_msg) { + std::lock_guard lock(this->cmdMutex); this->cmdPosRel = _msg.data(); } public: void OnCmdPosMaxVel(const msgs::Double &_msg) { + std::lock_guard lock(this->cmdMutex); this->positionCorrectionMaxVelocity = _msg.data(); } public: void Reset(EntityComponentManager& _ecm) { + std::lock_guard lock(this->cmdMutex); this->angularSpeed = 0; this->staticAngle.reset(); this->positionCorrectionMaxVelocity.reset(); @@ -273,6 +281,7 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys protected: math::Angle positionCorrectionTolerance{math::Angle::Pi / 180.0}; // 1 degree protected: double maxAngularVelocity{0.5}; protected: std::vector velocityCommand; + protected: std::mutex cmdMutex; }; }