From 2816f0291bdd2a77ca1c75c6bb6c27a1b24eb2f2 Mon Sep 17 00:00:00 2001 From: Bhajneet Singh Bedi Date: Wed, 27 Aug 2025 22:07:14 +0530 Subject: [PATCH 1/3] added motor-plugin and DroneCAN ESC telemetry --- CMakeLists.txt | 43 +- include/MotorPlugin.hh | 67 +++ models/iris_with_gimbal/model.sdf | 73 ++- models/iris_with_standoffs/model.sdf | 8 +- proto/ardupilot_gazebo/msgs/motor_stats.proto | 20 + scripts/motor_stats.py | 94 +++ src/MotorPlugin.cc | 548 ++++++++++++++++++ 7 files changed, 843 insertions(+), 10 deletions(-) create mode 100644 include/MotorPlugin.hh create mode 100644 proto/ardupilot_gazebo/msgs/motor_stats.proto create mode 100644 scripts/motor_stats.py create mode 100644 src/MotorPlugin.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index c98b602d..d8753264 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,6 +31,9 @@ if("$ENV{GZ_VERSION}" STREQUAL "harmonic" OR NOT DEFINED "ENV{GZ_VERSION}") gz_find_package(gz-sim8 REQUIRED) set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + gz_find_package(gz-msgs10 REQUIRED) + set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + message(STATUS "Compiling against Gazebo Harmonic") elseif("$ENV{GZ_VERSION}" STREQUAL "ionic") # Ionic @@ -69,10 +72,29 @@ endif() # --------------------------------------------------------------------------- # find_package(RapidJSON REQUIRED) find_package(OpenCV REQUIRED) +find_package(Protobuf REQUIRED) pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0) - +# --------------------------------------------------------------------------- # +# Build proto custom messages +set(ARDUPILOT_MSGS_PROTOS + ${CMAKE_CURRENT_SOURCE_DIR}/proto/ardupilot_gazebo/msgs/motor_stats.proto +) + +gz_msgs_generate_messages( + # The cmake target to be generated for libraries/executables to link + TARGET msgs + # The protobuf package to generate (Typically based on the path) + PROTO_PACKAGE "ardupilot_gazebo.msgs" + # The path to the base directory of the proto files + # All import paths should be relative to this (eg gz/custom_msgs/vector3d.proto) + MSGS_PATH ${CMAKE_CURRENT_SOURCE_DIR}/proto + # List of proto files to generate + MSGS_PROTOS ${ARDUPILOT_MSGS_PROTOS} + # List of message targets this library imports from + DEPENDENCIES gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} +) # --------------------------------------------------------------------------- # # Build plugin. @@ -89,6 +111,24 @@ target_link_libraries(ArduPilotPlugin PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ) +add_library(MotorPlugin + SHARED + src/MotorPlugin.cc + src/Util.cc + +) +target_include_directories(MotorPlugin PRIVATE + include + # ${CMAKE_CURRENT_BINARY_DIR}/ardupilot_gazebo-msgs_genmsg +) +target_link_libraries(MotorPlugin PRIVATE + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + ${PROJECT_NAME}-msgs + # gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} + # ardupilot_gazebo-msgs +) +# add_dependencies(MotorPlugin ardupilot_gazebo-msgs) + add_library(ParachutePlugin SHARED src/ParachutePlugin.cc @@ -134,6 +174,7 @@ target_link_libraries(GstCameraPlugin PRIVATE install( TARGETS ArduPilotPlugin + MotorPlugin ParachutePlugin CameraZoomPlugin GstCameraPlugin diff --git a/include/MotorPlugin.hh b/include/MotorPlugin.hh new file mode 100644 index 00000000..0f79c0c6 --- /dev/null +++ b/include/MotorPlugin.hh @@ -0,0 +1,67 @@ +/* + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef MOTORPLUGIN_HH_ +#define MOTORPLUGIN_HH_ + +#include + +#include + +namespace gz { +namespace sim { +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems { + + +class MotorPlugin : + public System, + public ISystemPreUpdate, + public ISystemConfigure +{ + /// \brief Destructor + public: virtual ~MotorPlugin(); + + /// \brief Constructor + public: MotorPlugin(); + + // Documentation inherited + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) final; + + /// \brief Load control channels + private: void LoadControlChannels( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \internal + /// \brief Private implementation + private: class Impl; + private: std::unique_ptr impl; +}; + +} // namespace systems +} +} // namespace sim +} // namespace gz + +#endif // MOTORPLUGIN_HH_ diff --git a/models/iris_with_gimbal/model.sdf b/models/iris_with_gimbal/model.sdf index 70935b39..3c910338 100755 --- a/models/iris_with_gimbal/model.sdf +++ b/models/iris_with_gimbal/model.sdf @@ -215,7 +215,7 @@ 0 1100 1900 - VELOCITY + COMMAND 0.20 0 0 @@ -223,6 +223,7 @@ 0 2.5 -2.5 + joint_0 1 @@ -233,7 +234,7 @@ 0 1100 1900 - VELOCITY + COMMAND 0.20 0 0 @@ -241,6 +242,7 @@ 0 2.5 -2.5 + joint_1 1 @@ -251,7 +253,7 @@ 0 1100 1900 - VELOCITY + COMMAND 0.20 0 0 @@ -259,6 +261,7 @@ 0 2.5 -2.5 + joint_2 1 @@ -269,7 +272,7 @@ 0 1100 1900 - VELOCITY + COMMAND 0.20 0 0 @@ -277,6 +280,7 @@ 0 2.5 -2.5 + joint_3 1 @@ -318,6 +322,65 @@ + + + + iris_with_standoffs::rotor_0_joint + 16 + 920 + 0.115 + 0.8 + joint_0 + 838 + 0 + 1.4 + 214.28 + 25.0 + + + + iris_with_standoffs::rotor_1_joint + 16 + 920 + 0.115 + 0.8 + joint_1 + 838 + 0 + 1.4 + 214.28 + 25.0 + + + + iris_with_standoffs::rotor_2_joint + 16 + 920 + 0.115 + 0.8 + joint_2 + -838 + 0 + 1.4 + 214.28 + 25.0 + + + + iris_with_standoffs::rotor_3_joint + 16 + 920 + 0.115 + 0.8 + joint_3 + -838 + 0 + 1.4 + 214.28 + 25.0 + + + @@ -341,4 +404,4 @@ - + \ No newline at end of file diff --git a/models/iris_with_standoffs/model.sdf b/models/iris_with_standoffs/model.sdf index ad4980cc..b8b8a987 100755 --- a/models/iris_with_standoffs/model.sdf +++ b/models/iris_with_standoffs/model.sdf @@ -241,7 +241,7 @@ 1e+16 - 0.004 + 1.0e-4 @@ -315,7 +315,7 @@ 1e+16 - 0.004 + 1.0e-4 @@ -389,7 +389,7 @@ 1e+16 - 0.004 + 1.0e-4 @@ -463,7 +463,7 @@ 1e+16 - 0.004 + 1.0e-4 diff --git a/proto/ardupilot_gazebo/msgs/motor_stats.proto b/proto/ardupilot_gazebo/msgs/motor_stats.proto new file mode 100644 index 00000000..c28c8942 --- /dev/null +++ b/proto/ardupilot_gazebo/msgs/motor_stats.proto @@ -0,0 +1,20 @@ +syntax = "proto3"; +package ardupilot_gazebo.msgs; + +// Message to hold the status of a single motor. +message MotorStats { + // Motor Id + int32 motor_id = 1; + + // Rotational speed of the motor in revolutions per minute (RPM). + double rpm = 2; + + // Voltage supplied to the motor in Volts. + double voltage = 3; + + // Current drawn by the motor in Amperes. + double current = 4; + + // Motor Temperature in Celsius. + double temperature = 5; +} diff --git a/scripts/motor_stats.py b/scripts/motor_stats.py new file mode 100644 index 00000000..fff72a7e --- /dev/null +++ b/scripts/motor_stats.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +# To run this script set PYTHONPATH environment variable to register custom motor_stats message. +# Run this in main directory:- export PYTHONPATH=$PYTHONPATH:`pwd`/build/ardupilot_gazebo-msgs_genmsg/python/ + +# Basic setup for dronecan:- +# sudo modprobe vcan +# sudo ip link add dev vcan0 type vcan +# sudo ip link set up vcan0 +# To check:- ip link show vcan0 + + +import time +import argparse +import threading +import dronecan +from gz.transport13 import Node + + +try: + from ardupilot_gazebo.msgs.motor_stats_pb2 import MotorStats +except ImportError: + print("Error: Could not import the MotorStats protobuf message.") + print("Please ensure you have compiled MotorStats.proto and the resulting") + print("Python module is in your PYTHONPATH.") + exit(1) + +class MotorStatsSubscriber: + def __init__(self, node_id, dronecan_port, model_name, joint_name): + self._model_name = model_name + self._joint_name = joint_name + self._node_id = node_id + self._dronecan_port = dronecan_port + self._lock = threading.Lock() + self._node = Node() + self._dronecan_node = None + self._initialized = False + + + print(f"Initializing DroneCAN node {node_id} on '{dronecan_port}'...") + try: + self._dronecan_node = dronecan.make_node(dronecan_port, node_id=node_id, bitrate=1000000) + except Exception as e: + print(f"Error initializing DroneCAN node: {e}") + print("Please ensure the CAN interface exists and is configured correctly.") + return + + + self._topic = f"/model/{self._model_name}/joint/{self._joint_name}/motor_stats" + if self._node.subscribe(MotorStats, self._topic, self.motor_stats_cb): + print("Subscribing to type {} on topic [{}]".format(MotorStats, self._topic)) + self._initialized = True + else: + print("Error subscribing to topic [{}]".format(self._topic)) + return + + def motor_stats_cb(self, msg: MotorStats): + if not self._initialized or self._dronecan_node is None: + return + + try: + with self._lock: + dronecan_msg = dronecan.uavcan.equipment.esc.Status() + dronecan_msg.esc_index = int(msg.motor_id) + dronecan_msg.rpm = int(msg.rpm) + dronecan_msg.voltage = float(msg.voltage) + dronecan_msg.current = float(msg.current) + dronecan_msg.temperature = float(msg.temperature) + + self._dronecan_node.broadcast(dronecan_msg) + except Exception as e: + print(f"Error broadcasting DroneCAN message: {e}") + +def main(): + parser = argparse.ArgumentParser(description="Gazebo to DroneCAN ESC bridge.") + parser.add_argument('dronecan_port', type=str, help="CAN interface name (e.g., vcan0, slcan:/dev/ttyACM0).") + parser.add_argument('--node-id', type=int, default=100, help="DroneCAN node ID for this bridge.") + parser.add_argument('--model-name', type=str, default="iris_with_gimbal", help="Name of the model in Gazebo.") + parser.add_argument('--num-motors', type=int, default=4, help="Number of motors on the model.") + args = parser.parse_args() + + print(f"Number of motors: {args.num_motors}") + + for i in range(args.num_motors): + joint_name = f"iris_with_standoffs::rotor_{i}_joint" + MotorStatsSubscriber(args.node_id, args.dronecan_port, args.model_name, joint_name) + try: + while True: + time.sleep(0.1) + except KeyboardInterrupt: + print("\nShutting down...") + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/MotorPlugin.cc b/src/MotorPlugin.cc new file mode 100644 index 00000000..5639e118 --- /dev/null +++ b/src/MotorPlugin.cc @@ -0,0 +1,548 @@ +/* + Copyright (C) 2025 ArduPilot.org + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with this program. If not, see . +*/ + +#include "MotorPlugin.hh" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "Util.hh" + + +namespace gz { +namespace sim { +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems { + +/// \brief class Control is responsible for controlling a joint +class Control +{ + /// \brief Constructor + public: Control() {} + + /// \brief Desctuctor + public: ~Control() {} + + /// \brief The PWM channel used to command this control + public: int channel = 0; + + /// \brief name of the joint being controlled + public: std::string jointName; + + /// \brief battery voltage + public: double voltageBat; + + /// \brief speed constant of motor (Kv) + public: double speedConstant; + + /// \brief motor internal resistance + public: double internal_resistance; + + /// \brief dynamic motor resistance while working + public: double resistance; + + /// \brief no load current of motor + public: double noLoadCurrent; + + /// \brief A multiplier to scale the raw input command + public: double multiplier; + + /// \brief An offset to shift the zero-point of the raw input command + public: double offset; + + /// \brief thermal resistance of the motor + public: double thermal_resistance; + + /// \brief thermal capacitance of the motor + public: double thermal_capacitance; + + /// \brief ambient working temperature + public: double ambient_temperature; + + /// \brief Motor temperature + public: double temperature; + + /// \brief joint being controlled + public: gz::sim::Entity joint; + + /// \brief Publisher for motor stats + public: gz::transport::Node::Publisher motorStatsPub; +}; + +////////////////////////////////////////////////// +class MotorPlugin::Impl +{ + /// \brief Callback for subscription for Velocity msg . + /// + /// \param controlIndex -> Index of the message + /// \param _msg -> message itself. + /// The command message is a target Velocity. + public: void OnVelMsg(int controlIndex, const gz::msgs::Double &_msg); + + /// \brief World occupied by the parent model. + public: World world{kNullEntity}; + + /// \brief Name of the world entity. + public: std::string worldName; + + /// \brief Model entity of Motor Model. + public: Model parentModel{kNullEntity}; + + /// \brief Name of the model entity. + public: std::string parentModelName; + + /// \brief Array of controllers + public: std::vector controls; + + /// \brief Array of msg command topics. + public: std::vector topics; + + /// \brief Stores target velocity values. + public: std::vector velValues; + + /// \brief Mutex to protect velValues + public: std::mutex velMutex; + + /// \brief Check to see if the config is valid + public: bool validConfig{false}; + + /// \brief gz-transport Node to subscribe to velValues data coming from ArduPilotPlugin + gz::transport::Node node; + + /// \brief Joint Entity + public: Joint joint{kNullEntity}; +}; + +////////////////////////////////////////////////// +void MotorPlugin::Impl::OnVelMsg(int controlIndex, const gz::msgs::Double &_msg) +{ + std::lock_guard lock(this->velMutex); + + // Bounds checking + if (controlIndex >= 0 && controlIndex < static_cast(this->velValues.size())) + { + this->velValues[controlIndex] = _msg.data(); + } + else + { + gzwarn << "Invalid control index " << controlIndex << " for PWM message. Expected [0, " + << (this->velValues.size() - 1) << "]" << std::endl; + } +} + + +////////////////////////////////////////////////// +MotorPlugin::~MotorPlugin() = default; + +////////////////////////////////////////////////// +MotorPlugin::MotorPlugin() : impl(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void MotorPlugin::Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) +{ + + // Make a clone so that we can call non-const methods + sdf::ElementPtr sdfClone = _sdf->Clone(); + + // retrieve world entity + this->impl->world = World( + _ecm.EntityByComponents(components::World())); + if (!this->impl->world.Valid(_ecm)) + { + gzerr << "MotorPlugin - world not found. " + "Failed to initialize.\n"; + return; + } + this->impl->worldName = this->impl->world.Name(_ecm).value(); + + // capture model entity + this->impl->parentModel = Model(_entity); + if (!this->impl->parentModel.Valid(_ecm)) + { + gzerr << "MotorPlugin should be attached to a model. " + "Failed to initialize.\n"; + return; + } + this->impl->parentModelName = this->impl->parentModel.Name(_ecm); + + // Load control channel params + this->LoadControlChannels(sdfClone, _ecm); + + // Initialize msg values vector for safety + this->impl->velValues.resize(this->impl->controls.size(), 0.0); + + // create components and subscriptions. + for (int i = 0; i < this->impl->controls.size(); ++i) + { + auto &control = this->impl->controls[i]; + + // Create joint components + if (!_ecm.Component(control.joint)) + { + _ecm.CreateComponent(control.joint, gz::sim::components::JointVelocity({0.0})); + } + if (!_ecm.Component(control.joint)) + { + _ecm.CreateComponent(control.joint, gz::sim::components::JointForceCmd({0.0})); + } + + // Subscriber + std::string topic = this->impl->topics[i]; + this->impl->node.Subscribe( + topic, + [this, i](const gz::msgs::Double &_msg, const gz::transport::MessageInfo &_info) { + this->impl->OnVelMsg(i, _msg); + }); + + gzdbg << "MotorPlugin subscribing to PWM messages on [" << topic + << "] for control index " << i << std::endl; + } + this->impl->validConfig = true; + +} + +///////////////////////////////////////////////// +void MotorPlugin::LoadControlChannels( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm) +{ + // per control channel + sdf::ElementPtr controlSdf; + if (_sdf->HasElement("control")) + { + controlSdf = _sdf->GetElement("control"); + } + while (controlSdf) + { + Control control; + + if (controlSdf->HasAttribute("channel")) + { + control.channel = atoi(controlSdf->GetAttribute("channel")->GetAsString().c_str()); + } + else + { + gzwarn << "[" << this->impl->parentModelName << "] " + << "id/channel attribute not specified, use order parsed [" + << control.channel << "].\n"; + } + + // parameters + if (controlSdf->HasElement("joint_name")) + { + control.jointName = controlSdf->Get("joint_name"); + } + else + { + gzerr << "[" << this->impl->parentModelName << "] " + << "Please specify a joint_name," + << " where the control channel is attached.\n"; + } + + // Get the pointer to the joint. + control.joint = JointByName(_ecm, this->impl->parentModel.Entity(), control.jointName); + if (control.joint == gz::sim::kNullEntity) + { + gzerr << "Joint [" << control.joint << "] not found in model [" << this->impl->parentModel.Name(_ecm) << "]" << "\n"; + return; + } + else + { + gzmsg << "Got Joint [" << control.joint << "]\n"; + } + + if (controlSdf->HasElement("voltage_bat")) + { + control.voltageBat = controlSdf->Get("voltage_bat"); + } + else + { + gzerr << "MotorPlugin requires parameter 'voltage_bat'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("speed_constant")) + { + control.speedConstant = controlSdf->Get("speed_constant"); + } + else + { + gzerr << "MotorPlugin requires parameter 'speed_constant'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("resistance")) + { + control.internal_resistance = controlSdf->Get("resistance"); + } + else + { + gzerr << "MotorPlugin requires parameter 'resistance'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("no_load_current")) + { + control.noLoadCurrent = controlSdf->Get("no_load_current"); + } + else + { + gzerr << "MotorPlugin requires parameter 'no_load_current'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("cmd_topic")) + { + this->impl->topics.push_back (controlSdf->Get("cmd_topic")); + } + else + { + gzerr << "MotorPlugin requires parameter 'cmd_topic'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("multiplier")) + { + control.multiplier = controlSdf->Get("multiplier"); + } + else + { + gzerr << "MotorPlugin requires parameter 'multiplier'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("offset")) + { + control.offset = controlSdf->Get("offset"); + } + else + { + gzerr << "MotorPlugin requires parameter 'offset'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("thermal_resistance")) + { + control.thermal_resistance = controlSdf->Get("thermal_resistance"); + } + else + { + gzerr << "MotorPlugin requires parameter 'thermal_resistance'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("thermal_capacitance")) + { + control.thermal_capacitance = controlSdf->Get("thermal_capacitance"); + } + else + { + gzerr << "MotorPlugin requires parameter 'thermal_capacitance'. " + "Failed to initialize.\n"; + return; + } + + if (controlSdf->HasElement("ambient_temperature")) + { + control.ambient_temperature = controlSdf->Get("ambient_temperature"); + } + else + { + gzerr << "MotorPlugin requires parameter 'ambient_temperature'. " + "Failed to initialize.\n"; + return; + } + + std::string motorStatsTopic = "/model/" + this->impl->parentModelName + "/joint/" + control.jointName + "/motor_stats"; + control.motorStatsPub = this->impl->node.Advertise(motorStatsTopic); + + this->impl->controls.push_back(control); + controlSdf = controlSdf->GetNextElement("control"); + } +} + + +////////////////////////////////////////////////// +void MotorPlugin::PreUpdate( + const gz::sim::UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("MotorPlugin::PreUpdate"); + double voltage=0.0; + double current=0.0; + + // gzdbg << "This is Dt:- " << dt << "\n"; + for (size_t i = 0; i < this->impl->controls.size(); ++i) + { + auto &control = this->impl->controls[i]; + double targetSpeed = 0.0; + + auto joint_vel_comp = _ecm.Component(control.joint); + if (!joint_vel_comp) + { + gzerr << "JointVelocity component missing for joint [" << control.jointName << "]\n"; + return; + } + + const auto &velocities = joint_vel_comp->Data(); + if (velocities.empty()) + { + gzwarn << "Empty velocities for joint [" << control.jointName << "]\n"; + continue; + } + + // current joint speed (rad/s) + double currSpeed = velocities[0]; + { + std::lock_guard lock(this->impl->velMutex); + targetSpeed = this->impl->velValues[i]; + } + + double kv = (control.speedConstant * (2.0 * M_PI)) / 60.0; + double pwm = (targetSpeed / control.multiplier) - control.offset; + + if (targetSpeed < 0) + { + pwm = -pwm; + } + voltage = control.voltageBat * pwm; + + double backEmfV = currSpeed / kv ; // Ω/KV + + // R_T = R_0 * (1 + a(T - T_0)) + // a -> alpha (temperature coefficient for copper is 0.00393) + // T_0 -> reference temperature taken as ambient temperature for simplicity + control.resistance = control.internal_resistance * (1.0 + 0.00393 * (control.temperature - control.ambient_temperature)); + current = (voltage - backEmfV) / control.resistance; + + double torque = 0.0; + if (std::abs(current) > control.noLoadCurrent) + { + torque = (current - (current > 0 ? 1 : -1) * control.noLoadCurrent) / kv; + } + + // Temperature calculation + // Parameter calcultion + // P_loss = P_resistive + P_friction + // P_resistance = I^2 * R = 16.37^2 * 0.115; + // P_friction = i_0 * v_m; (no load current and backemf) + // so P_loss = 39.38W (at full throttle) + + // R_th = (T - T_amb)/P_loss = (80 - 25)/39.38 = 1.40 deg C + // C_th = t/R_th = 300/1.40 = 214.28 (t -> tau (thermal time constant), estimated from datasheet) + // temperature eqns + // dT/dt = P_loss/C_th - (T - T_amb)/(R_th*C_th) + // C_th -> thermal capacitance, r_th -> therma resistance + // T -> current motor temperature + // T_amb -> ambient temperature + + double p_resistive = std::pow(std::abs(current), 2) * control.resistance; + double p_friction = control.noLoadCurrent * std::abs(backEmfV); + double p_loss = p_resistive + p_friction; + double dt = std::chrono::duration(_info.dt).count(); + double dT_dt = (p_loss / control.thermal_capacitance) - ((control.temperature - control.ambient_temperature) / (control.thermal_resistance * control.thermal_capacitance)); + control.temperature += dT_dt * dt; + if (control.temperature < control.ambient_temperature) { + control.temperature = control.ambient_temperature; + } + + currSpeed = (currSpeed * 60.0) / (2.0 * M_PI); // rad/s -> rpm + + // Publish motor stats + ardupilot_gazebo::msgs::MotorStats motorStatsMsg; + motorStatsMsg.set_motor_id(control.channel); + motorStatsMsg.set_rpm(currSpeed); + motorStatsMsg.set_voltage(voltage); + motorStatsMsg.set_current(current); + motorStatsMsg.set_temperature(control.temperature); + if (!control.motorStatsPub.Publish(motorStatsMsg)) + { + gzerr << "Failed to publish motor stats for joint [" << control.jointName << "]\n"; + } + + // debugging + // gzdbg << "Index:- " << i << " TargetS:- " << targetSpeed << " Curr Speed:- " << currSpeed << " Pwm:- "<< pwm << " Torque:- " << torque << " Voltage:- " << voltage << " Current:- " << current << " Temperature:- " << control.temperature << "\n"; + + // Apply torque to joint + auto jfcComp = _ecm.Component(control.joint); + if (jfcComp) + { + auto &forceCmd = jfcComp->Data(); + forceCmd[0] = torque; + } + else + { + gzerr << "JointForceCmd component missing for joint [" << control.jointName << "]\n"; + } + } +} + + +////////////////////////////////////////////////// + +} // namespace systems +} +} // namespace sim +} // namespace gz + +GZ_ADD_PLUGIN( + gz::sim::systems::MotorPlugin, + gz::sim::System, + gz::sim::systems::MotorPlugin::ISystemConfigure, + gz::sim::systems::MotorPlugin::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS( + gz::sim::systems::MotorPlugin, + "MotorPlugin") \ No newline at end of file From d7b7960f9916e06086ef3ca394f87c175a4ea2f3 Mon Sep 17 00:00:00 2001 From: Bhajneet Singh Bedi Date: Fri, 29 Aug 2025 07:23:36 +0530 Subject: [PATCH 2/3] formatted files, corrected MotorPlugin, restored iris_with_gimbal, added motor_model.sdf, corrected temperature units --- CMakeLists.txt | 2 +- include/MotorPlugin.hh | 89 +++++---- models/iris_with_gimbal/model.sdf | 71 +------ .../{motor_stats.proto => motor_status.proto} | 4 +- scripts/{motor_stats.py => motor_status.py} | 6 +- src/MotorPlugin.cc | 168 +++++++--------- worlds/motor_model.sdf | 188 ++++++++++++++++++ 7 files changed, 321 insertions(+), 207 deletions(-) rename proto/ardupilot_gazebo/msgs/{motor_stats.proto => motor_status.proto} (87%) rename scripts/{motor_stats.py => motor_status.py} (97%) create mode 100644 worlds/motor_model.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index d8753264..35cc8eda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,7 +79,7 @@ pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0) # --------------------------------------------------------------------------- # # Build proto custom messages set(ARDUPILOT_MSGS_PROTOS - ${CMAKE_CURRENT_SOURCE_DIR}/proto/ardupilot_gazebo/msgs/motor_stats.proto + ${CMAKE_CURRENT_SOURCE_DIR}/proto/ardupilot_gazebo/msgs/motor_status.proto ) gz_msgs_generate_messages( diff --git a/include/MotorPlugin.hh b/include/MotorPlugin.hh index 0f79c0c6..07e32c8e 100644 --- a/include/MotorPlugin.hh +++ b/include/MotorPlugin.hh @@ -1,17 +1,18 @@ /* - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * + Copyright (C) 2025 ArduPilot.org + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU Lesser General Public License + along with this program. If not, see . */ #ifndef MOTORPLUGIN_HH_ @@ -26,37 +27,55 @@ namespace sim { inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { - +/// \brief Plugin for an electro-mechanical model of a motor. +/// \class MotorPlugin +/// +/// A Gazebo plugin that simulates a brushless DC motor with thermal characteristics. +/// The plugin calculates motor performance including voltage, current, RPM, and temperature +/// based on physical parameters and publishes these as motor statistics. +/// +/// Parameters for each control element: +/// +/// Name of the joint to control +/// Battery voltage (V) +/// Motor Kv rating (RPM/V) +/// Motor internal resistance (Ohm) +/// No-load current draw (A) +/// Topic for receiving motor commands +/// Thermal resistance (°C/W) +/// Thermal capacitance (J/°C) +/// Ambient temperature (°C) +/// +/// +/// Published Data: +/// Motor statistics are published on topic: +/// /model/[model_name]/joint/[joint_name]/motor_stats +/// class MotorPlugin : public System, public ISystemPreUpdate, public ISystemConfigure { - /// \brief Destructor - public: virtual ~MotorPlugin(); + /// \brief Destructor + public: virtual ~MotorPlugin(); - /// \brief Constructor - public: MotorPlugin(); + /// \brief Constructor + public: MotorPlugin(); - // Documentation inherited - public: void PreUpdate(const gz::sim::UpdateInfo &_info, - EntityComponentManager &_ecm) final; + // Documentation inherited + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + EntityComponentManager &_ecm) final; - // Documentation inherited - public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &) final; + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) final; - /// \brief Load control channels - private: void LoadControlChannels( - sdf::ElementPtr _sdf, - gz::sim::EntityComponentManager &_ecm); - - /// \internal - /// \brief Private implementation - private: class Impl; - private: std::unique_ptr impl; + /// \internal + /// \brief Private implementation + private: class Impl; + private: std::unique_ptr impl; }; } // namespace systems diff --git a/models/iris_with_gimbal/model.sdf b/models/iris_with_gimbal/model.sdf index 3c910338..22353ac6 100755 --- a/models/iris_with_gimbal/model.sdf +++ b/models/iris_with_gimbal/model.sdf @@ -215,7 +215,7 @@ 0 1100 1900 - COMMAND + VELOCITY 0.20 0 0 @@ -223,7 +223,6 @@ 0 2.5 -2.5 - joint_0 1 @@ -234,7 +233,7 @@ 0 1100 1900 - COMMAND + VELOCITY 0.20 0 0 @@ -242,7 +241,6 @@ 0 2.5 -2.5 - joint_1 1 @@ -253,7 +251,7 @@ 0 1100 1900 - COMMAND + VELOCITY 0.20 0 0 @@ -261,7 +259,6 @@ 0 2.5 -2.5 - joint_2 1 @@ -272,7 +269,7 @@ 0 1100 1900 - COMMAND + VELOCITY 0.20 0 0 @@ -280,7 +277,6 @@ 0 2.5 -2.5 - joint_3 1 @@ -322,65 +318,6 @@ - - - - iris_with_standoffs::rotor_0_joint - 16 - 920 - 0.115 - 0.8 - joint_0 - 838 - 0 - 1.4 - 214.28 - 25.0 - - - - iris_with_standoffs::rotor_1_joint - 16 - 920 - 0.115 - 0.8 - joint_1 - 838 - 0 - 1.4 - 214.28 - 25.0 - - - - iris_with_standoffs::rotor_2_joint - 16 - 920 - 0.115 - 0.8 - joint_2 - -838 - 0 - 1.4 - 214.28 - 25.0 - - - - iris_with_standoffs::rotor_3_joint - 16 - 920 - 0.115 - 0.8 - joint_3 - -838 - 0 - 1.4 - 214.28 - 25.0 - - - diff --git a/proto/ardupilot_gazebo/msgs/motor_stats.proto b/proto/ardupilot_gazebo/msgs/motor_status.proto similarity index 87% rename from proto/ardupilot_gazebo/msgs/motor_stats.proto rename to proto/ardupilot_gazebo/msgs/motor_status.proto index c28c8942..9655626c 100644 --- a/proto/ardupilot_gazebo/msgs/motor_stats.proto +++ b/proto/ardupilot_gazebo/msgs/motor_status.proto @@ -2,7 +2,7 @@ syntax = "proto3"; package ardupilot_gazebo.msgs; // Message to hold the status of a single motor. -message MotorStats { +message MotorStatus { // Motor Id int32 motor_id = 1; @@ -15,6 +15,6 @@ message MotorStats { // Current drawn by the motor in Amperes. double current = 4; - // Motor Temperature in Celsius. + // Motor Temperature in Kelvin. double temperature = 5; } diff --git a/scripts/motor_stats.py b/scripts/motor_status.py similarity index 97% rename from scripts/motor_stats.py rename to scripts/motor_status.py index fff72a7e..5567df28 100644 --- a/scripts/motor_stats.py +++ b/scripts/motor_status.py @@ -4,9 +4,9 @@ # Run this in main directory:- export PYTHONPATH=$PYTHONPATH:`pwd`/build/ardupilot_gazebo-msgs_genmsg/python/ # Basic setup for dronecan:- -# sudo modprobe vcan -# sudo ip link add dev vcan0 type vcan -# sudo ip link set up vcan0 +# modprobe vcan +# ip link add dev vcan0 type vcan +# ip link set up vcan0 # To check:- ip link show vcan0 diff --git a/src/MotorPlugin.cc b/src/MotorPlugin.cc index 5639e118..0be38ef7 100644 --- a/src/MotorPlugin.cc +++ b/src/MotorPlugin.cc @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include "Util.hh" @@ -74,7 +74,7 @@ class Control public: double speedConstant; /// \brief motor internal resistance - public: double internal_resistance; + public: double internalResistance; /// \brief dynamic motor resistance while working public: double resistance; @@ -82,20 +82,14 @@ class Control /// \brief no load current of motor public: double noLoadCurrent; - /// \brief A multiplier to scale the raw input command - public: double multiplier; - - /// \brief An offset to shift the zero-point of the raw input command - public: double offset; - /// \brief thermal resistance of the motor - public: double thermal_resistance; + public: double thermalResistance; /// \brief thermal capacitance of the motor - public: double thermal_capacitance; + public: double thermalCapacitance; /// \brief ambient working temperature - public: double ambient_temperature; + public: double ambientTemperature; /// \brief Motor temperature public: double temperature; @@ -104,20 +98,26 @@ class Control public: gz::sim::Entity joint; /// \brief Publisher for motor stats - public: gz::transport::Node::Publisher motorStatsPub; + public: gz::transport::Node::Publisher motorStatusPub; }; ////////////////////////////////////////////////// class MotorPlugin::Impl { - /// \brief Callback for subscription for Velocity msg . + /// \brief Callback for subscription for Velocity msg. /// - /// \param controlIndex -> Index of the message + /// \param _controlIndex -> Index of the message /// \param _msg -> message itself. - /// The command message is a target Velocity. - public: void OnVelMsg(int controlIndex, const gz::msgs::Double &_msg); - - /// \brief World occupied by the parent model. + /// The command message is a PWM signal. + public: void OnPwmMsg(int _controlIndex, const gz::msgs::Double &_msg); + + /// \brief Load motors + /// + /// \param _sdf -> sdf pointer + /// \param _ecm -> entity component manager + public: void LoadControlChannels(sdf::ElementPtr _sdf, gz::sim::EntityComponentManager &_ecm); + + /// \brief World occupied by the parent model. public: World world{kNullEntity}; /// \brief Name of the world entity. @@ -135,11 +135,11 @@ class MotorPlugin::Impl /// \brief Array of msg command topics. public: std::vector topics; - /// \brief Stores target velocity values. - public: std::vector velValues; + /// \brief Stores target cmd values. + public: std::vector pwmValues; - /// \brief Mutex to protect velValues - public: std::mutex velMutex; + /// \brief Mutex to protect pwmValues + public: std::mutex pwmMutex; /// \brief Check to see if the config is valid public: bool validConfig{false}; @@ -152,20 +152,19 @@ class MotorPlugin::Impl }; ////////////////////////////////////////////////// -void MotorPlugin::Impl::OnVelMsg(int controlIndex, const gz::msgs::Double &_msg) +void MotorPlugin::Impl::OnPwmMsg(int _controlIndex, const gz::msgs::Double &_msg) { - std::lock_guard lock(this->velMutex); - - // Bounds checking - if (controlIndex >= 0 && controlIndex < static_cast(this->velValues.size())) - { - this->velValues[controlIndex] = _msg.data(); - } - else - { - gzwarn << "Invalid control index " << controlIndex << " for PWM message. Expected [0, " - << (this->velValues.size() - 1) << "]" << std::endl; - } + std::lock_guard lock(this->pwmMutex); + // Bounds checking + if (_controlIndex >= 0 && _controlIndex < static_cast(this->pwmValues.size())) + { + this->pwmValues[_controlIndex] = _msg.data(); + } + else + { + gzwarn << "Invalid control index " << _controlIndex << " for PWM message. Expected [0, " + << (this->pwmValues.size() - 1) << "]" << std::endl; + } } @@ -210,10 +209,10 @@ void MotorPlugin::Configure( this->impl->parentModelName = this->impl->parentModel.Name(_ecm); // Load control channel params - this->LoadControlChannels(sdfClone, _ecm); + this->impl->LoadControlChannels(sdfClone, _ecm); - // Initialize msg values vector for safety - this->impl->velValues.resize(this->impl->controls.size(), 0.0); + // Initialize pwmValues vector for safety + this->impl->pwmValues.resize(this->impl->controls.size(), 0.0); // create components and subscriptions. for (int i = 0; i < this->impl->controls.size(); ++i) @@ -235,7 +234,7 @@ void MotorPlugin::Configure( this->impl->node.Subscribe( topic, [this, i](const gz::msgs::Double &_msg, const gz::transport::MessageInfo &_info) { - this->impl->OnVelMsg(i, _msg); + this->impl->OnPwmMsg(i, _msg); }); gzdbg << "MotorPlugin subscribing to PWM messages on [" << topic @@ -246,7 +245,7 @@ void MotorPlugin::Configure( } ///////////////////////////////////////////////// -void MotorPlugin::LoadControlChannels( +void MotorPlugin::Impl::LoadControlChannels( sdf::ElementPtr _sdf, gz::sim::EntityComponentManager &_ecm) { @@ -266,7 +265,7 @@ void MotorPlugin::LoadControlChannels( } else { - gzwarn << "[" << this->impl->parentModelName << "] " + gzwarn << "[" << this->parentModelName << "] " << "id/channel attribute not specified, use order parsed [" << control.channel << "].\n"; } @@ -278,16 +277,16 @@ void MotorPlugin::LoadControlChannels( } else { - gzerr << "[" << this->impl->parentModelName << "] " + gzerr << "[" << this->parentModelName << "] " << "Please specify a joint_name," << " where the control channel is attached.\n"; } // Get the pointer to the joint. - control.joint = JointByName(_ecm, this->impl->parentModel.Entity(), control.jointName); + control.joint = JointByName(_ecm, this->parentModel.Entity(), control.jointName); if (control.joint == gz::sim::kNullEntity) { - gzerr << "Joint [" << control.joint << "] not found in model [" << this->impl->parentModel.Name(_ecm) << "]" << "\n"; + gzerr << "Joint [" << control.joint << "] not found in model [" << this->parentModel.Name(_ecm) << "]" << "\n"; return; } else @@ -319,7 +318,7 @@ void MotorPlugin::LoadControlChannels( if (controlSdf->HasElement("resistance")) { - control.internal_resistance = controlSdf->Get("resistance"); + control.internalResistance = controlSdf->Get("resistance"); } else { @@ -341,40 +340,18 @@ void MotorPlugin::LoadControlChannels( if (controlSdf->HasElement("cmd_topic")) { - this->impl->topics.push_back (controlSdf->Get("cmd_topic")); + this->topics.push_back(controlSdf->Get("cmd_topic")); } else { gzerr << "MotorPlugin requires parameter 'cmd_topic'. " "Failed to initialize.\n"; return; - } - - if (controlSdf->HasElement("multiplier")) - { - control.multiplier = controlSdf->Get("multiplier"); - } - else - { - gzerr << "MotorPlugin requires parameter 'multiplier'. " - "Failed to initialize.\n"; - return; - } - - if (controlSdf->HasElement("offset")) - { - control.offset = controlSdf->Get("offset"); - } - else - { - gzerr << "MotorPlugin requires parameter 'offset'. " - "Failed to initialize.\n"; - return; } if (controlSdf->HasElement("thermal_resistance")) { - control.thermal_resistance = controlSdf->Get("thermal_resistance"); + control.thermalResistance = controlSdf->Get("thermal_resistance"); } else { @@ -385,7 +362,7 @@ void MotorPlugin::LoadControlChannels( if (controlSdf->HasElement("thermal_capacitance")) { - control.thermal_capacitance = controlSdf->Get("thermal_capacitance"); + control.thermalCapacitance = controlSdf->Get("thermal_capacitance"); } else { @@ -396,7 +373,7 @@ void MotorPlugin::LoadControlChannels( if (controlSdf->HasElement("ambient_temperature")) { - control.ambient_temperature = controlSdf->Get("ambient_temperature"); + control.ambientTemperature = controlSdf->Get("ambient_temperature"); } else { @@ -405,10 +382,10 @@ void MotorPlugin::LoadControlChannels( return; } - std::string motorStatsTopic = "/model/" + this->impl->parentModelName + "/joint/" + control.jointName + "/motor_stats"; - control.motorStatsPub = this->impl->node.Advertise(motorStatsTopic); + std::string motorStatusTopic = "/model/" + this->parentModelName + "/joint/" + control.jointName + "/motor_stats"; + control.motorStatusPub = this->node.Advertise(motorStatusTopic); - this->impl->controls.push_back(control); + this->controls.push_back(control); controlSdf = controlSdf->GetNextElement("control"); } } @@ -427,7 +404,7 @@ void MotorPlugin::PreUpdate( for (size_t i = 0; i < this->impl->controls.size(); ++i) { auto &control = this->impl->controls[i]; - double targetSpeed = 0.0; + double pwm = 0.0; auto joint_vel_comp = _ecm.Component(control.joint); if (!joint_vel_comp) @@ -445,26 +422,19 @@ void MotorPlugin::PreUpdate( // current joint speed (rad/s) double currSpeed = velocities[0]; + std::lock_guard lock(this->impl->pwmMutex); { - std::lock_guard lock(this->impl->velMutex); - targetSpeed = this->impl->velValues[i]; + pwm = this->impl->pwmValues[i]; } double kv = (control.speedConstant * (2.0 * M_PI)) / 60.0; - double pwm = (targetSpeed / control.multiplier) - control.offset; - - if (targetSpeed < 0) - { - pwm = -pwm; - } voltage = control.voltageBat * pwm; - double backEmfV = currSpeed / kv ; // Ω/KV // R_T = R_0 * (1 + a(T - T_0)) // a -> alpha (temperature coefficient for copper is 0.00393) // T_0 -> reference temperature taken as ambient temperature for simplicity - control.resistance = control.internal_resistance * (1.0 + 0.00393 * (control.temperature - control.ambient_temperature)); + control.resistance = control.internalResistance * (1.0 + 0.00393 * (control.temperature - control.ambientTemperature)); current = (voltage - backEmfV) / control.resistance; double torque = 0.0; @@ -492,28 +462,28 @@ void MotorPlugin::PreUpdate( double p_friction = control.noLoadCurrent * std::abs(backEmfV); double p_loss = p_resistive + p_friction; double dt = std::chrono::duration(_info.dt).count(); - double dT_dt = (p_loss / control.thermal_capacitance) - ((control.temperature - control.ambient_temperature) / (control.thermal_resistance * control.thermal_capacitance)); + double dT_dt = (p_loss / control.thermalCapacitance) - ((control.temperature - control.ambientTemperature) / (control.thermalResistance * control.thermalCapacitance)); control.temperature += dT_dt * dt; - if (control.temperature < control.ambient_temperature) { - control.temperature = control.ambient_temperature; + if (control.temperature < control.ambientTemperature) { + control.temperature = control.ambientTemperature; } - + double temp_kelvin = control.temperature + 273.15; currSpeed = (currSpeed * 60.0) / (2.0 * M_PI); // rad/s -> rpm // Publish motor stats - ardupilot_gazebo::msgs::MotorStats motorStatsMsg; - motorStatsMsg.set_motor_id(control.channel); - motorStatsMsg.set_rpm(currSpeed); - motorStatsMsg.set_voltage(voltage); - motorStatsMsg.set_current(current); - motorStatsMsg.set_temperature(control.temperature); - if (!control.motorStatsPub.Publish(motorStatsMsg)) + ardupilot_gazebo::msgs::MotorStatus motorStatusMsg; + motorStatusMsg.set_motor_id(control.channel); + motorStatusMsg.set_rpm(currSpeed); + motorStatusMsg.set_voltage(voltage); + motorStatusMsg.set_current(current); + motorStatusMsg.set_temperature(temp_kelvin); + if (!control.motorStatusPub.Publish(motorStatusMsg)) { - gzerr << "Failed to publish motor stats for joint [" << control.jointName << "]\n"; + gzerr << "Failed to publish motor status for joint [" << control.jointName << "]\n"; } // debugging - // gzdbg << "Index:- " << i << " TargetS:- " << targetSpeed << " Curr Speed:- " << currSpeed << " Pwm:- "<< pwm << " Torque:- " << torque << " Voltage:- " << voltage << " Current:- " << current << " Temperature:- " << control.temperature << "\n"; + // gzdbg << "Index:- " << i << " PWM:- " << pwm << " Curr Speed:- " << currSpeed << " Torque:- " << torque << " Voltage:- " << voltage << " Current:- " << current << " Temperature:- " << control.temperature << "\n"; // Apply torque to joint auto jfcComp = _ecm.Component(control.joint); @@ -545,4 +515,4 @@ GZ_ADD_PLUGIN( GZ_ADD_PLUGIN_ALIAS( gz::sim::systems::MotorPlugin, - "MotorPlugin") \ No newline at end of file + "MotorPlugin") diff --git a/worlds/motor_model.sdf b/worlds/motor_model.sdf new file mode 100644 index 00000000..62dad495 --- /dev/null +++ b/worlds/motor_model.sdf @@ -0,0 +1,188 @@ + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 0 0 0 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + + 0.0 0.0 0.1 0 0 0 + + + 0.0001 + 0 + 0 + 0.0001 + 0 + 0.0001 + + 0.2 + + + + + 0.025 + 0.05 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + + + + + + 0.025 + 0.05 + + + + + + + + 0.0 0.0 0.13 0 0 0 + + + 0.00001 + 0 + 0 + 0.00025 + 0 + 0.00025 + + 0.05 + + + + + model://iris_with_standoffs/meshes/iris_prop_ccw.dae + 1 1 1 + + + + 0.1 0.1 0.1 1 + 0.2 0.2 0.2 1 + + + + + + 0.128 + 0.005 + + + + + + + world + base_link + + + + + base_link + motor_housing + + + + + 0 0 0 0 0 0 + motor_housing + propeller + + 0 0 1 + + 1.0e-5 + + + -1e+16 + 1e+16 + + + + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.0 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + 0.084 0 0 + 0 1 0 + 0 0 1 + propeller + + + + + + + + + + joint_0 + 16 + 920 + 0.115 + 0.8 + joint_0_topic + 1.40 + 214.28 + 25.0 + + + + + \ No newline at end of file From 04a3e33ec8953fe129affb90cee2f90e3991dce6 Mon Sep 17 00:00:00 2001 From: Bhajneet Singh Bedi Date: Fri, 29 Aug 2025 07:35:13 +0530 Subject: [PATCH 3/3] added deadzone and clamped throttle input --- src/MotorPlugin.cc | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/MotorPlugin.cc b/src/MotorPlugin.cc index 0be38ef7..55ce18f4 100644 --- a/src/MotorPlugin.cc +++ b/src/MotorPlugin.cc @@ -424,7 +424,23 @@ void MotorPlugin::PreUpdate( double currSpeed = velocities[0]; std::lock_guard lock(this->impl->pwmMutex); { - pwm = this->impl->pwmValues[i]; + if (this->impl->pwmValues[i] > 1.0 || this->impl->pwmValues[i] < -1.0) + { + gzerr << "PWM exceeded the input limit for joint [" << control.jointName << "]\n"; + return; + } + else + { + // Deadzone of ±0.02 to prevent motor chattering + if (std::abs(this->impl->pwmValues[i]) < 0.02) + { + pwm = 0.0; + } + else + { + pwm = this->impl->pwmValues[i]; + } + } } double kv = (control.speedConstant * (2.0 * M_PI)) / 60.0;