Upload
others
View
5
Download
0
Embed Size (px)
Citation preview
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:
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);
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;
}