Improved mecanum wheel model

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}
+)

mecanum_gazebo_plugin/package.xml

@@ -2,17 +2,19 @@
 <package format="2">
   <name>mecanum_gazebo_plugin</name>
   <version>0.1.1</version>
-  <description>Plugin which uses directional friction to simulate mecanum wheels.</description>
+  <description>Plugin which uses a passive revolute joint aligned with the roller and a shphere shape to simulate mecanum wheels.</description>
 
   <maintainer email="[email protected]">Mike Purvis</maintainer>
   <author>Mike Purvis</author>
+  <author>Vladimir Ivan</author>
 
   <license>BSD</license>
 
   <url type="website">http://wiki.ros.org/mecanum_gazebo_plugin</url>
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roslint</build_depend>
+  <build_depend>libgazebo9-dev</build_depend>
   <depend>gazebo</depend>
   <depend>rosconsole</depend>
 </package>

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:
+```
+<!-- Proxy roller link -->
+<link name="wheel_link_fl_passive">
+    <inertial>
+        <mass value="10.0"/>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <inertia ixx="0.0441" ixy="0" ixz="0" iyy="0.0441" iyz="0" izz="0.0441"/>
+    </inertial>
+    <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+        <sphere radius="0.001"/>
+        </geometry>
+        <transparency>1</transparency>
+    </visual>
+    <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+        <sphere radius="0.105"/>
+        </geometry>
+    </collision>
+</link>
+
+<!-- Passive joint -->
+<joint name="wheel_joint_fl_passive" type="continuous">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <dynamics damping="0.0" friction="0.0"/>
+    <safety_controller k_velocity="10.0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="wheel_link_fl"/>
+    <child link="wheel_link_fl_passive"/>
+</joint>
+
+<!-- Actuation of the wheel -->
+<transmission name="wheel_joint_fl_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wheel_joint_fl">
+    <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="wheel_joint_fl_motor">
+    <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+</transmission>
+```
+
+Plugin configuration for 4 mecanum wheels:
+```
+<plugin name="wheel_controller_fl" filename="libmecanum_gazebo_plugin.so">
+    <wheelLinkName>wheel_link_fl</wheelLinkName>
+    <axis>-1 1 0</axis>
+</plugin>
+<plugin name="wheel_controller_fr" filename="libmecanum_gazebo_plugin.so">
+    <wheelLinkName>wheel_link_fr</wheelLinkName>
+    <axis>-1 -1 0</axis>
+</plugin>
+<plugin name="wheel_controller_rl" filename="libmecanum_gazebo_plugin.so">
+    <wheelLinkName>wheel_link_rl</wheelLinkName>
+    <axis>1 1 0</axis>
+</plugin>
+<plugin name="wheel_controller_rr" filename="libmecanum_gazebo_plugin.so">
+    <wheelLinkName>wheel_link_rr</wheelLinkName>
+    <axis>1 -1 0</axis>
+</plugin>
+```

mecanum_gazebo_plugin/src/mecanum_plugin.cpp

@@ -2,8 +2,8 @@
 Software License Agreement (BSD)
 
 \file      mecanum_plugin.cpp
-\authors   Mike Purvis <[email protected]>
-\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.
+\authors   Mike Purvis <[email protected]>, Vladimir Ivan <[email protected]>
+\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 <gazebo/common/Events.hh>
 #include <gazebo/common/Plugin.hh>
-#include <gazebo/physics/ode/ODESurfaceParams.hh>
 #include <gazebo/physics/physics.hh>
 #include <ros/console.h>
 
 #include <string>
 
-
 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<std::string>("fixedLinkName");
-    fixed_link_ = model_->GetLink(link_name);
-    if (!fixed_link_)
+    std::istringstream iss(_sdf->Get<std::string>("axis"));
+    std::vector<std::string> numbers(std::istream_iterator<std::string>{iss}, 
+                                    std::istream_iterator<std::string>());
+    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<double>::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<double>("rollerAngle");
-  }
-  else
-  {
-    roller_angle_ = M_PI / 4;
-  }
-
-  if (_sdf->HasElement("rollerFriction"))
-  {
-    roller_friction_ = _sdf->Get<double>("rollerFriction");
-  }
-  else
-  {
-    roller_friction_ = 100;
-  }
-
-  if (_sdf->HasElement("rollerSkidFriction"))
-  {
-    roller_skid_friction_ = _sdf->Get<double>("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<physics::ODESurfaceParams*>(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