Mecanum omnidirectional robot example (demo_IRR_mecanum.cpp)

Simulate a small mobile robot with omnidirectional 'mecanum' wheels.

Use keyboard to perform basic motion.

This tutorial shows how to:

  • make complex models, with 'barrel' collision shapes.
  • add motors between parts.
  • use the keyboard in Irrlicht 3D view, to interact with the system in realtime.
  • manage rotations of references using ChCoordsys and ChFrame classes.
// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Alessandro Tasora
// =============================================================================
//
// Demo code about
// - collisions and contacts
// - using the 'barrel' shape to create rollers for building omnidirectional
// wheels in a mobile robot.
//
// =============================================================================
#include "chrono/core/ChRealtimeStep.h"
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono/utils/ChUtilsGeometry.h"
#include "chrono/assets/ChBarrelShape.h"
#include "chrono_irrlicht/ChIrrApp.h"
// Use the namespaces of Chrono
using namespace chrono;
using namespace chrono::irrlicht;
// Use the main namespaces of Irrlicht
using namespace irr;
using namespace irr::core;
using namespace irr::scene;
using namespace irr::video;
using namespace irr::io;
using namespace irr::gui;
double STATIC_rot_speed = 0;
double STATIC_x_speed = 0;
double STATIC_z_speed = 0;
float STATIC_wheelfriction = 0.6f;
#define MAX_ROT_SPEED 0.8
#define MAX_XZ_SPEED 10
class MyEventReceiver : public IEventReceiver {
public:
MyEventReceiver(ChIrrAppInterface* myapp) {
// store pointer to physical system & other stuff so we can tweak them by user keyboard
app = myapp;
}
bool OnEvent(const SEvent& event) {
// check if user presses keys
if (event.EventType == irr::EET_KEY_INPUT_EVENT && !event.KeyInput.PressedDown) {
switch (event.KeyInput.Key) {
case irr::KEY_KEY_Q:
STATIC_x_speed += 1.5;
if (STATIC_x_speed > MAX_XZ_SPEED)
STATIC_x_speed = MAX_XZ_SPEED;
return true;
case irr::KEY_KEY_W:
STATIC_x_speed -= 1.5;
if (STATIC_x_speed < -MAX_XZ_SPEED)
STATIC_x_speed = -MAX_XZ_SPEED;
return true;
case irr::KEY_KEY_A:
STATIC_z_speed += 1.5;
if (STATIC_z_speed > MAX_XZ_SPEED)
STATIC_z_speed = MAX_XZ_SPEED;
return true;
case irr::KEY_KEY_Z:
STATIC_z_speed -= 1.5;
if (STATIC_z_speed < -MAX_XZ_SPEED)
STATIC_z_speed = -MAX_XZ_SPEED;
return true;
case irr::KEY_KEY_E:
STATIC_rot_speed += 0.05;
if (STATIC_rot_speed > MAX_ROT_SPEED)
STATIC_rot_speed = MAX_ROT_SPEED;
return true;
case irr::KEY_KEY_R:
STATIC_rot_speed -= 0.05;
if (STATIC_rot_speed < -MAX_ROT_SPEED)
STATIC_rot_speed = -MAX_ROT_SPEED;
return true;
default:
break;
}
}
return false;
}
private:
};
// This small function creates a Mecanum wheel, made with many ChBodySceneNode rigid bodies (a central
// wheel and the many radial rollers, already lined to the wheel with revolute joints.)
// The function returns the pointer to the central wheel.
std::shared_ptr<ChBody> create_mecanum_wheel(ChSystemNSC& mphysicalSystem,
ChVector<> shaft_position,
ChQuaternion<> shaft_alignment,
double wheel_radius,
double wheel_width,
int n_rollers,
double roller_angle,
double roller_midradius,
double roller_density,
double spindle_density) {
ChFrameMoving<> ftot(shaft_position, shaft_alignment); // will be used to transform pos & rot of all objects
auto mCentralWheel = chrono_types::make_shared<ChBodyEasyCylinder>(wheel_radius / 2, wheel_width, // radius, height
spindle_density, // density
true, // visualize
false); // no collision
mCentralWheel->SetPos(shaft_position);
mCentralWheel->SetRot(shaft_alignment);
mphysicalSystem.Add(mCentralWheel);
auto mtexturepw = chrono_types::make_shared<ChTexture>();
mtexturepw->SetTextureFilename(GetChronoDataFile("textures/pinkwhite.png"));
mCentralWheel->AddAsset(mtexturepw);
auto wheel_mat = chrono_types::make_shared<ChMaterialSurfaceNSC>();
wheel_mat->SetFriction(STATIC_wheelfriction);
double half_length_roller = 0.5 * wheel_width * 1.0 / (cos(roller_angle));
double roller_elliptical_rad_Hor = wheel_radius;
double roller_elliptical_rad_Vert = wheel_radius * 1.0 / (cos(roller_angle));
for (int iroller = 0; iroller < n_rollers; iroller++) {
double pitch = CH_C_2PI * ((double)iroller / (double)n_rollers);
double Roffset = -(wheel_radius - roller_midradius);
// Create the roller
auto mRoller = chrono_types::make_shared<ChBody>();
mphysicalSystem.Add(mRoller);
// move it to slanted aligment
ChFrameMoving<> f1( ChVector<>(0, 0, -(wheel_radius - roller_midradius)),
Q_from_AngAxis(roller_angle, ChVector<>(0, 0, 1)));
ChFrameMoving<> f2( ChVector<>(0, 0, 0),
Q_from_AngAxis(pitch, ChVector<>(0, 1, 0)));
ChFrameMoving<> f3 = f1 >> f2 >> ftot;
// approximate mass & inertia to a cylinder:
mRoller->SetMass(utils::CalcCylinderVolume(roller_elliptical_rad_Hor + Roffset, 2 * half_length_roller) * roller_density);
mRoller->SetInertia(utils::CalcCylinderGyration(roller_elliptical_rad_Hor + Roffset, 2 * half_length_roller) * roller_density);
// add collision shape
mRoller->GetCollisionModel()->ClearModel();
mRoller->GetCollisionModel()->AddBarrel(wheel_mat, //
-half_length_roller, +half_length_roller, //
roller_elliptical_rad_Vert, roller_elliptical_rad_Hor, //
Roffset);
mRoller->GetCollisionModel()->BuildModel();
mRoller->SetCollide(true);
// add visualization shape
auto mrollershape =
chrono_types::make_shared<ChBarrelShape>(-half_length_roller, +half_length_roller, //
roller_elliptical_rad_Vert, roller_elliptical_rad_Hor, //
Roffset);
mRoller->AddAsset(mrollershape);
// Make the revolute joint between the roller and the central wheel
// (preconcatenate rotation 90 degrees on X, to set axis of revolute joint)
ChFrameMoving<> fr(ChVector<>(0, 0, 0), Q_from_AngAxis(CH_C_PI / 2.0, ChVector<>(1, 0, 0)));
ChFrameMoving<> frabs = fr >> f3;
auto my_link_roller = chrono_types::make_shared<ChLinkLockRevolute>();
my_link_roller->Initialize(mRoller, mCentralWheel, frabs.GetCoord());
mphysicalSystem.AddLink(my_link_roller);
}
return mCentralWheel;
}
int main(int argc, char* argv[]) {
GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";
// Create a ChronoENGINE physical system
ChSystemNSC mphysicalSystem;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrApp application(&mphysicalSystem, L"Mecanum robot simulator", core::dimension2d<u32>(800, 600), false);
// create text with info
IGUIStaticText* textFPS = application.GetIGUIEnvironment()->addStaticText(
L"Use keys Q,W, A,Z, E,R to move the robot", rect<s32>(150, 10, 430, 40), true);
// Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
ChIrrWizard::add_typical_Logo(application.GetDevice());
ChIrrWizard::add_typical_Sky(application.GetDevice());
ChIrrWizard::add_typical_Lights(application.GetDevice());
ChIrrWizard::add_typical_Camera(application.GetDevice(), core::vector3df(0, 14, -20));
// This is for GUI tweaking of system parameters..
MyEventReceiver receiver(&application);
// note how to add a custom event receiver to the default interface:
application.SetUserEventReceiver(&receiver);
double platform_radius = 8;
double wheel_radius = 3;
double roller_angle = CH_C_PI / 4;
// Create the robot truss, as a circular platform
auto mTrussPlatform = chrono_types::make_shared<ChBodyEasyCylinder>(platform_radius * 0.7, 2, // radius, height
1000, // density
true, // visualize
false); // no collision
mphysicalSystem.Add(mTrussPlatform);
// ChCollisionModel::SetDefaultSuggestedEnvelope(0.01);
// ChCollisionModel::SetDefaultSuggestedMargin(0.005);
// create the wheels and link them to the platform
ChFrame<> f0(ChVector<>(0, 0, 0), Q_from_AngAxis(CH_C_PI / 2.0, ChVector<>(1, 0, 0)));
ChFrame<> f1(ChVector<>(0, 0, platform_radius), QUNIT);
ChFrame<> f2_wA(VNULL, Q_from_AngAxis(0 * (CH_C_2PI / 3.0), ChVector<>(0, 1, 0)));
ChFrame<> f2_wB(VNULL, Q_from_AngAxis(1 * (CH_C_2PI / 3.0), ChVector<>(0, 1, 0)));
ChFrame<> f2_wC(VNULL, Q_from_AngAxis(2 * (CH_C_2PI / 3.0), ChVector<>(0, 1, 0)));
ChFrame<> ftot_wA = f0 >> f1 >> f2_wA;
ChFrame<> ftot_wB = f0 >> f1 >> f2_wB;
ChFrame<> ftot_wC = f0 >> f1 >> f2_wC;
auto spindle_A =
create_mecanum_wheel(mphysicalSystem,
ftot_wA.GetCoord().pos, // wheel position
ftot_wA.GetCoord().rot, // wheel alignment
wheel_radius, // wheel radius
2.2, // wheel width
8, // n. of rollers
roller_angle, // angle of rollers
0.65, // max rad. of roller
1000, // density of roller
1000); // density of the spindle
auto my_link_shaftA = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
my_link_shaftA->Initialize(spindle_A, mTrussPlatform, (f1 >> f2_wA));
my_link_shaftA->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(0));
mphysicalSystem.AddLink(my_link_shaftA);
auto spindle_B =
create_mecanum_wheel(mphysicalSystem,
ftot_wB.GetCoord().pos, // wheel position
ftot_wB.GetCoord().rot, // wheel alignment
wheel_radius, // wheel radius
2.2, // wheel width
8, // n. of rollers
roller_angle, // angle of rollers
0.65, // max rad. of roller
1000, // density of roller
1000); // density of the spindle
auto my_link_shaftB = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
my_link_shaftB->Initialize(spindle_B, mTrussPlatform, (f1 >> f2_wB));
my_link_shaftB->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(0));
mphysicalSystem.AddLink(my_link_shaftB);
auto spindle_C =
create_mecanum_wheel(mphysicalSystem,
ftot_wC.GetCoord().pos, // wheel position
ftot_wC.GetCoord().rot, // wheel alignment
wheel_radius, // wheel radius
2.2, // wheel width
8, // n. of rollers
roller_angle, // angle of rollers
0.65, // max rad. of roller
1000, // density of roller
1000); // density of the spindle
auto my_link_shaftC = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
my_link_shaftC->Initialize(spindle_C, mTrussPlatform, (f1 >> f2_wC));
my_link_shaftC->SetSpeedFunction(chrono_types::make_shared<ChFunction_Const>(0));
mphysicalSystem.AddLink(my_link_shaftC);
// Create the ground for the collision
auto ground_mat = chrono_types::make_shared<ChMaterialSurfaceNSC>();
ground_mat->SetFriction(STATIC_wheelfriction);
auto ground = chrono_types::make_shared<ChBodyEasyBox>(200, 1, 200, // size
1000, // density
true, // visualize
true, // collide
ground_mat); // contact material
ground->SetPos(ChVector<>(0, -5, 0));
ground->SetBodyFixed(true);
mphysicalSystem.Add(ground);
auto mtexture = chrono_types::make_shared<ChTexture>();
mtexture->SetTextureFilename(GetChronoDataFile("textures/concrete.jpg"));
mtexture->SetTextureScale(100, 100);
ground->AddAsset(mtexture);
// Use this function for adding a ChIrrNodeAsset to all already created items.
// Otherwise use application.AssetBind(myitem); on a per-item basis.
application.AssetBindAll();
application.AssetUpdateAll();
// Prepare the physical system for the simulation
mphysicalSystem.SetTimestepperType(ChTimestepper::Type::EULER_IMPLICIT_PROJECTED);
mphysicalSystem.SetSolverMaxIterations(30);
//
// THE SOFT-REAL-TIME CYCLE
//
application.SetTimestep(0.01);
application.SetTryRealtime(true);
while (application.GetDevice()->run()) {
application.BeginScene(true, true, SColor(255, 140, 161, 192));
application.DrawAll();
// ADVANCE THE SIMULATION FOR ONE TIMESTEP
application.DoStep();
// change motor speeds depending on user setpoints from GUI
ChVector<> imposed_speed(STATIC_x_speed, 0, STATIC_z_speed);
ChFrame<> roll_twist(ChVector<>(0, -wheel_radius, 0), Q_from_AngAxis(-roller_angle, ChVector<>(0, 1, 0)));
ChFrame<> abs_roll_wA = roll_twist >> f2_wA >> ChFrame<>(mTrussPlatform->GetCoord());
double wheel_A_rotspeed =
(STATIC_rot_speed * platform_radius) +
((abs_roll_wA.GetA().transpose() * imposed_speed).x() / sin(roller_angle)) / wheel_radius;
ChFrame<> abs_roll_wB = roll_twist >> f2_wB >> ChFrame<>(mTrussPlatform->GetCoord());
double wheel_B_rotspeed =
(STATIC_rot_speed * platform_radius) +
((abs_roll_wB.GetA().transpose() * imposed_speed).x() / sin(roller_angle)) / wheel_radius;
ChFrame<> abs_roll_wC = roll_twist >> f2_wC >> ChFrame<>(mTrussPlatform->GetCoord());
double wheel_C_rotspeed =
(STATIC_rot_speed * platform_radius) +
((abs_roll_wC.GetA().transpose() * imposed_speed).x() / sin(roller_angle)) / wheel_radius;
if (auto mfun = std::dynamic_pointer_cast<ChFunction_Const>(my_link_shaftA->GetSpeedFunction()))
mfun->Set_yconst(wheel_A_rotspeed);
if (auto mfun = std::dynamic_pointer_cast<ChFunction_Const>(my_link_shaftB->GetSpeedFunction()))
mfun->Set_yconst(wheel_B_rotspeed);
if (auto mfun = std::dynamic_pointer_cast<ChFunction_Const>(my_link_shaftC->GetSpeedFunction()))
mfun->Set_yconst(wheel_C_rotspeed);
application.EndScene();
}
return 0;
}
std::string GetChronoDataFile(const std::string &filename)
Obtain the complete path to the specified filename, given relative to the Chrono data directory (thre...
Definition: ChGlobal.cpp:95
void Add(std::shared_ptr< ChPhysicsItem > item)
Attach an arbitrary ChPhysicsItem (e.g.
Definition: ChSystem.cpp:146
static void add_typical_Camera(irr::IrrlichtDevice *device, irr::core::vector3df mpos=irr::core::vector3df(0, 0, -8), irr::core::vector3df mtarg=irr::core::vector3df(0, 0, 0))
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
Definition: ChIrrWizard.cpp:53
ChLog & GetLog()
Global function to get the current ChLog object.
Definition: ChLog.cpp:39
virtual void AddLink(std::shared_ptr< ChLinkBase > link)
Attach a link to the underlying assembly.
Definition: ChSystem.h:268
void SetTimestep(double val)
Set/Get the time step for time integration.
Definition: ChIrrAppInterface.cpp:558
static void add_typical_Logo(irr::IrrlichtDevice *device, const std::string &mlogofilename=GetChronoDataFile("logo_chronoengine_alpha.png"))
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
Definition: ChIrrWizard.cpp:20
static void add_typical_Sky(irr::IrrlichtDevice *device, const std::string &mtexturedir=GetChronoDataFile("skybox/"))
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
Definition: ChIrrWizard.cpp:40
static void add_typical_Lights(irr::IrrlichtDevice *device, irr::core::vector3df pos1=irr::core::vector3df(30.f, 100.f, 30.f), irr::core::vector3df pos2=irr::core::vector3df(30.f, 80.f, -30.f), double rad1=290, double rad2=190, irr::video::SColorf col1=irr::video::SColorf(0.7f, 0.7f, 0.7f, 1.0f), irr::video::SColorf col2=irr::video::SColorf(0.7f, 0.8f, 0.8f, 1.0f))
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
Definition: ChIrrWizard.cpp:25
ChFrame: a class for coordinate systems in 3D space.
Definition: ChFrame.h:42
Class to add some GUI to Irrlicht+ChronoEngine applications.
Definition: ChIrrApp.h:29
ChQuaternion< double > Q_from_AngAxis(double angle, const ChVector< double > &axis)
Get the quaternion from an angle of rotation and an axis, defined in abs coords.
Definition: ChQuaternion.cpp:100
const ChApi ChQuaternion< double > QUNIT
Constant unit quaternion: {1, 0, 0, 0} , corresponds to no rotation (diagonal rotation matrix)
virtual void EndScene()
Call this to end the scene draw at the end of each animation frame.
Definition: ChIrrAppInterface.cpp:634
Namespace with classes for the Irrlicht module.
Definition: ChApiIrr.h:48
Projected SOR (Successive Over-Relaxation)
Definition of general purpose 3d vector variables, such as points in 3D.
Definition: ChVector.h:35
virtual void DoStep()
Call this function inside a loop such as.
Definition: ChIrrAppInterface.cpp:644
void SetSolverType(ChSolver::Type type)
Choose the solver type, to be used for the simultaneous solution of the constraints in dynamical simu...
Definition: ChSystem.cpp:214
void SetSolverMaxIterations(int max_iters)
Set the maximum number of iterations, if using an iterative solver.
Definition: ChSystem.cpp:188
ChFrameMoving: a class for coordinate systems in 3D space.
Definition: ChFrameMoving.h:38
ChCoordsys< Real > & GetCoord()
Return both current rotation and translation as a coordsystem object, with vector and quaternion.
Definition: ChFrame.h:201
Class defining quaternion objects, that is four-dimensional numbers, also known as Euler parameters.
Definition: ChQuaternion.h:45
Class to add some GUI to Irrlicht + ChronoEngine applications.
Definition: ChIrrAppInterface.h:45
virtual void BeginScene(bool backBuffer=true, bool zBuffer=true, irr::video::SColor color=irr::video::SColor(255, 0, 0, 0))
Call this to clean the canvas at the beginning of each animation frame.
Definition: ChIrrAppInterface.cpp:617
Main namespace for the Chrono package.
Definition: ChAsset.cpp:18
void SetTimestepperType(ChTimestepper::Type type)
Set the method for time integration (time stepper type).
Definition: ChSystem.cpp:348
ChMatrix33< Real > & GetA()
Return the current rotation as a 3x3 matrix.
Definition: ChFrame.h:213
virtual void DrawAll()
Call this function inside a loop such as.
Definition: ChIrrAppInterface.cpp:757
Class for a physical system in which contact is modeled using a non-smooth (complementarity-based) me...
Definition: ChSystemNSC.h:29
void SetTryRealtime(bool val)
If enabled, the function DoStep() will enforce soft real-time, by spinning in place until simulation ...
Definition: ChIrrAppInterface.h:94
void SetUserEventReceiver(irr::IEventReceiver *mreceiver)
Use this function to hook a custom event receiver to the application.
Definition: ChIrrAppInterface.h:151
void ConcatenatePreTransformation(const ChFrameMoving< Real > &T)
Apply a transformation (rotation and translation) represented by another ChFrameMoving T.
Definition: ChFrameMoving.h:329