From 8ef7e33241051150c6f48ca1cfbc0a3b5a92238e Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Wed, 8 Jul 2020 13:51:18 +0100 Subject: [PATCH 1/5] [mecanum_gazebo_plugin] Improved wheel model --- mecanum_gazebo_plugin/CMakeLists.txt | 23 ++--- mecanum_gazebo_plugin/package.xml | 6 +- mecanum_gazebo_plugin/readme.md | 88 ++++++++++++++++++ mecanum_gazebo_plugin/src/mecanum_plugin.cpp | 94 ++++++-------------- 4 files changed, 133 insertions(+), 78 deletions(-) create mode 100644 mecanum_gazebo_plugin/readme.md diff --git a/mecanum_gazebo_plugin/CMakeLists.txt b/mecanum_gazebo_plugin/CMakeLists.txt index 7453c6e..9b60cdd 100644 --- a/mecanum_gazebo_plugin/CMakeLists.txt +++ b/mecanum_gazebo_plugin/CMakeLists.txt @@ -3,9 +3,10 @@ project(mecanum_gazebo_plugin) find_package(catkin REQUIRED COMPONENTS rosconsole roslint) find_package(gazebo) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") catkin_package( - # LIBRARIES mecanum_gazebo_plugin + #LIBRARIES mecanum_gazebo_plugin ) include_directories( @@ -13,15 +14,15 @@ include_directories( ${GAZEBO_INCLUDE_DIRS} ) -#add_library(${PROJECT_NAME} src/mecanum_plugin) -#target_link_libraries(${PROJECT_NAME} -# ${catkin_LIBRARIES} -# ${GAZEBO_LIBRARIES} -#) +add_library(${PROJECT_NAME} src/mecanum_plugin) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GAZEBO_LIBRARIES} +) -# roslint_cpp() -# roslint_add_test() +roslint_cpp() +roslint_add_test() -#install(TARGETS ${PROJECT_NAME} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -#) +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) diff --git a/mecanum_gazebo_plugin/package.xml b/mecanum_gazebo_plugin/package.xml index 93a4acc..0c96b25 100644 --- a/mecanum_gazebo_plugin/package.xml +++ b/mecanum_gazebo_plugin/package.xml @@ -2,10 +2,11 @@ mecanum_gazebo_plugin 0.1.1 - Plugin which uses directional friction to simulate mecanum wheels. + Plugin which uses a passive revolute joint aligned with the roller and a shphere shape to simulate mecanum wheels. - Mike Purvis + Vladimir Ivan Mike Purvis + Vladimir Ivan BSD @@ -13,6 +14,7 @@ catkin roslint + libgazebo9-dev gazebo rosconsole diff --git a/mecanum_gazebo_plugin/readme.md b/mecanum_gazebo_plugin/readme.md new file mode 100644 index 0000000..34cbf44 --- /dev/null +++ b/mecanum_gazebo_plugin/readme.md @@ -0,0 +1,88 @@ +# Macanum Gazebo Plugin + +This plugin simulates a mecanum wheel using a passive roller joint and a sphere collision shape. +For this to work, the wheel URDF has to be modified as follows: + + - The wheel joint should be `continuous` and acutated, e.g. using a transmission with an approriate hardware interface and the ros_control plugin. + - The wheel link should contain the visual elelment and a dummy mass and inertia but no collision shape. + - A second `continuous` joint should be added with zero origin. The axis is not important but as a convention it should be the same as the wheel joint axis. The joint should have a proxy roller link as a child. + - The proxy roller link should contain the wheel mass and inertia. It should also contain a sphere collision shape whe the same radius as the wheel. + + The proxy roller link emulates the passive rolling of the wheel. This plugin will then align the axis of the passive joint so that it matches with the axis of the roller that would be in contact with ground. + + The plugin has two parameters: + - `wheelLinkName`: the name of the wheel link + - `axis`: axis of the passive roller joint in the wheel frame, e.g., for mecanum wheels at 45deg angle this would be `[1 1 0]` (or similar, depending on how the wheel is mounted). The axis will be normalized. + +Friction parameters can be tuned via gazebo in the URDF. The wheel control can be set up and tuned to match the robot using ros_control and its gazebo plugin. The sphere collision model is not an accurate representation of the mecanum wheel. It allows the wheel to drive over obstacles sideways and the curvature of the sphere may not match the curvature of the rollers for all widths. It does allow the wheel to drive on uneven terrain and model the friction and it produces motion very similar to the real world behaviour. + +This plugin can also be used to simulate omni wheels by specifying the axis to be perpendicular to wheel axis. + +### Example + +Additional link and joint in the URDF: +``` + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + 1 + + +``` + +Plugin configuration for 4 mecanum wheels: +``` + + wheel_link_fl + -1 1 0 + + + wheel_link_fr + -1 -1 0 + + + wheel_link_rl + 1 1 0 + + + wheel_link_rr + 1 -1 0 + +``` diff --git a/mecanum_gazebo_plugin/src/mecanum_plugin.cpp b/mecanum_gazebo_plugin/src/mecanum_plugin.cpp index e7f8944..129b994 100644 --- a/mecanum_gazebo_plugin/src/mecanum_plugin.cpp +++ b/mecanum_gazebo_plugin/src/mecanum_plugin.cpp @@ -2,8 +2,8 @@ Software License Agreement (BSD) \file mecanum_plugin.cpp -\authors Mike Purvis -\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved. +\authors Mike Purvis , Vladimir Ivan +\copyright Copyright (c) 2020, Clearpath Robotics, Inc., The University of Edinburgh, All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -25,13 +25,11 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include -#include #include #include #include - namespace gazebo { @@ -47,10 +45,8 @@ class MecanumPlugin : public ModelPlugin event::ConnectionPtr update_connection_; physics::ModelPtr model_; physics::LinkPtr wheel_link_; - physics::LinkPtr fixed_link_; - double roller_angle_; - double roller_friction_; - double roller_skid_friction_; + physics::JointPtr passive_joint_; + ignition::math::v4::Vector3d axis_; }; // Register this plugin with the simulator @@ -79,52 +75,41 @@ void MecanumPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) return; } - if (_sdf->HasElement("fixedLinkName")) + if (_sdf->HasElement("axis")) { - link_name = _sdf->Get("fixedLinkName"); - fixed_link_ = model_->GetLink(link_name); - if (!fixed_link_) + std::istringstream iss(_sdf->Get("axis")); + std::vector numbers(std::istream_iterator{iss}, + std::istream_iterator()); + if (numbers.size() == 3) + { + axis_.X() = std::stod(numbers[0]); + axis_.Y() = std::stod(numbers[1]); + axis_.Z() = std::stod(numbers[2]); + axis_ = axis_.Normalize(); + if (axis_.Length() < 1.0 - std::numeric_limits::epsilon()) + { + ROS_FATAL_STREAM("The `axis` parameter is not a valid vector."); + return; + } + } + else { - ROS_FATAL_STREAM("Fixed link [" << link_name << "] not found!"); + ROS_FATAL("The mecanum plugin requires a valid `axis` parameter."); return; } + } else { - ROS_FATAL("The mecanum plugin requires a `fixedLinkName` parameter."); + ROS_FATAL("The mecanum plugin requires an `axis` parameter."); return; } - if (_sdf->HasElement("rollerAngle")) - { - roller_angle_ = _sdf->Get("rollerAngle"); - } - else - { - roller_angle_ = M_PI / 4; - } - - if (_sdf->HasElement("rollerFriction")) - { - roller_friction_ = _sdf->Get("rollerFriction"); - } - else - { - roller_friction_ = 100; - } - - if (_sdf->HasElement("rollerSkidFriction")) - { - roller_skid_friction_ = _sdf->Get("rollerSkidFriction"); - } - else - { - roller_skid_friction_ = 100000; - } + passive_joint_ = wheel_link_->GetChildJoints()[0]; ROS_INFO_STREAM("Mecanum plugin initialized for " << wheel_link_->GetName() << - ", referenced to " << fixed_link_->GetName() << ", with a roller " << - "angle of " << roller_angle_ << " radians."); + ", axis: [" << axis_.X() << ", " << axis_.Y() << ", " << + axis_.Z() <<"]"); // Register update event handler this->update_connection_ = event::Events::ConnectWorldUpdateBegin( @@ -133,29 +118,8 @@ void MecanumPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) void MecanumPlugin::GazeboUpdate() { - math::Pose wheel_pose = wheel_link_->GetWorldCoGPose(); - math::Pose fixed_pose = fixed_link_->GetWorldCoGPose(); - math::Quaternion wheel_orientation = wheel_pose.CoordRotationSub(fixed_pose.rot); - double wheel_angle = wheel_orientation.GetPitch(); - ROS_DEBUG_STREAM(wheel_link_->GetName() << " angle is " << wheel_angle << " radians."); - - // TODO: Understand better what the deal is with multiple collisions on a link. - unsigned int collision_index = 0; - physics::SurfaceParamsPtr surface = wheel_link_->GetCollision(collision_index)->GetSurface(); - - // TODO: Check that the physics engine is ODE to avoid a segfault here. - physics::ODESurfaceParams* ode_surface = dynamic_cast(surface.get()); - physics::FrictionPyramid& fric(ode_surface->frictionPyramid); - - // TODO: Parameterize these. - fric.SetMuPrimary(roller_skid_friction_); - fric.SetMuSecondary(roller_friction_); - - // TODO: Investigate replacing this manual trigonometry with Pose::rot::RotateVector. Doing this - // would also make it easier to support wheels which rotate about an axis other than Y. - fric.direction1.x = cos(roller_angle_) * cos(wheel_angle); - fric.direction1.y = sin(roller_angle_); - fric.direction1.z = cos(roller_angle_) * sin(wheel_angle); + // Re-align the passive hoint axis + passive_joint_->SetAxis(0, axis_); } } // namespace gazebo From 49e9ddfda228cc87d9ad79ad3b56febe59936f3f Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Wed, 8 Jul 2020 20:27:59 +0100 Subject: [PATCH 2/5] [mecanum_gazebo_plugin] Physics support check --- mecanum_gazebo_plugin/src/mecanum_plugin.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/mecanum_gazebo_plugin/src/mecanum_plugin.cpp b/mecanum_gazebo_plugin/src/mecanum_plugin.cpp index 129b994..f005467 100644 --- a/mecanum_gazebo_plugin/src/mecanum_plugin.cpp +++ b/mecanum_gazebo_plugin/src/mecanum_plugin.cpp @@ -57,6 +57,14 @@ void MecanumPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) // Store the model model_ = _parent; + auto physics = model_->GetWorld()->Physics()->GetType(); + if (physics != "ode" && physics != "dart") + { + ROS_FATAL("Only ODE and Dart physics engines are supported with mecanum wheels!"); + return; + } + + std::string link_name; if (_sdf->HasElement("wheelLinkName")) From a7fef33ab33869685dea6da852b39e0ccf41c325 Mon Sep 17 00:00:00 2001 From: Vladimir Date: Sat, 19 Jun 2021 17:13:44 +0100 Subject: [PATCH 3/5] Fixed typo --- mecanum_gazebo_plugin/src/mecanum_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_gazebo_plugin/src/mecanum_plugin.cpp b/mecanum_gazebo_plugin/src/mecanum_plugin.cpp index f005467..d395b45 100644 --- a/mecanum_gazebo_plugin/src/mecanum_plugin.cpp +++ b/mecanum_gazebo_plugin/src/mecanum_plugin.cpp @@ -126,7 +126,7 @@ void MecanumPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) void MecanumPlugin::GazeboUpdate() { - // Re-align the passive hoint axis + // Re-align the passive joint axis passive_joint_->SetAxis(0, axis_); } From 8bab51df26c75d06fa1b146b108548b3e8fc444c Mon Sep 17 00:00:00 2001 From: Vladimir Date: Thu, 24 Jun 2021 19:05:34 +0100 Subject: [PATCH 4/5] Removed comment --- mecanum_gazebo_plugin/CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mecanum_gazebo_plugin/CMakeLists.txt b/mecanum_gazebo_plugin/CMakeLists.txt index 9b60cdd..09a527b 100644 --- a/mecanum_gazebo_plugin/CMakeLists.txt +++ b/mecanum_gazebo_plugin/CMakeLists.txt @@ -5,9 +5,7 @@ find_package(catkin REQUIRED COMPONENTS rosconsole roslint) find_package(gazebo) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") -catkin_package( - #LIBRARIES mecanum_gazebo_plugin -) +catkin_package() include_directories( ${catkin_INCLUDE_DIRS} From 107bae4efb8a1cca5061f179454ddfb7a72ab125 Mon Sep 17 00:00:00 2001 From: Vladimir Date: Tue, 29 Jun 2021 01:25:26 +0100 Subject: [PATCH 5/5] Reverted maintainer change --- mecanum_gazebo_plugin/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mecanum_gazebo_plugin/package.xml b/mecanum_gazebo_plugin/package.xml index 0c96b25..13c02e7 100644 --- a/mecanum_gazebo_plugin/package.xml +++ b/mecanum_gazebo_plugin/package.xml @@ -4,7 +4,7 @@ 0.1.1 Plugin which uses a passive revolute joint aligned with the roller and a shphere shape to simulate mecanum wheels. - Vladimir Ivan + Mike Purvis Mike Purvis Vladimir Ivan