Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Wheelslip plugin parameter addition #2518

Draft
wants to merge 4 commits into
base: gz-sim8
Choose a base branch
from
Draft
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Next Next commit
add parameters to wheelslip system plugin
Signed-off-by: knmcguire <[email protected]>
knmcguire committed Aug 6, 2024
commit 57ba9255742d9dd3457fbcbfeffbe3d3345de656
65 changes: 56 additions & 9 deletions src/systems/wheel_slip/WheelSlip.cc
Original file line number Diff line number Diff line change
@@ -27,6 +27,7 @@

#include "gz/sim/Link.hh"
#include "gz/sim/Model.hh"
#include "gz/sim/Util.hh"
#include "gz/sim/components/AngularVelocity.hh"
#include "gz/sim/components/ChildLinkName.hh"
#include "gz/sim/components/Collision.hh"
@@ -35,6 +36,9 @@
#include "gz/sim/components/SlipComplianceCmd.hh"
#include "gz/sim/components/WheelSlipCmd.hh"

#include <gz/msgs/wheel_slip_parameters.pb.h>


using namespace gz;
using namespace sim;
using namespace systems;
@@ -53,6 +57,9 @@ class gz::sim::systems::WheelSlipPrivate
/// \brief Gazebo communication node
public: transport::Node node;

/// \brief Parameters registry
public: transport::parameters::ParametersRegistry * registry;

/// \brief Joint Entity
public: Entity jointEntity;

@@ -248,29 +255,48 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm)
for (auto &linkSurface : this->mapLinkSurfaceParams)
{
auto &params = linkSurface.second;
const auto * wheelSlipCmdComp =
_ecm.Component<components::WheelSlipCmd>(linkSurface.first);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Deleting this line removes the ability to set wheel slip parameters via the ECM. I think it would be better to keep the ECM support (since it is more deterministic) but to add the gz-transport parameters interface in addition to it

if (wheelSlipCmdComp)
std::string scopedName = ignition::gazebo::scopedName(
linkSurface.first, _ecm, ".", false);

// TODO(ivanpauno): WHY THE SCOPED NAME CHANGES BETWEEN HERE AND
// ConfigureParameters()?
// Here the scoped name starts with "wheel_slip."
// In `ConfigureParameters()` that doesn't happen!
auto paramName = std::string("systems.") + scopedName;
transport::parameters::ParametersRegistry::ParameterValue value;
try {
value = this->registry->Parameter(paramName);
} catch (const std::exception & ex) {
ignerr << "WheelSlip system Update(): failed to get parameter ["
<< paramName << "]: " << ex.what() << std::endl;
}
auto * msg = dynamic_cast<msgs::WheelSlipParameters *>(value.msg.get());
if (msg)
{
const auto & wheelSlipCmdParams = wheelSlipCmdComp->Data();
bool changed = (!math::equal(
params.slipComplianceLateral,
wheelSlipCmdParams.slip_compliance_lateral(),
msg->slip_compliance_lateral(),
1e-6)) ||
(!math::equal(
params.slipComplianceLongitudinal,
wheelSlipCmdParams.slip_compliance_longitudinal(),
msg->slip_compliance_longitudinal(),
1e-6));

if (changed)
{
gzdbg << "WheelSlip system Update(): parameter ["
<< paramName << "] updated"
<< std::endl;
params.slipComplianceLateral =
wheelSlipCmdParams.slip_compliance_lateral();
msg->slip_compliance_lateral();
params.slipComplianceLongitudinal =
wheelSlipCmdParams.slip_compliance_longitudinal();
msg->slip_compliance_longitudinal();
}
_ecm.RemoveComponent<components::WheelSlipCmd>(linkSurface.first);
}
} else if (value.msg.get()) {
gzerr << "WheelSlip system Update(): parameter ["
<< paramName << "] is not of type [ign_msgs.WheelSlipParameters]"
<< std::endl; }

// get user-defined normal force constant
double force = params.wheelNormalForce;
@@ -346,6 +372,26 @@ void WheelSlip::Configure(const Entity &_entity,
this->dataPtr->validConfig = this->dataPtr->Load(_ecm, sdfClone);
}

void WheelSlip::ConfigureParameters(
ignition::transport::parameters::ParametersRegistry & _registry,
EntityComponentManager &_ecm)
{
this->dataPtr->registry = &_registry;
for (const auto & linkParamsPair : this->dataPtr->mapLinkSurfaceParams) {
std::string scopedName = ignition::gazebo::scopedName(
linkParamsPair.first, _ecm, ".", false);

auto paramName = std::string("systems.wheel_slip.") + scopedName;
auto wsParams = std::make_unique<ignition::msgs::WheelSlipParameters>();
wsParams->set_slip_compliance_lateral(
linkParamsPair.second.slipComplianceLateral);
wsParams->set_slip_compliance_longitudinal(
linkParamsPair.second.slipComplianceLongitudinal);
_registry.DeclareParameter(paramName, std::move(wsParams));
}
}


//////////////////////////////////////////////////
void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm)
{
@@ -388,6 +434,7 @@ void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm)
GZ_ADD_PLUGIN(WheelSlip,
System,
WheelSlip::ISystemConfigure,
WheelSlip::ISystemConfigureParameters,
WheelSlip::ISystemPreUpdate)

GZ_ADD_PLUGIN_ALIAS(WheelSlip,
6 changes: 6 additions & 0 deletions src/systems/wheel_slip/WheelSlip.hh
Original file line number Diff line number Diff line change
@@ -118,6 +118,7 @@ namespace systems
class WheelSlip
: public System,
public ISystemConfigure,
public ISystemConfigureParameters,
public ISystemPreUpdate
{
/// \brief Constructor
@@ -132,6 +133,11 @@ namespace systems
EntityComponentManager &_ecm,
EventManager &_eventMgr) override;

public: void ConfigureParameters(
gz::transport::parameters::ParametersRegistry &
_registry,
EntityComponentManager &_ecm) override;

// Documentation inherited
public: void PreUpdate(
const gz::sim::UpdateInfo &_info,