diff --git a/mecanum_gazebo_plugin/CMakeLists.txt b/mecanum_gazebo_plugin/CMakeLists.txt index 7453c6e..09a527b 100644 --- a/mecanum_gazebo_plugin/CMakeLists.txt +++ b/mecanum_gazebo_plugin/CMakeLists.txt @@ -3,25 +3,24 @@ 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 -) +catkin_package() include_directories( ${catkin_INCLUDE_DIRS} ${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..13c02e7 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 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..d395b45 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 @@ -61,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")) @@ -79,52 +83,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) { - ROS_FATAL_STREAM("Fixed link [" << link_name << "] not found!"); + 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("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 +126,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 joint axis + passive_joint_->SetAxis(0, axis_); } } // namespace gazebo