From 17df3620b3dac9676a26f3924c9b58042d540102 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 14 Mar 2021 00:18:15 -0800 Subject: [PATCH 01/15] Initial diff_drive_mecanum example world Initial version copied from diff_drive_skid example. Signed-off-by: Steve Peters --- examples/worlds/diff_drive_mecanum.sdf | 354 +++++++++++++++++++++++++ 1 file changed, 354 insertions(+) create mode 100644 examples/worlds/diff_drive_mecanum.sdf diff --git a/examples/worlds/diff_drive_mecanum.sdf b/examples/worlds/diff_drive_mecanum.sdf new file mode 100644 index 0000000000..4edf93497d --- /dev/null +++ b/examples/worlds/diff_drive_mecanum.sdf @@ -0,0 +1,354 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + 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 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + + + + + + From 48010ca383d5a876872f34f00a7e62ea0ec0cc70 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 14 Mar 2021 00:20:16 -0800 Subject: [PATCH 02/15] Set friction parameters for mecanum wheel model This modifies the friction parameters of the wheels in the skid-steer example vehicle to behave like Mecanum wheels, with anisotropic friction and friction directions that are constant in the chassis frame. To specify these friction directions, the //fdir1/@gazebo:expressed_in attribute is used. The friction values are specified as 1.0 in the direction corresponding to the Mecanum wheel roller axes, and 0.0 in the perpendicular direction, which corresponds to the rolling direction. Signed-off-by: Steve Peters --- examples/worlds/diff_drive_mecanum.sdf | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/examples/worlds/diff_drive_mecanum.sdf b/examples/worlds/diff_drive_mecanum.sdf index 4edf93497d..97fa63bfd5 100644 --- a/examples/worlds/diff_drive_mecanum.sdf +++ b/examples/worlds/diff_drive_mecanum.sdf @@ -79,7 +79,7 @@ - + 0 2 0.325 0 -0 0 @@ -150,9 +150,9 @@ - 0.5 - 1.0 - 0 0 1 + 1.0 + 0.0 + 1 -1 0 @@ -193,9 +193,9 @@ - 0.5 - 1.0 - 0 0 1 + 1.0 + 0.0 + 1 1 0 @@ -236,9 +236,9 @@ - 0.5 - 1.0 - 0 0 1 + 1.0 + 0.0 + 1 1 0 @@ -279,9 +279,9 @@ - 0.5 - 1.0 - 0 0 1 + 1.0 + 0.0 + 1 -1 0 From f0b99621f5570fd0c616ae95b8542e8480889c27 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sun, 14 Mar 2021 00:25:00 -0800 Subject: [PATCH 03/15] debug: box visuals for wheels to see rotations Signed-off-by: Steve Peters --- examples/worlds/diff_drive_mecanum.sdf | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/examples/worlds/diff_drive_mecanum.sdf b/examples/worlds/diff_drive_mecanum.sdf index 97fa63bfd5..2cf6249671 100644 --- a/examples/worlds/diff_drive_mecanum.sdf +++ b/examples/worlds/diff_drive_mecanum.sdf @@ -131,9 +131,14 @@ - + + 0.6 0.6 0.6 + 0.2 0.2 0.2 1 @@ -174,9 +179,10 @@ - + + 0.6 0.6 0.6 0.2 0.2 0.2 1 @@ -217,9 +223,10 @@ - + + 0.6 0.6 0.6 0.2 0.2 0.2 1 @@ -260,9 +267,10 @@ - + + 0.6 0.6 0.6 0.2 0.2 0.2 1 From 9f30a09cc681461678bc303e2654534b83279518 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 19 Mar 2021 01:07:15 -0700 Subject: [PATCH 04/15] Duplicate diff_drive system as mecanum_drive No files changed yet, this is only a file copy. Signed-off-by: Steve Peters --- src/systems/mecanum_drive/CMakeLists.txt | 7 + src/systems/mecanum_drive/DiffDrive.cc | 577 ++++++++++++++++++++++ src/systems/mecanum_drive/DiffDrive.hh | 116 +++++ src/systems/mecanum_drive/SpeedLimiter.cc | 200 ++++++++ src/systems/mecanum_drive/SpeedLimiter.hh | 130 +++++ 5 files changed, 1030 insertions(+) create mode 100644 src/systems/mecanum_drive/CMakeLists.txt create mode 100644 src/systems/mecanum_drive/DiffDrive.cc create mode 100644 src/systems/mecanum_drive/DiffDrive.hh create mode 100644 src/systems/mecanum_drive/SpeedLimiter.cc create mode 100644 src/systems/mecanum_drive/SpeedLimiter.hh diff --git a/src/systems/mecanum_drive/CMakeLists.txt b/src/systems/mecanum_drive/CMakeLists.txt new file mode 100644 index 0000000000..0a415da6d4 --- /dev/null +++ b/src/systems/mecanum_drive/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(diff-drive + SOURCES + DiffDrive.cc + SpeedLimiter.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/mecanum_drive/DiffDrive.cc b/src/systems/mecanum_drive/DiffDrive.cc new file mode 100644 index 0000000000..29a095a79f --- /dev/null +++ b/src/systems/mecanum_drive/DiffDrive.cc @@ -0,0 +1,577 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * 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. + * + */ + +#include "DiffDrive.hh" + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +#include "SpeedLimiter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Velocity command. +struct Commands +{ + /// \brief Linear velocity. + double lin; + + /// \brief Angular velocity. + double ang; + + Commands() : lin(0.0), ang(0.0) {} +}; + +class ignition::gazebo::systems::DiffDrivePrivate +{ + /// \brief Callback for velocity subscription + /// \param[in] _msg Velocity message + public: void OnCmdVel(const ignition::msgs::Twist &_msg); + + /// \brief Update odometry and publish an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Update the linear and angular velocities. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Entity of the left joint + public: std::vector leftJoints; + + /// \brief Entity of the right joint + public: std::vector rightJoints; + + /// \brief Name of left joint + public: std::vector leftJointNames; + + /// \brief Name of right joint + public: std::vector rightJointNames; + + /// \brief Calculated speed of left joint + public: double leftJointSpeed{0}; + + /// \brief Calculated speed of right joint + public: double rightJointSpeed{0}; + + /// \brief Distance between wheels + public: double wheelSeparation{1.0}; + + /// \brief Wheel radius + public: double wheelRadius{0.2}; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief The model's canonical link. + public: Link canonicalLink{kNullEntity}; + + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odomPubPeriod{0}; + + /// \brief Last sim time odom was published. + public: std::chrono::steady_clock::duration lastOdomPubTime{0}; + + /// \brief Diff drive odometry. + public: math::DiffDriveOdometry odom; + + /// \brief Diff drive odometry message publisher. + public: transport::Node::Publisher odomPub; + + /// \brief Diff drive tf message publisher. + public: transport::Node::Publisher tfPub; + + /// \brief Linear velocity limiter. + public: std::unique_ptr limiterLin; + + /// \brief Angular velocity limiter. + public: std::unique_ptr limiterAng; + + /// \brief Previous control command. + public: Commands last0Cmd; + + /// \brief Previous control command to last0Cmd. + public: Commands last1Cmd; + + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; + + /// \brief A mutex to protect the target velocity command. + public: std::mutex mutex; + + /// \brief frame_id from sdf. + public: std::string sdfFrameId; + + /// \brief child_frame_id from sdf. + public: std::string sdfChildFrameId; +}; + +////////////////////////////////////////////////// +DiffDrive::DiffDrive() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void DiffDrive::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + // Get the canonical link + std::vector links = _ecm.ChildrenByComponents( + this->dataPtr->model.Entity(), components::CanonicalLink()); + if (!links.empty()) + this->dataPtr->canonicalLink = Link(links[0]); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "DiffDrive plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + // Get params from SDF + sdf::ElementPtr sdfElem = ptr->GetElement("left_joint"); + while (sdfElem) + { + this->dataPtr->leftJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("left_joint"); + } + sdfElem = ptr->GetElement("right_joint"); + while (sdfElem) + { + this->dataPtr->rightJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("right_joint"); + } + + this->dataPtr->wheelSeparation = _sdf->Get("wheel_separation", + this->dataPtr->wheelSeparation).first; + this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", + this->dataPtr->wheelRadius).first; + + // Parse speed limiter parameters. + bool hasVelocityLimits = false; + bool hasAccelerationLimits = false; + bool hasJerkLimits = false; + double minVel = std::numeric_limits::lowest(); + double maxVel = std::numeric_limits::max(); + double minAccel = std::numeric_limits::lowest(); + double maxAccel = std::numeric_limits::max(); + double minJerk = std::numeric_limits::lowest(); + double maxJerk = std::numeric_limits::max(); + + if (_sdf->HasElement("min_velocity")) + { + minVel = _sdf->Get("min_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("max_velocity")) + { + maxVel = _sdf->Get("max_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("min_acceleration")) + { + minAccel = _sdf->Get("min_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("max_acceleration")) + { + maxAccel = _sdf->Get("max_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("min_jerk")) + { + minJerk = _sdf->Get("min_jerk"); + hasJerkLimits = true; + } + if (_sdf->HasElement("max_jerk")) + { + maxJerk = _sdf->Get("max_jerk"); + hasJerkLimits = true; + } + + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique( + hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, + minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); + + this->dataPtr->limiterAng = std::make_unique( + hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, + minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration odomPer{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast(odomPer); + } + + // Setup odometry. + this->dataPtr->odom.SetWheelParams(this->dataPtr->wheelSeparation, + this->dataPtr->wheelRadius, this->dataPtr->wheelRadius); + + // Subscribe to commands + std::vector topics; + if (_sdf->HasElement("topic")) + { + topics.push_back(_sdf->Get("topic")); + } + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); + auto topic = validTopic(topics); + + this->dataPtr->node.Subscribe(topic, &DiffDrivePrivate::OnCmdVel, + this->dataPtr.get()); + + std::vector odomTopics; + if (_sdf->HasElement("odom_topic")) + { + odomTopics.push_back(_sdf->Get("odom_topic")); + } + odomTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry"); + auto odomTopic = validTopic(odomTopics); + + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopic); + + std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/tf"}; + if (_sdf->HasElement("tf_topic")) + tfTopic = _sdf->Get("tf_topic"); + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); + + if (_sdf->HasElement("frame_id")) + this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); + + if (_sdf->HasElement("child_frame_id")) + this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + + ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]" + << std::endl; +} + +////////////////////////////////////////////////// +void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("DiffDrive::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // If the joints haven't been identified yet, look for them + static std::set warnedModels; + auto modelName = this->dataPtr->model.Name(_ecm); + if (this->dataPtr->leftJoints.empty() || + this->dataPtr->rightJoints.empty()) + { + bool warned{false}; + for (const std::string &name : this->dataPtr->leftJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->leftJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find left joint [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->rightJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->rightJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find right joint [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + if (warned) + { + warnedModels.insert(modelName); + } + } + + if (this->dataPtr->leftJoints.empty() || this->dataPtr->rightJoints.empty()) + return; + + if (warnedModels.find(modelName) != warnedModels.end()) + { + ignmsg << "Found joints for model [" << modelName + << "], plugin will start working." << std::endl; + warnedModels.erase(modelName); + } + + // Nothing left to do if paused. + if (_info.paused) + return; + + for (Entity joint : this->dataPtr->leftJoints) + { + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); + } + } + + for (Entity joint : this->dataPtr->rightJoints) + { + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent(joint, + components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); + } + } + + // Create the left and right side joint position components if they + // don't exist. + auto leftPos = _ecm.Component( + this->dataPtr->leftJoints[0]); + if (!leftPos) + { + _ecm.CreateComponent(this->dataPtr->leftJoints[0], + components::JointPosition()); + } + + auto rightPos = _ecm.Component( + this->dataPtr->rightJoints[0]); + if (!rightPos) + { + _ecm.CreateComponent(this->dataPtr->rightJoints[0], + components::JointPosition()); + } +} + +////////////////////////////////////////////////// +void DiffDrive::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("DiffDrive::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("DiffDrive::UpdateOdometry"); + // Initialize, if not already initialized. + if (!this->odom.Initialized()) + { + this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime)); + return; + } + + if (this->leftJoints.empty() || this->rightJoints.empty()) + return; + + // Get the first joint positions for the left and right side. + auto leftPos = _ecm.Component(this->leftJoints[0]); + auto rightPos = _ecm.Component( + this->rightJoints[0]); + + // Abort if the joints were not found or just created. + if (!leftPos || !rightPos || leftPos->Data().empty() || + rightPos->Data().empty()) + { + return; + } + + this->odom.Update(leftPos->Data()[0], rightPos->Data()[0], + std::chrono::steady_clock::time_point(_info.simTime)); + + // Throttle publishing + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + + // Construct the odometry message and publish it. + msgs::Odometry msg; + msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); + msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); + + math::Quaterniond orientation(0, 0, *this->odom.Heading()); + msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); + + msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); + msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame id. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + if (this->sdfFrameId.empty()) + { + frame->add_value(this->model.Name(_ecm) + "/odom"); + } + else + { + frame->add_value(this->sdfFrameId); + } + + std::optional linkName = this->canonicalLink.Name(_ecm); + if (this->sdfChildFrameId.empty()) + { + if (linkName) + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); + } + } + else + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->sdfChildFrameId); + } + + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + ignition::msgs::Pose *tfMsgPose = nullptr; + tfMsgPose = tfMsg.add_pose(); + tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); + tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); + tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); + + // Publish the messages + this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); +} + +////////////////////////////////////////////////// +void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + IGN_PROFILE("DiffDrive::UpdateVelocity"); + + double linVel; + double angVel; + { + std::lock_guard lock(this->mutex); + linVel = this->targetVel.linear().x(); + angVel = this->targetVel.angular().z(); + } + + const double dt = std::chrono::duration(_info.dt).count(); + + // Limit the target velocity if needed. + this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); + this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); + + // Update history of commands. + this->last1Cmd = last0Cmd; + this->last0Cmd.lin = linVel; + this->last0Cmd.ang = angVel; + + // Convert the target velocities to joint velocities. + this->rightJointSpeed = + (linVel + angVel * this->wheelSeparation / 2.0) / this->wheelRadius; + this->leftJointSpeed = + (linVel - angVel * this->wheelSeparation / 2.0) / this->wheelRadius; +} + +////////////////////////////////////////////////// +void DiffDrivePrivate::OnCmdVel(const msgs::Twist &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetVel = _msg; +} + +IGNITION_ADD_PLUGIN(DiffDrive, + ignition::gazebo::System, + DiffDrive::ISystemConfigure, + DiffDrive::ISystemPreUpdate, + DiffDrive::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(DiffDrive, "ignition::gazebo::systems::DiffDrive") diff --git a/src/systems/mecanum_drive/DiffDrive.hh b/src/systems/mecanum_drive/DiffDrive.hh new file mode 100644 index 0000000000..35002495d5 --- /dev/null +++ b/src/systems/mecanum_drive/DiffDrive.hh @@ -0,0 +1,116 @@ +/* + * Copyright (C) 2018 Open Source Robotics Foundation + * + * 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 IGNITION_GAZEBO_SYSTEMS_DIFFDRIVE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_DIFFDRIVE_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class DiffDrivePrivate; + + /// \brief Differential drive controller which can be attached to a model + /// with any number of left and right wheels. + /// + /// # System Parameters + /// + /// ``: Name of a joint that controls a left wheel. This + /// element can appear multiple times, and must appear at least once. + /// + /// ``: Name of a joint that controls a right wheel. This + /// element can appear multiple times, and must appear at least once. + /// + /// ``: Distance between wheels, in meters. This element + /// is optional, although it is recommended to be included with an + /// appropriate value. The default value is 1.0m. + /// + /// ``: Wheel radius in meters. This element is optional, + /// although it is recommended to be included with an appropriate value. The + /// default value is 0.2m. + /// + /// ``: Odometry publication frequency. This + /// element is optional, and the default value is 50Hz. + /// + /// ``: Custom topic that this system will subscribe to in order to + /// receive command velocity messages. This element if optional, and the + /// default value is `/model/{name_of_model}/cmd_vel`. + /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element if optional, and the default value is + /// `/model/{name_of_model}/odometry`. + /// + /// ``: Custom topic on which this system will publish the + /// transform from `frame_id` to `child_frame_id`. This element if optional, + /// and the default value is `/model/{name_of_model}/tf`. + /// + /// ``: Custom `frame_id` field that this system will use as the + /// origin of the odometry transform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, and the + /// default value is `{name_of_model}/odom`. + /// + /// ``: Custom `child_frame_id` that this system will use as + /// the target of the odometry trasnform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, + /// and the default value is `{name_of_model}/{name_of_link}`. + class DiffDrive + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: DiffDrive(); + + /// \brief Destructor + public: ~DiffDrive() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/mecanum_drive/SpeedLimiter.cc b/src/systems/mecanum_drive/SpeedLimiter.cc new file mode 100644 index 0000000000..59b50f54b9 --- /dev/null +++ b/src/systems/mecanum_drive/SpeedLimiter.cc @@ -0,0 +1,200 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + * Modified: Carlos Agüero + */ + +#include + +#include "SpeedLimiter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private SpeedLimiter data class. +class ignition::gazebo::systems::SpeedLimiterPrivate +{ + /// \brief Class constructor. + public: SpeedLimiterPrivate(bool _hasVelocityLimits, + bool _hasAccelerationLimits, + bool _hasJerkLimits, + double _minVelocity, + double _maxVelocity, + double _minAcceleration, + double _maxAcceleration, + double _minJerk, + double _maxJerk) + : hasVelocityLimits(_hasVelocityLimits), + hasAccelerationLimits(_hasAccelerationLimits), + hasJerkLimits(_hasJerkLimits), + minVelocity(_minVelocity), + maxVelocity(_maxVelocity), + minAcceleration(_minAcceleration), + maxAcceleration(_maxAcceleration), + minJerk(_minJerk), + maxJerk(_maxJerk) + { + } + + /// \brief Enable/Disable velocity. + public: bool hasVelocityLimits; + + /// \brief Enable/Disable acceleration. + public: bool hasAccelerationLimits; + + /// \brief Enable/Disable jerk. + public: bool hasJerkLimits; + + /// \brief Minimum velocity limit. + public: double minVelocity; + + /// \brief Maximum velocity limit. + public: double maxVelocity; + + /// \brief Minimum acceleration limit. + public: double minAcceleration; + + /// \brief Maximum acceleration limit. + public: double maxAcceleration; + + /// \brief Minimum jerk limit. + public: double minJerk; + + /// \brief Maximum jerk limit. + public: double maxJerk; +}; + +////////////////////////////////////////////////// +SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits, + bool _hasAccelerationLimits, + bool _hasJerkLimits, + double _minVelocity, + double _maxVelocity, + double _minAcceleration, + double _maxAcceleration, + double _minJerk, + double _maxJerk) + : dataPtr(std::make_unique(_hasVelocityLimits, + _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity, + _minAcceleration, _maxAcceleration, _minJerk, _maxJerk)) +{ +} + +////////////////////////////////////////////////// +SpeedLimiter::~SpeedLimiter() +{ +} + +////////////////////////////////////////////////// +double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const +{ + const double tmp = _v; + + this->LimitJerk(_v, _v0, _v1, _dt); + this->LimitAcceleration(_v, _v0, _dt); + this->LimitVelocity(_v); + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitVelocity(double &_v) const +{ + const double tmp = _v; + + if (this->dataPtr->hasVelocityLimits) + { + _v = ignition::math::clamp( + _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); + } + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const +{ + const double tmp = _v; + + if (this->dataPtr->hasAccelerationLimits) + { + const double dvMin = this->dataPtr->minAcceleration * _dt; + const double dvMax = this->dataPtr->maxAcceleration * _dt; + + const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax); + + _v = _v0 + dv; + } + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} + +////////////////////////////////////////////////// +double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt) + const +{ + const double tmp = _v; + + if (this->dataPtr->hasJerkLimits) + { + const double dv = _v - _v0; + const double dv0 = _v0 - _v1; + + const double dt2 = 2. * _dt * _dt; + + const double daMin = this->dataPtr->minJerk * dt2; + const double daMax = this->dataPtr->maxJerk * dt2; + + const double da = ignition::math::clamp(dv - dv0, daMin, daMax); + + _v = _v0 + dv0 + da; + } + + if (ignition::math::equal(tmp, 0.0)) + return 1.0; + else + return _v / tmp; +} diff --git a/src/systems/mecanum_drive/SpeedLimiter.hh b/src/systems/mecanum_drive/SpeedLimiter.hh new file mode 100644 index 0000000000..34286bc3ae --- /dev/null +++ b/src/systems/mecanum_drive/SpeedLimiter.hh @@ -0,0 +1,130 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + * Modified: Carlos Agüero + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration. + class SpeedLimiterPrivate; + + /// \brief Class to limit velocity, acceleration and jerk. + /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller + class SpeedLimiter + { + /// \brief Constructor. + /// \param [in] _hasVelocityLimits if true, applies velocity limits. + /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. + /// \param [in] _hasJerkLimits if true, applies jerk limits. + /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. + /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. + /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. + /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. + /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. + /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. + public: SpeedLimiter(bool _hasVelocityLimits = false, + bool _hasAccelerationLimits = false, + bool _hasJerkLimits = false, + double _minVelocity = 0.0, + double _maxVelocity = 0.0, + double _minAcceleration = 0.0, + double _maxAcceleration = 0.0, + double _minJerk = 0.0, + double _maxJerk = 0.0); + + /// \brief Destructor. + public: ~SpeedLimiter(); + + /// \brief Limit the velocity and acceleration. + /// \param [in, out] _v Velocity [m/s]. + /// \param [in] _v0 Previous velocity to v [m/s]. + /// \param [in] _v1 Previous velocity to v0 [m/s]. + /// \param [in] _dt Time step [s]. + /// \return Limiting factor (1.0 if none). + public: double Limit(double &_v, + double _v0, + double _v1, + double _dt) const; + + /// \brief Limit the velocity. + /// \param [in, out] _v Velocity [m/s]. + /// \return Limiting factor (1.0 if none). + public: double LimitVelocity(double &_v) const; + + /// \brief Limit the acceleration. + /// \param [in, out] _v Velocity [m/s]. + /// \param [in] _v0 Previous velocity [m/s]. + /// \param [in] _dt Time step [s]. + /// \return Limiting factor (1.0 if none). + public: double LimitAcceleration(double &_v, + double _v0, + double _dt) const; + + /// \brief Limit the jerk. + /// \param [in, out] _v Velocity [m/s]. + /// \param [in] _v0 Previous velocity to v [m/s]. + /// \param [in] _v1 Previous velocity to v0 [m/s]. + /// \param [in] _dt Time step [s]. + /// \return Limiting factor (1.0 if none). + /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. + public: double LimitJerk(double &_v, + double _v0, + double _v1, + double _dt) const; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif From 7ce81bd23b680ac5cfab8aeccde993a9c1740edf Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Mar 2021 11:04:59 -0700 Subject: [PATCH 05/15] Use ignition as xml namespace Signed-off-by: Steve Peters --- examples/worlds/diff_drive_mecanum.sdf | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/worlds/diff_drive_mecanum.sdf b/examples/worlds/diff_drive_mecanum.sdf index 2cf6249671..6711c8f929 100644 --- a/examples/worlds/diff_drive_mecanum.sdf +++ b/examples/worlds/diff_drive_mecanum.sdf @@ -157,7 +157,7 @@ 1.0 0.0 - 1 -1 0 + 1 -1 0 @@ -201,7 +201,7 @@ 1.0 0.0 - 1 1 0 + 1 1 0 @@ -245,7 +245,7 @@ 1.0 0.0 - 1 1 0 + 1 1 0 @@ -289,7 +289,7 @@ 1.0 0.0 - 1 -1 0 + 1 -1 0 From a270912eec9f733e5922f2e44abc0d5bbe373bb9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Mar 2021 11:23:19 -0700 Subject: [PATCH 06/15] Rename DiffDrive.* to MecanumDrive.* Signed-off-by: Steve Peters --- src/systems/CMakeLists.txt | 1 + src/systems/mecanum_drive/CMakeLists.txt | 4 +- .../{DiffDrive.cc => MecanumDrive.cc} | 46 +++++++++---------- .../{DiffDrive.hh => MecanumDrive.hh} | 16 +++---- 4 files changed, 34 insertions(+), 33 deletions(-) rename src/systems/mecanum_drive/{DiffDrive.cc => MecanumDrive.cc} (92%) rename src/systems/mecanum_drive/{DiffDrive.hh => MecanumDrive.hh} (92%) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 9105d9b285..bf3519db1b 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -109,6 +109,7 @@ add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) add_subdirectory(magnetometer) +add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) add_subdirectory(optical_tactile_plugin) diff --git a/src/systems/mecanum_drive/CMakeLists.txt b/src/systems/mecanum_drive/CMakeLists.txt index 0a415da6d4..c0f2c42e70 100644 --- a/src/systems/mecanum_drive/CMakeLists.txt +++ b/src/systems/mecanum_drive/CMakeLists.txt @@ -1,6 +1,6 @@ -gz_add_system(diff-drive +gz_add_system(mecanum-drive SOURCES - DiffDrive.cc + MecanumDrive.cc SpeedLimiter.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} diff --git a/src/systems/mecanum_drive/DiffDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc similarity index 92% rename from src/systems/mecanum_drive/DiffDrive.cc rename to src/systems/mecanum_drive/MecanumDrive.cc index 29a095a79f..59e180a12b 100644 --- a/src/systems/mecanum_drive/DiffDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -15,7 +15,7 @@ * */ -#include "DiffDrive.hh" +#include "MecanumDrive.hh" #include @@ -56,7 +56,7 @@ struct Commands Commands() : lin(0.0), ang(0.0) {} }; -class ignition::gazebo::systems::DiffDrivePrivate +class ignition::gazebo::systems::MecanumDrivePrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message @@ -150,13 +150,13 @@ class ignition::gazebo::systems::DiffDrivePrivate }; ////////////////////////////////////////////////// -DiffDrive::DiffDrive() - : dataPtr(std::make_unique()) +MecanumDrive::MecanumDrive() + : dataPtr(std::make_unique()) { } ////////////////////////////////////////////////// -void DiffDrive::Configure(const Entity &_entity, +void MecanumDrive::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) @@ -171,7 +171,7 @@ void DiffDrive::Configure(const Entity &_entity, if (!this->dataPtr->model.Valid(_ecm)) { - ignerr << "DiffDrive plugin should be attached to a model entity. " + ignerr << "MecanumDrive plugin should be attached to a model entity. " << "Failed to initialize." << std::endl; return; } @@ -271,7 +271,7 @@ void DiffDrive::Configure(const Entity &_entity, topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); auto topic = validTopic(topics); - this->dataPtr->node.Subscribe(topic, &DiffDrivePrivate::OnCmdVel, + this->dataPtr->node.Subscribe(topic, &MecanumDrivePrivate::OnCmdVel, this->dataPtr.get()); std::vector odomTopics; @@ -299,15 +299,15 @@ void DiffDrive::Configure(const Entity &_entity, if (_sdf->HasElement("child_frame_id")) this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); - ignmsg << "DiffDrive subscribing to twist messages on [" << topic << "]" + ignmsg << "MecanumDrive subscribing to twist messages on [" << topic << "]" << std::endl; } ////////////////////////////////////////////////// -void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, +void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("DiffDrive::PreUpdate"); + IGN_PROFILE("MecanumDrive::PreUpdate"); // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) @@ -421,10 +421,10 @@ void DiffDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrive::PostUpdate(const UpdateInfo &_info, +void MecanumDrive::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { - IGN_PROFILE("DiffDrive::PostUpdate"); + IGN_PROFILE("MecanumDrive::PostUpdate"); // Nothing left to do if paused. if (_info.paused) return; @@ -434,10 +434,10 @@ void DiffDrive::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, +void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("DiffDrive::UpdateOdometry"); + IGN_PROFILE("MecanumDrive::UpdateOdometry"); // Initialize, if not already initialized. if (!this->odom.Initialized()) { @@ -530,10 +530,10 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, +void MecanumDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { - IGN_PROFILE("DiffDrive::UpdateVelocity"); + IGN_PROFILE("MecanumDrive::UpdateVelocity"); double linVel; double angVel; @@ -562,16 +562,16 @@ void DiffDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::OnCmdVel(const msgs::Twist &_msg) +void MecanumDrivePrivate::OnCmdVel(const msgs::Twist &_msg) { std::lock_guard lock(this->mutex); this->targetVel = _msg; } -IGNITION_ADD_PLUGIN(DiffDrive, +IGNITION_ADD_PLUGIN(MecanumDrive, ignition::gazebo::System, - DiffDrive::ISystemConfigure, - DiffDrive::ISystemPreUpdate, - DiffDrive::ISystemPostUpdate) + MecanumDrive::ISystemConfigure, + MecanumDrive::ISystemPreUpdate, + MecanumDrive::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(DiffDrive, "ignition::gazebo::systems::DiffDrive") +IGNITION_ADD_PLUGIN_ALIAS(MecanumDrive, "ignition::gazebo::systems::MecanumDrive") diff --git a/src/systems/mecanum_drive/DiffDrive.hh b/src/systems/mecanum_drive/MecanumDrive.hh similarity index 92% rename from src/systems/mecanum_drive/DiffDrive.hh rename to src/systems/mecanum_drive/MecanumDrive.hh index 35002495d5..52bf645565 100644 --- a/src/systems/mecanum_drive/DiffDrive.hh +++ b/src/systems/mecanum_drive/MecanumDrive.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * Copyright (C) 2021 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_DIFFDRIVE_HH_ -#define IGNITION_GAZEBO_SYSTEMS_DIFFDRIVE_HH_ +#ifndef IGNITION_GAZEBO_SYSTEMS_MECANUMDRIVE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_MECANUMDRIVE_HH_ #include @@ -30,7 +30,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Forward declaration - class DiffDrivePrivate; + class MecanumDrivePrivate; /// \brief Differential drive controller which can be attached to a model /// with any number of left and right wheels. @@ -77,17 +77,17 @@ namespace systems /// `ignition.msgs.Pose_V` message and the `` /// `ignition.msgs.Odometry` message. This element if optional, /// and the default value is `{name_of_model}/{name_of_link}`. - class DiffDrive + class MecanumDrive : public System, public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate { /// \brief Constructor - public: DiffDrive(); + public: MecanumDrive(); /// \brief Destructor - public: ~DiffDrive() override = default; + public: ~MecanumDrive() override = default; // Documentation inherited public: void Configure(const Entity &_entity, @@ -106,7 +106,7 @@ namespace systems const EntityComponentManager &_ecm) override; /// \brief Private data pointer - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; }; } } From 3653e410b9003aa1b0d825d31937e7ee4ef12b9d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Mar 2021 17:58:33 -0700 Subject: [PATCH 07/15] Update MecanumDrive kinematics Based on the following references: https://robohub.org/drive-kinematics-skid-steer-and-mecanum-ros-twist-included/ https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf Signed-off-by: Steve Peters --- examples/worlds/mecanum_drive.sdf | 363 ++++++++++++++++++++++ src/systems/mecanum_drive/MecanumDrive.cc | 221 ++++++++++--- 2 files changed, 540 insertions(+), 44 deletions(-) create mode 100644 examples/worlds/mecanum_drive.sdf diff --git a/examples/worlds/mecanum_drive.sdf b/examples/worlds/mecanum_drive.sdf new file mode 100644 index 0000000000..aab6535291 --- /dev/null +++ b/examples/worlds/mecanum_drive.sdf @@ -0,0 +1,363 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + 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 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.6 0.6 0.6 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.6 0.6 0.6 + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.6 0.6 0.6 + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 1 0 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.6 0.6 0.6 + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1.0 + 0.0 + 1 -1 0 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + front_right_wheel_joint + rear_left_wheel_joint + rear_right_wheel_joint + 1.25 + 1.511 + 0.3 + + + + + + diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 59e180a12b..6ec39223eb 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -50,6 +50,9 @@ struct Commands /// \brief Linear velocity. double lin; + /// \brief Lateral velocity. + double lat; + /// \brief Angular velocity. double ang; @@ -79,27 +82,48 @@ class ignition::gazebo::systems::MecanumDrivePrivate /// \brief Ignition communication node. public: transport::Node node; - /// \brief Entity of the left joint - public: std::vector leftJoints; + /// \brief Entity of the front left joint + public: std::vector frontLeftJoints; + + /// \brief Entity of the front right joint + public: std::vector frontRightJoints; + + /// \brief Entity of the back left joint + public: std::vector backLeftJoints; + + /// \brief Entity of the back right joint + public: std::vector backRightJoints; + + /// \brief Name of front left joint + public: std::vector frontLeftJointNames; - /// \brief Entity of the right joint - public: std::vector rightJoints; + /// \brief Name of front right joint + public: std::vector frontRightJointNames; - /// \brief Name of left joint - public: std::vector leftJointNames; + /// \brief Name of back left joint + public: std::vector backLeftJointNames; - /// \brief Name of right joint - public: std::vector rightJointNames; + /// \brief Name of back right joint + public: std::vector backRightJointNames; - /// \brief Calculated speed of left joint - public: double leftJointSpeed{0}; + /// \brief Calculated speed of front left joint + public: double frontLeftJointSpeed{0}; - /// \brief Calculated speed of right joint - public: double rightJointSpeed{0}; + /// \brief Calculated speed of front right joint + public: double frontRightJointSpeed{0}; - /// \brief Distance between wheels + /// \brief Calculated speed of back left joint + public: double backLeftJointSpeed{0}; + + /// \brief Calculated speed of back right joint + public: double backRightJointSpeed{0}; + + /// \brief Lateral distance between left and right wheels public: double wheelSeparation{1.0}; + /// \brief Longitudinal distance between front and back wheels + public: double wheelbase{1.0}; + /// \brief Wheel radius public: double wheelRadius{0.2}; @@ -181,21 +205,36 @@ void MecanumDrive::Configure(const Entity &_entity, auto ptr = const_cast(_sdf.get()); // Get params from SDF - sdf::ElementPtr sdfElem = ptr->GetElement("left_joint"); + sdf::ElementPtr sdfElem = ptr->GetElement("front_left_joint"); while (sdfElem) { - this->dataPtr->leftJointNames.push_back(sdfElem->Get()); - sdfElem = sdfElem->GetNextElement("left_joint"); + this->dataPtr->frontLeftJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("front_left_joint"); } - sdfElem = ptr->GetElement("right_joint"); + sdfElem = ptr->GetElement("front_right_joint"); while (sdfElem) { - this->dataPtr->rightJointNames.push_back(sdfElem->Get()); - sdfElem = sdfElem->GetNextElement("right_joint"); + this->dataPtr->frontRightJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("front_right_joint"); + } + + sdfElem = ptr->GetElement("back_left_joint"); + while (sdfElem) + { + this->dataPtr->backLeftJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("back_left_joint"); + } + sdfElem = ptr->GetElement("back_right_joint"); + while (sdfElem) + { + this->dataPtr->backRightJointNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("back_right_joint"); } this->dataPtr->wheelSeparation = _sdf->Get("wheel_separation", this->dataPtr->wheelSeparation).first; + this->dataPtr->wheelbase = _sdf->Get("wheelbase", + this->dataPtr->wheelbase).first; this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", this->dataPtr->wheelRadius).first; @@ -320,15 +359,43 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // If the joints haven't been identified yet, look for them static std::set warnedModels; auto modelName = this->dataPtr->model.Name(_ecm); - if (this->dataPtr->leftJoints.empty() || - this->dataPtr->rightJoints.empty()) + if (this->dataPtr->frontLeftJoints.empty() || + this->dataPtr->frontRightJoints.empty() || + this->dataPtr->backLeftJoints.empty() || + this->dataPtr->backRightJoints.empty()) { bool warned{false}; - for (const std::string &name : this->dataPtr->leftJointNames) + for (const std::string &name : this->dataPtr->frontLeftJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->frontLeftJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find left joint [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->frontRightJointNames) { Entity joint = this->dataPtr->model.JointByName(_ecm, name); if (joint != kNullEntity) - this->dataPtr->leftJoints.push_back(joint); + this->dataPtr->frontRightJoints.push_back(joint); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find right joint [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->backLeftJointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + this->dataPtr->backLeftJoints.push_back(joint); else if (warnedModels.find(modelName) == warnedModels.end()) { ignwarn << "Failed to find left joint [" << name << "] for model [" @@ -337,11 +404,11 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } } - for (const std::string &name : this->dataPtr->rightJointNames) + for (const std::string &name : this->dataPtr->backRightJointNames) { Entity joint = this->dataPtr->model.JointByName(_ecm, name); if (joint != kNullEntity) - this->dataPtr->rightJoints.push_back(joint); + this->dataPtr->backRightJoints.push_back(joint); else if (warnedModels.find(modelName) == warnedModels.end()) { ignwarn << "Failed to find right joint [" << name << "] for model [" @@ -349,14 +416,20 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, warned = true; } } + if (warned) { warnedModels.insert(modelName); } } - if (this->dataPtr->leftJoints.empty() || this->dataPtr->rightJoints.empty()) + if (this->dataPtr->frontLeftJoints.empty() || + this->dataPtr->frontRightJoints.empty() || + this->dataPtr->backLeftJoints.empty() || + this->dataPtr->backRightJoints.empty()) + { return; + } if (warnedModels.find(modelName) != warnedModels.end()) { @@ -369,7 +442,39 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (_info.paused) return; - for (Entity joint : this->dataPtr->leftJoints) + for (Entity joint : this->dataPtr->frontLeftJoints) + { + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed}); + } + } + + for (Entity joint : this->dataPtr->frontRightJoints) + { + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent(joint, + components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}); + } + } + + for (Entity joint : this->dataPtr->backLeftJoints) { // Update wheel velocity auto vel = _ecm.Component(joint); @@ -377,15 +482,15 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (vel == nullptr) { _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); + joint, components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed})); } else { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); + *vel = components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed}); } } - for (Entity joint : this->dataPtr->rightJoints) + for (Entity joint : this->dataPtr->backRightJoints) { // Update wheel velocity auto vel = _ecm.Component(joint); @@ -393,29 +498,45 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (vel == nullptr) { _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); + components::JointVelocityCmd({this->dataPtr->backRightJointSpeed})); } else { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); + *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}); } } // Create the left and right side joint position components if they // don't exist. - auto leftPos = _ecm.Component( - this->dataPtr->leftJoints[0]); - if (!leftPos) + auto frontLeftPos = _ecm.Component( + this->dataPtr->frontLeftJoints[0]); + if (!frontLeftPos) { - _ecm.CreateComponent(this->dataPtr->leftJoints[0], + _ecm.CreateComponent(this->dataPtr->frontLeftJoints[0], components::JointPosition()); } - auto rightPos = _ecm.Component( - this->dataPtr->rightJoints[0]); - if (!rightPos) + auto frontRightPos = _ecm.Component( + this->dataPtr->frontRightJoints[0]); + if (!frontRightPos) { - _ecm.CreateComponent(this->dataPtr->rightJoints[0], + _ecm.CreateComponent(this->dataPtr->frontRightJoints[0], + components::JointPosition()); + } + + auto backLeftPos = _ecm.Component( + this->dataPtr->backLeftJoints[0]); + if (!backLeftPos) + { + _ecm.CreateComponent(this->dataPtr->backLeftJoints[0], + components::JointPosition()); + } + + auto backRightPos = _ecm.Component( + this->dataPtr->backRightJoints[0]); + if (!backRightPos) + { + _ecm.CreateComponent(this->dataPtr->backRightJoints[0], components::JointPosition()); } } @@ -536,10 +657,12 @@ void MecanumDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_in IGN_PROFILE("MecanumDrive::UpdateVelocity"); double linVel; + double latVel; double angVel; { std::lock_guard lock(this->mutex); linVel = this->targetVel.linear().x(); + latVel = this->targetVel.linear().y(); angVel = this->targetVel.angular().z(); } @@ -547,18 +670,28 @@ void MecanumDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_in // Limit the target velocity if needed. this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); + this->limiterLin->Limit(latVel, this->last0Cmd.lat, this->last1Cmd.lat, dt); this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); // Update history of commands. this->last1Cmd = last0Cmd; this->last0Cmd.lin = linVel; + this->last0Cmd.lat = latVel; this->last0Cmd.ang = angVel; + // constant used in computing target velocities + const double angularLength = 0.5 * (this->wheelSeparation + this->wheelbase); + const double invWheelRadius = 1 / this->wheelRadius; + // Convert the target velocities to joint velocities. - this->rightJointSpeed = - (linVel + angVel * this->wheelSeparation / 2.0) / this->wheelRadius; - this->leftJointSpeed = - (linVel - angVel * this->wheelSeparation / 2.0) / this->wheelRadius; + this->frontLeftJointSpeed = + (linVel - latVel - angVel * angularLength) * invWheelRadius; + this->frontRightJointSpeed = + (linVel + latVel + angVel * angularLength) * invWheelRadius; + this->backLeftJointSpeed = + (linVel + latVel - angVel * angularLength) * invWheelRadius; + this->backRightJointSpeed = + (linVel - latVel + angVel * angularLength) * invWheelRadius; } ////////////////////////////////////////////////// From f51eaf906bfee477143c3fb9fa63d10766c84ace Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Mar 2021 17:59:28 -0700 Subject: [PATCH 08/15] Disable odometry for now Signed-off-by: Steve Peters --- src/systems/mecanum_drive/MecanumDrive.cc | 155 ++++++++++++---------- 1 file changed, 82 insertions(+), 73 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 6ec39223eb..556c921b40 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -566,88 +566,97 @@ void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_in return; } - if (this->leftJoints.empty() || this->rightJoints.empty()) - return; - - // Get the first joint positions for the left and right side. - auto leftPos = _ecm.Component(this->leftJoints[0]); - auto rightPos = _ecm.Component( - this->rightJoints[0]); - - // Abort if the joints were not found or just created. - if (!leftPos || !rightPos || leftPos->Data().empty() || - rightPos->Data().empty()) + if (this->frontLeftJoints.empty() || this->backLeftJoints.empty() || + this->frontRightJoints.empty() || this->backRightJoints.empty()) { return; } - this->odom.Update(leftPos->Data()[0], rightPos->Data()[0], - std::chrono::steady_clock::time_point(_info.simTime)); + // Get the first joint positions. + auto frontLeftPos = _ecm.Component( + this->frontLeftJoints[0]); + auto frontRightPos = _ecm.Component( + this->frontRightJoints[0]); + auto backLeftPos = _ecm.Component( + this->backLeftJoints[0]); + auto backRightPos = _ecm.Component( + this->backRightJoints[0]); - // Throttle publishing - auto diff = _info.simTime - this->lastOdomPubTime; - if (diff > std::chrono::steady_clock::duration::zero() && - diff < this->odomPubPeriod) + // Abort if the joints were not found or just created. + if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos || + frontLeftPos->Data().empty() || frontRightPos->Data().empty() || + backLeftPos->Data().empty() || backRightPos->Data().empty()) { return; } - this->lastOdomPubTime = _info.simTime; - - // Construct the odometry message and publish it. - msgs::Odometry msg; - msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); - msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); - - math::Quaterniond orientation(0, 0, *this->odom.Heading()); - msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); - - msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); - msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); - - // Set the time stamp in the header - msg.mutable_header()->mutable_stamp()->CopyFrom( - convert(_info.simTime)); - - // Set the frame id. - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - if (this->sdfFrameId.empty()) - { - frame->add_value(this->model.Name(_ecm) + "/odom"); - } - else - { - frame->add_value(this->sdfFrameId); - } - - std::optional linkName = this->canonicalLink.Name(_ecm); - if (this->sdfChildFrameId.empty()) - { - if (linkName) - { - auto childFrame = msg.mutable_header()->add_data(); - childFrame->set_key("child_frame_id"); - childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); - } - } - else - { - auto childFrame = msg.mutable_header()->add_data(); - childFrame->set_key("child_frame_id"); - childFrame->add_value(this->sdfChildFrameId); - } - // Construct the Pose_V/tf message and publish it. - msgs::Pose_V tfMsg; - ignition::msgs::Pose *tfMsgPose = nullptr; - tfMsgPose = tfMsg.add_pose(); - tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); - tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); - tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); - - // Publish the messages - this->odomPub.Publish(msg); - this->tfPub.Publish(tfMsg); + // this->odom.Update(leftPos->Data()[0], rightPos->Data()[0], + // std::chrono::steady_clock::time_point(_info.simTime)); + + // // Throttle publishing + // auto diff = _info.simTime - this->lastOdomPubTime; + // if (diff > std::chrono::steady_clock::duration::zero() && + // diff < this->odomPubPeriod) + // { + // return; + // } + // this->lastOdomPubTime = _info.simTime; + + // // Construct the odometry message and publish it. + // msgs::Odometry msg; + // msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); + // msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); + + // math::Quaterniond orientation(0, 0, *this->odom.Heading()); + // msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); + + // msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); + // msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); + + // // Set the time stamp in the header + // msg.mutable_header()->mutable_stamp()->CopyFrom( + // convert(_info.simTime)); + + // // Set the frame id. + // auto frame = msg.mutable_header()->add_data(); + // frame->set_key("frame_id"); + // if (this->sdfFrameId.empty()) + // { + // frame->add_value(this->model.Name(_ecm) + "/odom"); + // } + // else + // { + // frame->add_value(this->sdfFrameId); + // } + + // std::optional linkName = this->canonicalLink.Name(_ecm); + // if (this->sdfChildFrameId.empty()) + // { + // if (linkName) + // { + // auto childFrame = msg.mutable_header()->add_data(); + // childFrame->set_key("child_frame_id"); + // childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); + // } + // } + // else + // { + // auto childFrame = msg.mutable_header()->add_data(); + // childFrame->set_key("child_frame_id"); + // childFrame->add_value(this->sdfChildFrameId); + // } + + // // Construct the Pose_V/tf message and publish it. + // msgs::Pose_V tfMsg; + // ignition::msgs::Pose *tfMsgPose = nullptr; + // tfMsgPose = tfMsg.add_pose(); + // tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); + // tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); + // tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); + + // // Publish the messages + // this->odomPub.Publish(msg); + // this->tfPub.Publish(tfMsg); } ////////////////////////////////////////////////// From 095e0d5b1e25acc676af02301a9571f0d5e6ae64 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Mar 2021 23:55:00 -0700 Subject: [PATCH 09/15] MecanumDrive: update documentation Signed-off-by: Steve Peters --- src/systems/mecanum_drive/MecanumDrive.cc | 3 +++ src/systems/mecanum_drive/MecanumDrive.hh | 28 +++++++++++++++-------- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index 556c921b40..ec8e141efd 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -693,6 +693,9 @@ void MecanumDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_in const double invWheelRadius = 1 / this->wheelRadius; // Convert the target velocities to joint velocities. + // These calculations are based on the following references: + // https://robohub.org/drive-kinematics-skid-steer-and-mecanum-ros-twist-included + // https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf this->frontLeftJointSpeed = (linVel - latVel - angVel * angularLength) * invWheelRadius; this->frontRightJointSpeed = diff --git a/src/systems/mecanum_drive/MecanumDrive.hh b/src/systems/mecanum_drive/MecanumDrive.hh index 52bf645565..e9db61c2c9 100644 --- a/src/systems/mecanum_drive/MecanumDrive.hh +++ b/src/systems/mecanum_drive/MecanumDrive.hh @@ -32,20 +32,30 @@ namespace systems // Forward declaration class MecanumDrivePrivate; - /// \brief Differential drive controller which can be attached to a model - /// with any number of left and right wheels. + /// \brief Mecanum drive controller which can be attached to a model + /// with any number of front/back left/right wheels. /// /// # System Parameters /// - /// ``: Name of a joint that controls a left wheel. This - /// element can appear multiple times, and must appear at least once. + /// ``: Name of a joint that controls a front left wheel. + /// This element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a right wheel. This - /// element can appear multiple times, and must appear at least once. + /// ``: Name of a joint that controls a front right wheel. + /// This element can appear multiple times, and must appear at least once. /// - /// ``: Distance between wheels, in meters. This element - /// is optional, although it is recommended to be included with an - /// appropriate value. The default value is 1.0m. + /// ``: Name of a joint that controls a back left wheel. + /// This element can appear multiple times, and must appear at least once. + /// + /// ``: Name of a joint that controls a back right wheel. + /// This element can appear multiple times, and must appear at least once. + /// + /// ``: Longitudinal distance between front and back wheels, + /// in meters. This element is optional, although it is recommended to be + /// included with an appropriate value. The default value is 1.0m. + /// + /// ``: Lateral distance between left and right wheels, + /// in meters. This element is optional, although it is recommended to be + /// included with an appropriate value. The default value is 1.0m. /// /// ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The From 0925a045ee5055880e1110874b087428771aabbd Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 22 Mar 2021 23:56:39 -0700 Subject: [PATCH 10/15] fix codecheck Signed-off-by: Steve Peters --- src/systems/mecanum_drive/MecanumDrive.cc | 28 ++++++++++++++--------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index ec8e141efd..ae69d90d34 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -56,7 +56,7 @@ struct Commands /// \brief Angular velocity. double ang; - Commands() : lin(0.0), ang(0.0) {} + Commands() : lin(0.0), lat(0.0), ang(0.0) {} }; class ignition::gazebo::systems::MecanumDrivePrivate @@ -449,8 +449,8 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (vel == nullptr) { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed})); + _ecm.CreateComponent(joint, + components::JointVelocityCmd({this->dataPtr->frontLeftJointSpeed})); } else { @@ -470,7 +470,8 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *vel = components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}); + *vel = + components::JointVelocityCmd({this->dataPtr->frontRightJointSpeed}); } } @@ -481,8 +482,8 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (vel == nullptr) { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed})); + _ecm.CreateComponent(joint, + components::JointVelocityCmd({this->dataPtr->backLeftJointSpeed})); } else { @@ -555,7 +556,8 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, +void MecanumDrivePrivate::UpdateOdometry( + const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { IGN_PROFILE("MecanumDrive::UpdateOdometry"); @@ -611,7 +613,8 @@ void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_in // msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); // msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); - // msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); + // msg.mutable_twist()->mutable_angular()->set_z( + // *this->odom.AngularVelocity()); // // Set the time stamp in the header // msg.mutable_header()->mutable_stamp()->CopyFrom( @@ -652,7 +655,8 @@ void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_in // tfMsgPose = tfMsg.add_pose(); // tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); // tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); - // tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); + // tfMsgPose->mutable_orientation()->CopyFrom( + // msg.mutable_pose()->orientation()); // // Publish the messages // this->odomPub.Publish(msg); @@ -660,7 +664,8 @@ void MecanumDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_in } ////////////////////////////////////////////////// -void MecanumDrivePrivate::UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, +void MecanumDrivePrivate::UpdateVelocity( + const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { IGN_PROFILE("MecanumDrive::UpdateVelocity"); @@ -719,4 +724,5 @@ IGNITION_ADD_PLUGIN(MecanumDrive, MecanumDrive::ISystemPreUpdate, MecanumDrive::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(MecanumDrive, "ignition::gazebo::systems::MecanumDrive") +IGNITION_ADD_PLUGIN_ALIAS(MecanumDrive, + "ignition::gazebo::systems::MecanumDrive") From 9bbccebb80aae51dd645ed55c8d6c25687b8f9dc Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Mar 2021 08:36:15 -0700 Subject: [PATCH 11/15] Use ignition::math::SpeedLimiter Signed-off-by: Steve Peters --- CMakeLists.txt | 2 +- examples/worlds/mecanum_drive.sdf | 2 + src/systems/mecanum_drive/CMakeLists.txt | 1 - src/systems/mecanum_drive/MecanumDrive.cc | 71 ++++---- src/systems/mecanum_drive/SpeedLimiter.cc | 200 ---------------------- src/systems/mecanum_drive/SpeedLimiter.hh | 130 -------------- 6 files changed, 34 insertions(+), 372 deletions(-) delete mode 100644 src/systems/mecanum_drive/SpeedLimiter.cc delete mode 100644 src/systems/mecanum_drive/SpeedLimiter.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 63d193da19..d8b5d867ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,7 +119,7 @@ set(IGN_RENDERING_VER ${ignition-rendering5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-math -ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.6) +ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.8) set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/mecanum_drive.sdf b/examples/worlds/mecanum_drive.sdf index aab6535291..51cca09e46 100644 --- a/examples/worlds/mecanum_drive.sdf +++ b/examples/worlds/mecanum_drive.sdf @@ -355,6 +355,8 @@ 1.25 1.511 0.3 + -5 + 5 diff --git a/src/systems/mecanum_drive/CMakeLists.txt b/src/systems/mecanum_drive/CMakeLists.txt index c0f2c42e70..820eac4a2a 100644 --- a/src/systems/mecanum_drive/CMakeLists.txt +++ b/src/systems/mecanum_drive/CMakeLists.txt @@ -1,7 +1,6 @@ gz_add_system(mecanum-drive SOURCES MecanumDrive.cc - SpeedLimiter.cc PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index ae69d90d34..d1e5c4637c 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -38,8 +39,6 @@ #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" -#include "SpeedLimiter.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -149,10 +148,10 @@ class ignition::gazebo::systems::MecanumDrivePrivate public: transport::Node::Publisher tfPub; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -238,57 +237,48 @@ void MecanumDrive::Configure(const Entity &_entity, this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", this->dataPtr->wheelRadius).first; - // Parse speed limiter parameters. - bool hasVelocityLimits = false; - bool hasAccelerationLimits = false; - bool hasJerkLimits = false; - double minVel = std::numeric_limits::lowest(); - double maxVel = std::numeric_limits::max(); - double minAccel = std::numeric_limits::lowest(); - double maxAccel = std::numeric_limits::max(); - double minJerk = std::numeric_limits::lowest(); - double maxJerk = std::numeric_limits::max(); + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) { - minVel = _sdf->Get("min_velocity"); - hasVelocityLimits = true; + double minVel = _sdf->Get("min_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minVel); + this->dataPtr->limiterAng->SetMinVelocity(minVel); } if (_sdf->HasElement("max_velocity")) { - maxVel = _sdf->Get("max_velocity"); - hasVelocityLimits = true; + double maxVel = _sdf->Get("max_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxVel); + this->dataPtr->limiterAng->SetMaxVelocity(maxVel); } if (_sdf->HasElement("min_acceleration")) { - minAccel = _sdf->Get("min_acceleration"); - hasAccelerationLimits = true; + double minAccel = _sdf->Get("min_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minAccel); + this->dataPtr->limiterAng->SetMinAcceleration(minAccel); } if (_sdf->HasElement("max_acceleration")) { - maxAccel = _sdf->Get("max_acceleration"); - hasAccelerationLimits = true; + double maxAccel = _sdf->Get("max_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); } if (_sdf->HasElement("min_jerk")) { - minJerk = _sdf->Get("min_jerk"); - hasJerkLimits = true; + double minJerk = _sdf->Get("min_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minJerk); + this->dataPtr->limiterAng->SetMinJerk(minJerk); } if (_sdf->HasElement("max_jerk")) { - maxJerk = _sdf->Get("max_jerk"); - hasJerkLimits = true; + double maxJerk = _sdf->Get("max_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxJerk); + this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); - - this->dataPtr->limiterAng = std::make_unique( - hasVelocityLimits, hasAccelerationLimits, hasJerkLimits, - minVel, maxVel, minAccel, maxAccel, minJerk, maxJerk); - double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) { @@ -680,12 +670,13 @@ void MecanumDrivePrivate::UpdateVelocity( angVel = this->targetVel.angular().z(); } - const double dt = std::chrono::duration(_info.dt).count(); - // Limit the target velocity if needed. - this->limiterLin->Limit(linVel, this->last0Cmd.lin, this->last1Cmd.lin, dt); - this->limiterLin->Limit(latVel, this->last0Cmd.lat, this->last1Cmd.lat, dt); - this->limiterAng->Limit(angVel, this->last0Cmd.ang, this->last1Cmd.ang, dt); + this->limiterLin->Limit( + linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt); + this->limiterLin->Limit( + latVel, this->last0Cmd.lat, this->last1Cmd.lat, _info.dt); + this->limiterAng->Limit( + angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); // Update history of commands. this->last1Cmd = last0Cmd; diff --git a/src/systems/mecanum_drive/SpeedLimiter.cc b/src/systems/mecanum_drive/SpeedLimiter.cc deleted file mode 100644 index 59b50f54b9..0000000000 --- a/src/systems/mecanum_drive/SpeedLimiter.cc +++ /dev/null @@ -1,200 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#include - -#include "SpeedLimiter.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -/// \brief Private SpeedLimiter data class. -class ignition::gazebo::systems::SpeedLimiterPrivate -{ - /// \brief Class constructor. - public: SpeedLimiterPrivate(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : hasVelocityLimits(_hasVelocityLimits), - hasAccelerationLimits(_hasAccelerationLimits), - hasJerkLimits(_hasJerkLimits), - minVelocity(_minVelocity), - maxVelocity(_maxVelocity), - minAcceleration(_minAcceleration), - maxAcceleration(_maxAcceleration), - minJerk(_minJerk), - maxJerk(_maxJerk) - { - } - - /// \brief Enable/Disable velocity. - public: bool hasVelocityLimits; - - /// \brief Enable/Disable acceleration. - public: bool hasAccelerationLimits; - - /// \brief Enable/Disable jerk. - public: bool hasJerkLimits; - - /// \brief Minimum velocity limit. - public: double minVelocity; - - /// \brief Maximum velocity limit. - public: double maxVelocity; - - /// \brief Minimum acceleration limit. - public: double minAcceleration; - - /// \brief Maximum acceleration limit. - public: double maxAcceleration; - - /// \brief Minimum jerk limit. - public: double minJerk; - - /// \brief Maximum jerk limit. - public: double maxJerk; -}; - -////////////////////////////////////////////////// -SpeedLimiter::SpeedLimiter(bool _hasVelocityLimits, - bool _hasAccelerationLimits, - bool _hasJerkLimits, - double _minVelocity, - double _maxVelocity, - double _minAcceleration, - double _maxAcceleration, - double _minJerk, - double _maxJerk) - : dataPtr(std::make_unique(_hasVelocityLimits, - _hasAccelerationLimits, _hasJerkLimits, _minVelocity, _maxVelocity, - _minAcceleration, _maxAcceleration, _minJerk, _maxJerk)) -{ -} - -////////////////////////////////////////////////// -SpeedLimiter::~SpeedLimiter() -{ -} - -////////////////////////////////////////////////// -double SpeedLimiter::Limit(double &_v, double _v0, double _v1, double _dt) const -{ - const double tmp = _v; - - this->LimitJerk(_v, _v0, _v1, _dt); - this->LimitAcceleration(_v, _v0, _dt); - this->LimitVelocity(_v); - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitVelocity(double &_v) const -{ - const double tmp = _v; - - if (this->dataPtr->hasVelocityLimits) - { - _v = ignition::math::clamp( - _v, this->dataPtr->minVelocity, this->dataPtr->maxVelocity); - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitAcceleration(double &_v, double _v0, double _dt) const -{ - const double tmp = _v; - - if (this->dataPtr->hasAccelerationLimits) - { - const double dvMin = this->dataPtr->minAcceleration * _dt; - const double dvMax = this->dataPtr->maxAcceleration * _dt; - - const double dv = ignition::math::clamp(_v - _v0, dvMin, dvMax); - - _v = _v0 + dv; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} - -////////////////////////////////////////////////// -double SpeedLimiter::LimitJerk(double &_v, double _v0, double _v1, double _dt) - const -{ - const double tmp = _v; - - if (this->dataPtr->hasJerkLimits) - { - const double dv = _v - _v0; - const double dv0 = _v0 - _v1; - - const double dt2 = 2. * _dt * _dt; - - const double daMin = this->dataPtr->minJerk * dt2; - const double daMax = this->dataPtr->maxJerk * dt2; - - const double da = ignition::math::clamp(dv - dv0, daMin, daMax); - - _v = _v0 + dv0 + da; - } - - if (ignition::math::equal(tmp, 0.0)) - return 1.0; - else - return _v / tmp; -} diff --git a/src/systems/mecanum_drive/SpeedLimiter.hh b/src/systems/mecanum_drive/SpeedLimiter.hh deleted file mode 100644 index 34286bc3ae..0000000000 --- a/src/systems/mecanum_drive/SpeedLimiter.hh +++ /dev/null @@ -1,130 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the PAL Robotics nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* - * Author: Enrique Fernández - * Modified: Carlos Agüero - */ - -#ifndef IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_SPEEDLIMITER_HH_ - -#include - -#include - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // Forward declaration. - class SpeedLimiterPrivate; - - /// \brief Class to limit velocity, acceleration and jerk. - /// \ref https://github.com/ros-controls/ros_controllers/tree/melodic-devel/diff_drive_controller - class SpeedLimiter - { - /// \brief Constructor. - /// \param [in] _hasVelocityLimits if true, applies velocity limits. - /// \param [in] _hasAccelerationLimits if true, applies acceleration limits. - /// \param [in] _hasJerkLimits if true, applies jerk limits. - /// \param [in] _minVelocity Minimum velocity [m/s], usually <= 0. - /// \param [in] _maxVelocity Maximum velocity [m/s], usually >= 0. - /// \param [in] _minAcceleration Minimum acceleration [m/s^2], usually <= 0. - /// \param [in] _maxAcceleration Maximum acceleration [m/s^2], usually >= 0. - /// \param [in] _minJerk Minimum jerk [m/s^3], usually <= 0. - /// \param [in] _maxJerk Maximum jerk [m/s^3], usually >= 0. - public: SpeedLimiter(bool _hasVelocityLimits = false, - bool _hasAccelerationLimits = false, - bool _hasJerkLimits = false, - double _minVelocity = 0.0, - double _maxVelocity = 0.0, - double _minAcceleration = 0.0, - double _maxAcceleration = 0.0, - double _minJerk = 0.0, - double _maxJerk = 0.0); - - /// \brief Destructor. - public: ~SpeedLimiter(); - - /// \brief Limit the velocity and acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double Limit(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Limit the velocity. - /// \param [in, out] _v Velocity [m/s]. - /// \return Limiting factor (1.0 if none). - public: double LimitVelocity(double &_v) const; - - /// \brief Limit the acceleration. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - public: double LimitAcceleration(double &_v, - double _v0, - double _dt) const; - - /// \brief Limit the jerk. - /// \param [in, out] _v Velocity [m/s]. - /// \param [in] _v0 Previous velocity to v [m/s]. - /// \param [in] _v1 Previous velocity to v0 [m/s]. - /// \param [in] _dt Time step [s]. - /// \return Limiting factor (1.0 if none). - /// \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control. - public: double LimitJerk(double &_v, - double _v0, - double _v1, - double _dt) const; - - /// \brief Private data pointer. - private: std::unique_ptr dataPtr; - }; - } -} -} -} - -#endif From 53792dfcc08f928f452b0142882239c48b26b079 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 23 Mar 2021 08:39:18 -0700 Subject: [PATCH 12/15] Use sphere visuals for wheels Signed-off-by: Steve Peters --- examples/worlds/mecanum_drive.sdf | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/examples/worlds/mecanum_drive.sdf b/examples/worlds/mecanum_drive.sdf index 51cca09e46..f284036a25 100644 --- a/examples/worlds/mecanum_drive.sdf +++ b/examples/worlds/mecanum_drive.sdf @@ -131,10 +131,10 @@ - - 0.6 0.6 0.6 + + - 0.6 0.6 0.6 + + 0.2 0.2 0.2 1 @@ -223,10 +223,10 @@ - - 0.6 0.6 0.6 + + 0.2 0.2 0.2 1 @@ -267,10 +267,10 @@ - - 0.6 0.6 0.6 + + 0.2 0.2 0.2 1 From 49eb8cb1948e488eb9550871d2b637a1376f7dc9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 24 Mar 2021 00:52:59 -0700 Subject: [PATCH 13/15] MecanumDrive: remove odometry Signed-off-by: Steve Peters --- src/systems/mecanum_drive/MecanumDrive.cc | 150 ---------------------- 1 file changed, 150 deletions(-) diff --git a/src/systems/mecanum_drive/MecanumDrive.cc b/src/systems/mecanum_drive/MecanumDrive.cc index d1e5c4637c..59dd701675 100644 --- a/src/systems/mecanum_drive/MecanumDrive.cc +++ b/src/systems/mecanum_drive/MecanumDrive.cc @@ -64,13 +64,6 @@ class ignition::gazebo::systems::MecanumDrivePrivate /// \param[in] _msg Velocity message public: void OnCmdVel(const ignition::msgs::Twist &_msg); - /// \brief Update odometry and publish an odometry message. - /// \param[in] _info System update information. - /// \param[in] _ecm The EntityComponentManager of the given simulation - /// instance. - public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); - /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation @@ -496,40 +489,6 @@ void MecanumDrive::PreUpdate(const ignition::gazebo::UpdateInfo &_info, *vel = components::JointVelocityCmd({this->dataPtr->backRightJointSpeed}); } } - - // Create the left and right side joint position components if they - // don't exist. - auto frontLeftPos = _ecm.Component( - this->dataPtr->frontLeftJoints[0]); - if (!frontLeftPos) - { - _ecm.CreateComponent(this->dataPtr->frontLeftJoints[0], - components::JointPosition()); - } - - auto frontRightPos = _ecm.Component( - this->dataPtr->frontRightJoints[0]); - if (!frontRightPos) - { - _ecm.CreateComponent(this->dataPtr->frontRightJoints[0], - components::JointPosition()); - } - - auto backLeftPos = _ecm.Component( - this->dataPtr->backLeftJoints[0]); - if (!backLeftPos) - { - _ecm.CreateComponent(this->dataPtr->backLeftJoints[0], - components::JointPosition()); - } - - auto backRightPos = _ecm.Component( - this->dataPtr->backRightJoints[0]); - if (!backRightPos) - { - _ecm.CreateComponent(this->dataPtr->backRightJoints[0], - components::JointPosition()); - } } ////////////////////////////////////////////////// @@ -542,115 +501,6 @@ void MecanumDrive::PostUpdate(const UpdateInfo &_info, return; this->dataPtr->UpdateVelocity(_info, _ecm); - this->dataPtr->UpdateOdometry(_info, _ecm); -} - -////////////////////////////////////////////////// -void MecanumDrivePrivate::UpdateOdometry( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) -{ - IGN_PROFILE("MecanumDrive::UpdateOdometry"); - // Initialize, if not already initialized. - if (!this->odom.Initialized()) - { - this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime)); - return; - } - - if (this->frontLeftJoints.empty() || this->backLeftJoints.empty() || - this->frontRightJoints.empty() || this->backRightJoints.empty()) - { - return; - } - - // Get the first joint positions. - auto frontLeftPos = _ecm.Component( - this->frontLeftJoints[0]); - auto frontRightPos = _ecm.Component( - this->frontRightJoints[0]); - auto backLeftPos = _ecm.Component( - this->backLeftJoints[0]); - auto backRightPos = _ecm.Component( - this->backRightJoints[0]); - - // Abort if the joints were not found or just created. - if (!frontLeftPos || !frontRightPos || !backLeftPos || !backRightPos || - frontLeftPos->Data().empty() || frontRightPos->Data().empty() || - backLeftPos->Data().empty() || backRightPos->Data().empty()) - { - return; - } - - // this->odom.Update(leftPos->Data()[0], rightPos->Data()[0], - // std::chrono::steady_clock::time_point(_info.simTime)); - - // // Throttle publishing - // auto diff = _info.simTime - this->lastOdomPubTime; - // if (diff > std::chrono::steady_clock::duration::zero() && - // diff < this->odomPubPeriod) - // { - // return; - // } - // this->lastOdomPubTime = _info.simTime; - - // // Construct the odometry message and publish it. - // msgs::Odometry msg; - // msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); - // msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); - - // math::Quaterniond orientation(0, 0, *this->odom.Heading()); - // msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); - - // msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); - // msg.mutable_twist()->mutable_angular()->set_z( - // *this->odom.AngularVelocity()); - - // // Set the time stamp in the header - // msg.mutable_header()->mutable_stamp()->CopyFrom( - // convert(_info.simTime)); - - // // Set the frame id. - // auto frame = msg.mutable_header()->add_data(); - // frame->set_key("frame_id"); - // if (this->sdfFrameId.empty()) - // { - // frame->add_value(this->model.Name(_ecm) + "/odom"); - // } - // else - // { - // frame->add_value(this->sdfFrameId); - // } - - // std::optional linkName = this->canonicalLink.Name(_ecm); - // if (this->sdfChildFrameId.empty()) - // { - // if (linkName) - // { - // auto childFrame = msg.mutable_header()->add_data(); - // childFrame->set_key("child_frame_id"); - // childFrame->add_value(this->model.Name(_ecm) + "/" + *linkName); - // } - // } - // else - // { - // auto childFrame = msg.mutable_header()->add_data(); - // childFrame->set_key("child_frame_id"); - // childFrame->add_value(this->sdfChildFrameId); - // } - - // // Construct the Pose_V/tf message and publish it. - // msgs::Pose_V tfMsg; - // ignition::msgs::Pose *tfMsgPose = nullptr; - // tfMsgPose = tfMsg.add_pose(); - // tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); - // tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); - // tfMsgPose->mutable_orientation()->CopyFrom( - // msg.mutable_pose()->orientation()); - - // // Publish the messages - // this->odomPub.Publish(msg); - // this->tfPub.Publish(tfMsg); } ////////////////////////////////////////////////// From 15416d004dd400a75b25e071ff0e21b6f69b975f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 24 Mar 2021 00:54:02 -0700 Subject: [PATCH 14/15] Fix xmlns and update comments Signed-off-by: Steve Peters --- examples/worlds/mecanum_drive.sdf | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/examples/worlds/mecanum_drive.sdf b/examples/worlds/mecanum_drive.sdf index f284036a25..458778ecdb 100644 --- a/examples/worlds/mecanum_drive.sdf +++ b/examples/worlds/mecanum_drive.sdf @@ -1,14 +1,12 @@ @@ -79,7 +77,7 @@ - + 0 2 0.325 0 -0 0 From bcfb8816c6d73e81dbf41e2341e78bdec9781c0e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 24 Mar 2021 00:56:15 -0700 Subject: [PATCH 15/15] rename skid_steer_mecanum example world Signed-off-by: Steve Peters --- .../worlds/{diff_drive_mecanum.sdf => skid_steer_mecanum.sdf} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename examples/worlds/{diff_drive_mecanum.sdf => skid_steer_mecanum.sdf} (99%) diff --git a/examples/worlds/diff_drive_mecanum.sdf b/examples/worlds/skid_steer_mecanum.sdf similarity index 99% rename from examples/worlds/diff_drive_mecanum.sdf rename to examples/worlds/skid_steer_mecanum.sdf index 6711c8f929..970facaabf 100644 --- a/examples/worlds/diff_drive_mecanum.sdf +++ b/examples/worlds/skid_steer_mecanum.sdf @@ -1,6 +1,6 @@