29
User Manual 0.9.0 Jaeyoung Haan 2009-03-06

User Manual - [email protected]/srlib/files/manual.pdf · User Manual 0.9.0 Jaeyoung Haan 2009-03-06. Contents ... • provides a simple OpenGL viewer. • uses MKS unit

  • Upload
    others

  • View
    5

  • Download
    0

Embed Size (px)

Citation preview

User Manual0.9.0

Jaeyoung Haan

2009-03-06

Contents

1 Introduction to srLib 1

1.1 History . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Features . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.3 License . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.4 Version History . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.5 Official site and user forum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 srLib Overview 3

2.1 Concept and structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2 Space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.3 System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.4 Link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.5 Joint and actuation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.6 Collision object . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.7 Sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3 Getting Started 11

3.1 Download and Install . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2 Basics Usage of srLib . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

4 Tutorials 17

4.1 Tutorial 1: Mobile Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

Chapter 1

Introduction to srLib

srLib is an open source library for simulating multi-body rigid dynamics with collision detection andcontact behavior, especially supporting for robotics research.

srLib is based on LieGroup and recursive dynamics.The name srLib comes from Seoul National University Robotics Lab Library.

1.1 History

The very prototype of srLib was the result from a research project of Robotics Lab working for HyundaiHeavy Industry Inc. in 2002. From 2005 there were a few refinements of simulation library, and finally Nov2008 srLib was released.

1.2 Features

srLib

• is best for simulating articulated rigid body robots as well as wheeled robots.• carries fast and accurate dynamics calculation based on exponential formula and recursive dynamics

algorithms.• supports arbitrary mass and inertia distributed objects.• supports some types of joint exactly constrained: weld(fix), revolute(hinge), prismatic(slide), univer-

sal, and more will be added.• provides collision shape primitives: sphere, box, capsule(capped cylinder), plane, and ray.• provides some kinds of sensors: touch, IR spot, and IR scanner(range-finder).• solves contact and friction with PGS solver for LCP based on Coloumb friction model.• supports cross platform.• is written in C++.• provides a simple OpenGL viewer.• uses MKS unit. 1 means 1kg for mass, 1m for length, 1N for force, 1Nm for torque, and so forth.• is easy to create your own robot using relative relationships between objects.• ...

2 Introduction to srLib

1.3 License

srLib is Copyright 2008-2009 Robotics Lab. All rights reserved.This library is open source, free software without any warranty.

1.4 Version History

• srLib 0.9.4 (Mar 1, 2009)IR sensor, Range finder sensor bug fixed.Drawing IR sensor and Range finder sensor added.“GettingStarted” example updated: IR sensor & Range finder added to a mobile robot, controlchanged: ’w’-forward, ’s’-backward, ’a’-turn left, ’d’-turn right, ’x’-stop.Makefile fixed: “include” & “lib” directory path fixed.

• srLib 0.9.3 (Feb 23, 2009)Project for Xcode added.

• srLib 0.9.3 (Jan 29, 2009)Bug in collision between capsules fixed.Solution for Visual Studio 2005 added.

• srLib 0.9.2 (Jan 18, 2009)srLink, srJoint initializing bug fixed.

• srLib 0.9.1 (Nov 19, 2008)• srLib 0.9.0 (Nov 12, 2008)

First release of srLib.

1.5 Official site and user forum

Please visit and leave your valuable bug reports, comments, suggestions, or any criticisms.srLib site is http://www.r-station.co.kr/srlib.User forum site is http://www.r-station.co.kr/forum.

Chapter 2

srLib Overview

2.1 Concept and structure

So called “dynamics simulation” has been regarded as complex and difficult to learn how to use andconfigure it. srLib has been developed aiming for “EASE to USE” as if assembling Lego blocks, as wellas fast and accurate calculation.

srLib has two conceptual objects and four primitive objects in order to express robots flexibly,which are:

• Space(srSpace class): representing a whole 3D space and configuring its parameters, i.e. gravity,simulation time step.

• System(srSystem class): representing a robot which consists of links, joints, collision objects andsensors connected with each other.

• Link(srLink class): representing an object having mass and inertia.

• Joint(derived classes from srJoint): expressing the relationship between two links.

• Collision(srCollision): necessary to resolve collision and contact phenomena between links.

• Sensor(derived classed from srSensor): emulating real sensors.

srLink srJoint srCollision srSensor

srGeometryInfosrSpace srSystem srEntity

srObject

Figure 2.1: Class Diagram – Main Data Structure for Modeling

4 srLib Overview

2.2 Space

srSpace class represents a whole 3D space user construct. This class instance can exist only one at asimulation.

2.3 System

srSystem class represents an individual robot which user build, that is, one srSystem instance representsone robot.

2.4 Link

srLink class represents an object which has mass and inertia so that is ruled by physics. 4 kind of primitiveshapes are provided–sphere, box, capsule, and cylinder in addition to user shape which allows user to setmass and inertia directly. In case of primitive shapes, mass and inertia are calculated automatically.For simulation of collision and contact, link should have collision objects. A link can have multiple collisionobjects. For instance, simulaing with a link having a complicated outer shell or a concave shape, you canwrap this link with several primitive shapes.Inertia of link is consist of mass, moment of inertia, and center of mass. This is automatically determinedor user can specify it directly.Link can be connected to number of joints. One of them be a parent joint, the others be children.

srLink::m_ParentJoint

srLink::m_ParentLink

srLink::m_ChildJoints

Root Leaf

LINK

Figure 2.2:

2.5 Joint and actuation

srJoint and its derived classed describe relations between two links. “Joint” is not a real object but expres-sion explaining constrained movement of a link to the other link. In srLib, however, joint is implementedas an object user can watch and manipulate.Joint has only and always two links, while link can have many joints. Since joints having no links do notaffect dynamics, these are not taken account in dynamics calculation.User should specify parent/child link(srLink pointer) and relative frames(SE(3)) from each to joint rela-tively. Parent link is in a direction for Base Link, child link is in a direction for leaf.

2.5 Joint and actuation 5

LINK ALINK B

srJoint::m_ParentLinkToJoint srJoint::m_ChildLinkToJoint

srJoint::m_ParentLink srJoint::m_ChildLink

srLink(A)::m_Frame

Origin of the global space

srLink(B)::m_Frame

JOINT

Figure 2.3:

Joint Actuation

srJoint provides four kind of actuator mode: passive, torque, velocity and acceleration.

Passive mode is a default. You can not actuate this. By setting spring and damping constants, jointspringback or energy dissipation can be simulated. Prismatic joint can act as if it is a shock absorber.

Torque is a classic input of forward dynamics. You can actuate joints by inputting torque directly.

Velocity mode is useful when you simulate wheel. When you use this mode, you should specifymax torque value. srLib automatically generates an exact amount of torque in order to produce givenvelocity by user. Since max torque is bounded, you can simulate real servo motors.

Hybrid is a special mode. This mode assumes that actuation torque is unbounded. Input value isan acceleration of joint and joint follows exactly this acceleration. This is quite similar to just kinematicsimulation w/o dynamics. However, interactions with environments such as collision, friction, etc., arefully taken account and simulated in dynamics. This mode will be good when you simulate an articulatedrobot like a manipulator or a humanoid.

Weld joint

Weld joint fixes two links to each other. If link’s shape is so complicated that you could not get mass andinertia at once, you divide a link into several simple-shape links such as sphere, box, and so forth, andconnect them using this weld joint.

LINK A

LINK B

FIXED

JOINT

XY

Z

Figure 2.4:

6 srLib Overview

Revolute joint

Revolute joint acts as a hinge or a motor. Rotation axis is fixed as z-axis of frame of joint. Rotation angle ismeasured with respect to a parent link. In the figure above, suppose that LINK A is a parent link of JOINTand LINK B is a child. If joint rotates in CCW(counter clockwise) so LINK B does then joint angle valueincreases.

LINK A

LINK B

JOINT

XY

Z

Figure 2.5:

Prismatic joint

Prismatic joint is moving linearly–sliding. Sliding direction is along z-axis of frame of joint.

LINK A

LINK B

JOINT

XY

Z

Figure 2.6:

Universal joint

Universal joint can moves in two directions simultaneously. First axis is coaxial with z-axis, second one iswith x-axis of join frame. Joint rotates along z-axis first, then rotates along x-axis.

LINK A

LINK B

JOINTX

YZ

Figure 2.7:

2.6 Collision object 7

2.6 Collision object

Collision is an object for simulating collision and contact behaviors between links. There are five primitiveswhich can collide with each other: sphere, box, capsule, cylinder, and plane. A link can have multiplecollision primitives. Collision object can not exist solely without connection with any link.

LINK COLLISION

srCollision::m_pLink

srCollision::m_LocalFrame

Bounding Sphere

srColl

ision::

m_Bou

nding

Radius

Figure 2.8:

2.7 Sensor

srLib provides emulations of real sensors such as infra-red sensor, range finder, and touch sensor. Link canhave multiple sensors. Sensors can not exist solely without connection with any link.

LINK

SENSOR

srSensor::m_pLink

srSensor::m_LocalFrame

Figure 2.9:

Touch sensor

Touch sensor acts exactly same as sphere collision object, but it returns only boolean. True for collided,false for nod-collided. Range is a radius of touch sensor(sphere).

8 srLib Overview

LINK TOUCHSENSOR

srTouchSensor::m_Range

OBSTACLE

Collision Occurs!

Figure 2.10:

IR sensor

IR ray starts from the origin of IR sensor’s frame along with the x-axis with respect to the local coordinate.

LINK

IR SENSOR

OBSTACLE

Detect!

srIRSensor::m_Range

Figure 2.11:

Range finder (IR scanner)

Range Finder scans from -spread/2 to spread/2 on xy plane of the local frame. Resolution is an anglebetween adjacent IR spots. Spread is an angle of detecting range. The number of total spots are (spread /resolution + 1).

2.7 Sensor 9

OBSTACLEDetect!

LINK

RANGEFINDER

srRangeFinder::m_Resolution

srRangeFinder::m_Spread

srRangeFinder::m_Range

Figure 2.12:

10 srLib Overview

Chapter 3

Getting Started

3.1 Download and Install

Download srLib from the official site http://r-station.co.kr/srlib or user forum site http://r-station.co.kr/forum suitable for your operating system.srLib uses the OpenGL, the OpenGL Utility Toolkit(GLUT) and the OpenGL Extension Wrangler Li-brary(GLEW), therefore it requires glut and glew to be installed on your system.

Install on Linux machine & Mac OS X(terminal)

Install glut and glew using a package manager of your linux system. In case of Mac, use MacPorts or Fink.

• Download “srLib for Linux” and unzip it in an appropriate folder.• Make sure there are “include” and “lib” folders under “srLib-<version>” folder.• Open “srLib-<version>/src” folder. You can see 5 library folders; common, LieGroup, srDyn, srg,

and Renderer.• Compile sources of each folder in following order by typing “make new deploy”.

(common -> LieGroup -> srDyn -> srg -> Renderer)• Then, header files and lib files must be copied into “srLib-<version>/include” and “srLib-

<version>/lib” repectively.• Open “srLib-<version>/Examples” folder.• Choose one of examples and open it.• Rename “Makefile.LINUX” or “Makefile.MACOSX” Makefile.• “make”• Make your own robot world using an example source file.

Install on Mac OS X (Xcode)

srLib for Xcode 3.0 or higher version.

Install glut and glew using a package manager for Mac OSX, MacPorts or Fink. In this manual, weassume user use MacPorts.

• Download “srLib for Mac OSX” and unzip it in an appropriate folder.• Open “srLib-<version>/Xcode3.0” folder.

12 Getting Started

• Open “Xcode3.0.xcodeproj” file.• Set one of Examples as Active Target.• Build and Go (Command()+Return).

*Every header and source files are linked relative path to Xcode project except “framework”. So re-settingframework might be necessary. You can see the framework links in “Xcode3.0 > framework” of the left paneof Xcode. OpenGL.framework & GLUT.framework are in “/System/Library/Frameworks/” folder. But,libGLEW.a location depends on your glew configuration. If you install glew using MacPorts, libGLEW.amust be in “/opt/local/lib/” folder.

Install on Windows

srLib for Microsoft Visual Studio 2005, 2008, or 2008 Express Edition.

• Download “srLib for Windows” and unzip it in an appropriate folder.• You can see both “VS2005” and “VS2008” folders in the unzipped “srLib-<version>” folder.• Choose one suitable for your IDE.• Open “srLib.sln” file.• Right Click on one of example projects and choose “Set as StartUp Project” at context menu.• Build and run.• Build your own robot world using an example project as a template.

Install glut & glew on WindowsDownload glut 3.7 and glew 1.5.1 binaries, and unzip them.

• Copy * .h files of each into “(Folder A)\include\GL\” folder.• Copy * .lib files into “(Folder A)\lib\” folder.• Copy * .dll files into “C:\Windows\System32\” folder.

Folder A (*Generally, “(your install dir)” is “C:\Program Files”)VS 2005 (your install dir)\Microsoft Visual Studio 8\VC\PlatformSDK\VS 2008 (your install dir)\Microsoft SDKs\

3.2 Basics Usage of srLib

How to use “EulerZYX function”

SE3 EulerZYX(const Vec3 &angle, const Vec3 &position)

This function takes Euler ZYX angle and position then returns SE(3) corresponding to angle and position.

How to make a space

#include "../../src/srDyn/srSpace.h"

srSpace gSpace;

3.2 Basics Usage of srLib 13

How to make a link

Declare srLink instance.

srLink gLink;

Set geometry information.

gLink.GetGeomInfo().SetShape(srGeometry::BOX);gLink.GetGeomInfo().SetDimension(0.4, 0.3, 0.2);gLink.GetGeomInfo().SetColor(0.8, 0.2, 0.2);

There are 6 kinds of shapes for link; SPHERE, BOX, CYLINDER, CAPSULE, PLANE, and USER. “sr-Geometry::SetDimension” function takes up to 3 arguments. These arguments mean full length of eachside in meter. Make sure that “SetDimension” function takes NOT RADIUS BUT DIAMETER.Color arguments are OpenGL style: (R, G, B, α) where 0.0 ≤ R, G, B, α ≤ 1.0, default α value is 1.0.

argument first second thirdSPHERE diameter × ×

BOX width depth heightCAPSULE & CYLINDER diameter height ×

PLANE & USER × × ×

In case of SPHERE, BOX, CYLINDER, and CAPSULE, the mass and inertia of a link is calculated auto-matically by calling following function.

gLink.UpdateInertia(50);

The argument of above function is density (kg/m3). THIS FUNCTION MUST BE CALLED AFTERSETTING SHAPE AND DIMENSION.

How to make a collision object and attach it to a link

Declare srCollision instance.

srCollision gCollision;

Set geometry information.

gCollision.GetGeomInfo().SetShape(srGeometry::BOX);gCollision.GetGeomInfo().SetDimension(0.4, 0.3, 0.2);

Set a local frame of collision object with respect to a parent link.

gCollision.SetLocalFrame(EulerZYX(Vec3(0.0, 0.0, 0.0), Vec3(0.0, 0.0, 0.0)));

Attach a collision object to its parent link.

gLink.AddCollision(&gCollision);

14 Getting Started

How to make a joint

Declare a joint instance which you want to use. In this example, “srRevoluteJoint” is used.

srRevoluteJoint gRJoint;

Set actuation type of a joint. There are 4 kinds of actuation type: PASSIVE, TORQUE, VELOCITY, andHYBRID.

gRJoint.SetActType(srJoint::VELOCITY);

Set position limit and torque limit. Position limit is in degree(rotating joint) or in meter(sliding joint).

gRJoint.MakePositionLimit(true);gRJoint.SetPositionLimit(-180, 180);gRJoint.SetTorqueLimit(-5, 5);

When you model a wheel, set “MakePositionLimit(false)”.

How to connect a joint to two links

Set parent and child links.

gRJoint.SetParentLink(&gParentLink);gRJoint.SetChildLink(&gChildLink);

Set a local frame of a joint with respect to parent and child links respectively.

gRJoint.SetParentLinkFrame(EulerZYX(Vec3(SR_PI/2, 0.0, 0.0), Vec3(0.0, 0.0, 0.5)));gRJoint.SetChildLinkFrame(EulerZYX(Vec3(SR_PI/2, 0.0, 0.0), Vec3(0.0, 0.0, 0.5)));

How to make a sensor and attach it to a link

Declare a sensor instance which you want. In this example, “srRangeFinder” is used.

srRangeFinder gRF;

Set sensor parameters.

gRF.SetRange(2.0); // Maximum detection distance in meter.gRF.SetSpread(180); // Detection angle in degree.gRF.SetResolution(10); // Angle adjacent lasers in degree.

Set a local frame with respect to a link and attach a sensor to its parent link.

gRF.SetLocalFrame(EulerZYX(Vec3(0.0, 0.0, 0.0), Vec3(0.2, 0.0, 0.0)));gLink.AddSensor(&gRF);

3.2 Basics Usage of srLib 15

How to make a system and register it to space

Declare a system instance.

srSystem gSystem;

Set base link.

gSystem.SetBaseLink(&gLink);

Set system properties.

gSystem.SetBaseLinkType(srSystem::DYNAMIC);gSystem.SetSelfCollision(false);

Register a system to a space.

gSpace.AddSystem(&gSystem);

16 Getting Started

Chapter 4

Tutorials

4.1 Tutorial 1: Mobile Robot

Sketch your robot

I recommend you sketch your robot roughly on paper. This will help you not to get lost.

Figure 4.1: Rough Sketch

Determine its dimensions

Specify the dimensions of each part of your robot and relative positions and orientations.

Figure 4.2: Detail Design

18 Tutorials

The mobile robot in the above figure will have 2 driving wheels, 1 caster, 1 range finder sensor, and 1 IR(spot) sensor. We need 4 links, 3 joints, 4 collision objects and sensors. Because there is one robot, we alsoneed one system.DO NOT MISS CREATING A GROUND or your robot would be falling forever.

Write your code

Here is a template.

// Include necessary header files.#include "../../src/Renderer/SimpleViewer.h" // for rendering#include "../../src/srDyn/srSpace.h" // for dynamics

// Get srSimpleViewer instance. (singleton)srSimpleViewer& gViewer = srSimpleViewer::GetInstance();

// Declare srSpace instance.srSpace gSpace;

// >>>>> DECLARE VARIABLES HERE. <<<<<

// For modeling your robots(systems), we need many codes.// It would be too long to be in main() function.// So we this many codes put into the function named User_Modeling().void User_Modeling();

// For simulation, there are some necessary settings.// Following function will do that.void User_SimulationSetting();

// These functions are for simulation start and stop.// DO NOT MODIFY THEM.void User_Simulation_Go_One_Step(); // simulation main loopvoid User_Simulation_Pause(); // empty functionvoid User_CBFunc_Run_DYN(void);void User_CBFunc_Pause_DYN(void);

// If you want to control your mobile robot, we need a contorl function.// And this function must be registered at srSpace.void User_CBFunc_ControlLoop(); // User control loop.

// >>>>> DECLARE CALLBACK FUNCTIONS HERE. <<<<<

////////////////////////////////////////////////////////////////// main////////////////////////////////////////////////////////////////int main(int argc, char **argv){

// STEP 1: Viewer initializationgViewer.Init(&argc, argv);

// STEP 2: Robot ModelingUser_Modeling();

4.1 Tutorial 1: Mobile Robot 19

// STEP 3: Simulation settingUser_SimulationSetting();

// STEP 4: Run window view.gViewer.Run();

return 0;}

// >>>>> WRITE YOUR MODELING CODE HERE. <<<<<void User_Modeling(){}

// Simulation Setting.void User_SimulationSetting(){

// Set user control loop function.gSpace.SET_USER_CONTROL_FUNCTION(User_CBFunc_ControlLoop);

// Initialize for dynamics simulation.gSpace.DYN_MODE_PRESTEP();

// Set target space to render.// Let srSimpleRenderer know what you want to draw on screen.gViewer.SetTarget(&gSpace);

// Key mapping// One key can have only one function, but one function// can be connected with more than one key.gViewer.SetKeyFunc(User_CBFunc_Run_DYN, ’P’);gViewer.SetKeyFunc(User_CBFunc_Run_DYN, ’p’);

gViewer.SetKeyFunc(User_CBFunc_Pause_DYN, ’O’);gViewer.SetKeyFunc(User_CBFunc_Pause_DYN, ’o’);

}

// >>>>> WRITE YOUR CONTROL CODE HERE. <<<<<void User_CBFunc_ControlLoop(){}

// When we push the ’P’ key, this function will work.void User_CBFunc_Run_DYN(){

// Run dynamics simulation by setting glMainLoop idle function// as dynamics simulation step forward function.gViewer.SetLoopFunc(User_Simulation_Go_One_Step);

}

// When we push the ’O’ key, this function will work.void User_CBFunc_Pause_DYN(){

20 Tutorials

// Pause dynamics simulation by setting glMainLoop idle function// as empty function.gViewer.SetLoopFunc(User_Simulation_Pause);

}

// When we push the ’P’ key, this function will be register to loop// function of srSimpleViewer. Then the simulation will run.void User_Simulation_Go_One_Step(){

// Dynamics simulation step forward function.gSpace.DYN_MODE_RUNTIME_SIMULATION_LOOP();

}

// When we push the ’O’ key, this function will be register to loop// function of srSimpleViewer. Then the simulation will be stopped.void User_Simulation_Pause(){

// do nothings}

First, declare variables which is necessary to model.

// >>>>> DECLARE VARIABLES HERE. <<<<<// For modeling a ground.srSystem gGroundSystem;srLink gGroundLink;srCollision gGroundPlane;

// For modeling my robot.srSystem gMobile;srLink gBase;srLink gLWheel;srLink gRWheel;srLink gCaster;

srCollision gColBase;srCollision gColLWheel;srCollision gColRWheel;srCollision gColCaster;

srRevoluteJoint gLMotor;srRevoluteJoint gRMotor;srWeldJoint gFixCaster;

srRangeFinder gRF;srIRSensor gIR;

We will have 5 control commands: go forward, go backward, turn left, turn right, stop. Declare 5 functionscorresponding each command.

// >>>>> DECLARE CALLBACK FUNCTIONS HERE. <<<<<void User_CBFunc_FORWARD();void User_CBFunc_BACKWARD();void User_CBFunc_LEFT();

4.1 Tutorial 1: Mobile Robot 21

void User_CBFunc_RIGHT();void User_CBFunc_STOP();

We will control velocity of each wheel, so we need velocity variables.

real gLVelocity = 0.0;real gRVelocity = 0.0;

Write down “void User_Modeling()” function.

void User_Modeling(){

////////////////////////////////////////////////////////////////// System 1: Ground////////////////////////////////////////////////////////////////gGroundLink.GetGeomInfo().SetShape(srGeometryInfo::PLANE);gGroundPlane.GetGeomInfo().SetShape(srGeometryInfo::PLANE);gGroundPlane.SetLocalFrame(SE3());gGroundLink.AddCollision(&gGroundPlane);

gGroundSystem.SetBaseLink(&gGroundLink);gGroundSystem.SetBaseLinkType(srSystem::FIXED);

gSpace.AddSystem(&gGroundSystem);

Since the ground is an infinite plane, you don’t need to specify its dimension and update its mass andinertia.Let’s model a mobile robot.

////////////////////////////////////////////////////////////////// System 2: Mobile Robot////////////////////////////////////////////////////////////////// Main bodygBase.GetGeomInfo().SetShape(srGeometryInfo::BOX);gBase.GetGeomInfo().SetDimension(0.2, 0.1, 0.05); // width, depth, heightgBase.GetGeomInfo().SetColor(0.3, 0.3, 0.8);gBase.UpdateInertia(2700); // aluminum

gColBase.GetGeomInfo().SetShape(srGeometryInfo::BOX);gColBase.GetGeomInfo().SetDimension(0.2, 0.1, 0.05);gColBase.SetLocalFrame(SE3());

gBase.AddCollision(&gColBase);gBase.SetFrame(EulerZYX(Vec3(0.0, 0.0, 0.0),Vec3(0.0, 0.0, 0.5)));

The base body of the mobile robot is complete. The argument of “UpdateInertia()” function is density. Inthis tutorial, the density of aluminum (2.7g/cm3) is used. The mass is 2.7kg. The last line specifies theorientation and position of the base body in 3D space.The next is to create sensors and attach it to the base body.

gIR.SetRange(1.0);gIR.SetLocalFrame(EulerZYX(Vec3(SR_PI, 0.0, 0.0), Vec3(-0.1, 0.0, -0.025)));gBase.AddSensor(&gIR);

22 Tutorials

gRF.SetRange(2.0);gRF.SetSpread(120);gRF.SetResolution(5);gRF.SetLocalFrame(EulerZYX(Vec3(0.0, 0.0, 0.0), Vec3(0.1, 0.0, 0.0)));gBase.AddSensor(&gRF);

Now build two wheels and one caster.

// Left WheelgLWheel.GetGeomInfo().SetShape(srGeometryInfo::CYLINDER);gLWheel.GetGeomInfo().SetDimension(0.10, 0.01); // Diameter, height.gLWheel.GetGeomInfo().SetColor(0.1, 0.1, 0.1);gLWheel.UpdateInertia(1500); // Little greater than rubber(1250).

gColLWheel.GetGeomInfo().SetShape(srGeometryInfo::SPHERE);gColLWheel.GetGeomInfo().SetDimension(0.10); // DiametergColLWheel.SetLocalFrame(SE3());

gLWheel.AddCollision(&gColLWheel);

// Right WheelgRWheel.GetGeomInfo().SetShape(srGeometryInfo::CYLINDER);gRWheel.GetGeomInfo().SetDimension(0.10, 0.01); // Diameter, height.gRWheel.GetGeomInfo().SetColor(0.1, 0.1, 0.1);gRWheel.UpdateInertia(1500); // Little greater than rubber(1250).

gColRWheel.GetGeomInfo().SetShape(srGeometryInfo::SPHERE);gColRWheel.GetGeomInfo().SetDimension(0.10); // DiametergColRWheel.SetLocalFrame(SE3());

gRWheel.AddCollision(&gColRWheel);

// CastergCaster.GetGeomInfo().SetShape(srGeometryInfo::SPHERE);gCaster.GetGeomInfo().SetDimension(0.05);gCaster.UpdateInertia(1500);

// Attach a caster wheel with weld joint and make it frictionless.gCaster.SetFriction(0.0);

gColCaster.GetGeomInfo().SetShape(srGeometryInfo::SPHERE);gColCaster.GetGeomInfo().SetDimension(0.05);gColCaster.SetLocalFrame(SE3());gCaster.AddCollision(&gColCaster);

Generally, a caster is connected thru universal joint. But to connect a caster to a body and set frictionlesswill bring an equivalent effect.Then, create joints and connect to links.

// Left motorgLMotor.SetActType(srJoint::VELOCITY);gLMotor.MakePositionLimit(false);

4.1 Tutorial 1: Mobile Robot 23

gLMotor.SetParentLink(&gBase);gLMotor.SetChildLink (&gLWheel);

gLMotor.SetParentLinkFrame(EulerZYX(Vec3(0.0, 0.0, SR_PI/2),Vec3(0.05, 0.06, -0.025)));

gLMotor.SetChildLinkFrame(EulerZYX(Vec3(0.0, 0.0, 0.0),Vec3(0.0, 0.0, 0.0)));

// Right motorgRMotor.SetActType(srJoint::VELOCITY);gRMotor.MakePositionLimit(false);

gRMotor.SetParentLink(&gBase);gRMotor.SetChildLink (&gRWheel);

gRMotor.SetParentLinkFrame(EulerZYX(Vec3(0.0, 0.0, SR_PI/2),Vec3(0.05, -0.06, -0.025)));

gRMotor.SetChildLinkFrame(EulerZYX(Vec3(0.0, 0.0, 0.0),Vec3(0.0, 0.0, 0.0)));

// CastergFixCaster.SetParentLink(&gBase);gFixCaster.SetChildLink(&gCaster);

gFixCaster.SetParentLinkFrame(EulerZYX(Vec3(0.0, 0.0, 0.0),Vec3(-0.1, 0.0, -0.050)));

gFixCaster.SetChildLinkFrame(EulerZYX(Vec3(0.0, 0.0, 0.0),Vec3(0.0, 0.0, 0.0)));

Now register “gBase” link to “gMobile” system and set parameters.

gMobile.SetBaseLink(&gBase);gMobile.SetBaseLinkType(srSystem::DYNAMIC);gMobile.SetSelfCollision(false);

gSpace.AddSystem(&gMobile);

Set simulation parameters.

// Set simulation time step.gSpace.SetTimestep(0.001);

// Set gravitygSpace.SetGravity(0.0, 0.0, -9.8);

// Set number of sub-step for renderinggSpace.SetNumberofSubstepForRendering(50);

}

To map user functions onto keyboard inputs, write followings down in “User_SimulationSetting” function.

gViewer.SetKeyFunc(User_CBFunc_FORWARD, ’W’);gViewer.SetKeyFunc(User_CBFunc_FORWARD, ’w’);

24 Tutorials

gViewer.SetKeyFunc(User_CBFunc_BACKWARD, ’S’);gViewer.SetKeyFunc(User_CBFunc_BACKWARD, ’s’);

gViewer.SetKeyFunc(User_CBFunc_LEFT, ’A’);gViewer.SetKeyFunc(User_CBFunc_LEFT, ’a’);

gViewer.SetKeyFunc(User_CBFunc_RIGHT, ’D’);gViewer.SetKeyFunc(User_CBFunc_RIGHT, ’d’);

gViewer.SetKeyFunc(User_CBFunc_STOP, ’X’);gViewer.SetKeyFunc(User_CBFunc_STOP, ’x’);

And implement command functions. (You may use “+=” instead of “=”.)

void User_CBFunc_FORWARD(){

gLVelocity = -1.0;gRVelocity = -1.0;

}

void User_CBFunc_BACKWARD(){

gLVelocity = 1.0;gRVelocity = 1.0;

}

void User_CBFunc_LEFT(){

gLVelocity = 1.0;gRVelocity = -1.0;

}

void User_CBFunc_RIGHT(){

gLVelocity = -1.0;gRVelocity = 1.0;

}

void User_CBFunc_STOP(){

gLVelocity = 0.0;gRVelocity = 0.0;

}

Last, implement a control function.

void User_CBFunc_ControlLoop(){

gLMotor.m_State.m_rCommand = gLVelocity;gRMotor.m_State.m_rCommand = gRVelocity;

}

4.1 Tutorial 1: Mobile Robot 25

The command variable of each joint actuator is in the state of each joint.Compile and test. Press ‘5’ to watch sensors.