Задани на лабораторные работы. ПРК / Professional Microsoft Robotics Developer Studio

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

// reset position

yield return Arbiter.Choice( _diffDrivePort.RotateDegrees(180-angle, 0.2), delegate(DefaultUpdateResponseType response) { }, delegate(Fault f) { success = false; }

);

}

}

This test method sends DriveDistance and RotateDegrees messages to the _diffDrivePort to drive the Corobot a random distance in a random direction. It then turns around and returns to its starting point. Over time, the errors in rotation and driving distance accumulate and the robot will fail to return to its exact starting point. This code is a good example of how the Corobot can be driven from another service. You’ll see more examples of this later.

Tuning the Corobot Entity

Once you have a basic robot entity defined, it is a good idea to tune its properties so that it more closely resembles its real-world counterpart. In the following sections, you’ll adjust the top speed of the Corobot entity, the grip of its tires, and its appearance. You’ll also adjust the model so that its wheels actually turn.

Tuning MotorTorqueScaling

The MotorTorqueScaling property of the entity is a multiplier on the axle speed. When the entity is moved by calling SetMotorTorque, the torque that is passed as a parameter is scaled by MotorTorqueScaling to set the axle speed. A torque value of 1.0 represents the greatest torque that the motor can apply to the axle. Therefore, MotorTorqueScaling determines the speed of the entity when its torque is at the maximum. The CoroWare engineers indicate that the top speed of the Corobot is somewhere around two feet per second. In the initial implementation of the entity, a value of 20 was arbitrarily chosen to initialize MotorTorqueScaling. Now it is time to determine what the actual value should be.

You need to cause the entity to move forward at its maximum speed and somehow measure that speed to determine whether it is close to the rated top speed of two feet per second. The first step is to add the following line as the last line of the Corobot entity’s Initialize method:

SetMotorTorque(1, 1);

This will cause the Corobot to move forward at its maximum speed. Change the first few lines of the Update method as follows to measure the speed of the entity:

const float SPEED_DELTA = 0.5f; Pose startPose;

double totalTime = -1;

public override void Update(FrameUpdate update)

{

if (totalTime < 0)

{

startPose = State.Pose;

(continued)

313

www.it-ebooks.info

Part II: Simulations

(continued)

totalTime = update.ElapsedTime;

}

else

{

double distance = Vector3.Length( State.Pose.Position - startPose.Position);

double speed = distance / totalTime; totalTime += update.ElapsedTime; Console.WriteLine(speed.ToString());

}

The first time Update is called, the pose of the entity is stored. On each subsequent call to Update, the total distance traveled is calculated, along with the accumulated time since the first call. The speed is calculated and displayed. The value displayed on the console will eventually converge to the actual speed of the entity.

An initial run with this code yielded a speed of approximately 1.2 meters per second. Two feet per second is equal to 0.61 meters per second, so the entity’s top speed with a MotorTorqueScaling value of 20 is about twice as fast as it should be. The new MotorTorqueScaling value can be calculated by multiplying the initial value (20) by the ratio of the desired top speed to the current top speed:

NewMotorTorqueScaling = InitialValue * TopSpeedDesired / TopSpeedMeasured

The proper MotorTorqueScaling factor should be about 10.16. When that value is updated in the CorobotEntity constructor and the test is executed again, the top speed comes very close to 0.61 meters per second. After you have determined the appropriate value for MotorTorqueScaling, you can comment out this code so that it doesn’t interfere with normal operation of the entity.

Tuning the Tire Friction

After driving your Corobot around in the simulation environment, you may notice that the tires grip the ground very well. If you drive up to the giant box, the tires grip the ground and the box so well that the Corobot flips on its back. Furthermore, when the Corobot is turning, it shakes and jitters because the tires grip the ground very tightly and slip very little. This is different behavior than what is observed on the actual Corobot, so you need to adjust the tire friction.

As discussed in Chapter 5, the simulation environment allows each shape to have a material definition describing its bounciness (restitution) and its static and dynamic friction. The wheel shapes have a more advanced friction model that enables the friction along the longitudinal direction (in the direction of rotation) to be specified separately from the lateral direction (perpendicular to the direction of rotation). The friction model utilizes a spline function to determine the amount of wheel slippage as a function

of the amount of force acting on the wheel.

Four parameters specify the spline function: ExtremumSlip, ExtremumValue, AsymptoteSlip, and AsymptoteValue. A fifth factor, StiffnessFactor, acts as a multiplier on the tire forces. Higher values cause the wheel to grip the ground more strongly.

The AGEIA SDK documentation describes the effect of modifying the four spline function parameters. Changing the StiffnessFactor from its default value of 1000000.0 in the lateral direction will have the desired effect for the simulation. The values for the spline function are the same as the AGEIA defaults.

314

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

Add the following code just before the first WheelEntity is created in the Corobot Initialize method:

TireForceFunctionDescription LongitudalFunction =

new TireForceFunctionDescription();

LongitudalFunction.ExtremumSlip = 1.0f;

LongitudalFunction.ExtremumValue = 0.02f;

LongitudalFunction.AsymptoteSlip = 2.0f;

LongitudalFunction.AsymptoteValue = 0.01f;

LongitudalFunction.StiffnessFactor = 1000000.0f;

TireForceFunctionDescription LateralFunction = new TireForceFunctionDescription(); LateralFunction.ExtremumSlip = 1.0f;

LateralFunction.ExtremumValue = 0.02f;

LateralFunction.AsymptoteSlip = 2.0f;

LateralFunction.AsymptoteValue = 0.01f; LateralFunction.StiffnessFactor = 100000.0f;

wheelFRprop.TireLongitudalForceFunction = LongitudalFunction; wheelFLprop.TireLongitudalForceFunction = LongitudalFunction; wheelRRprop.TireLongitudalForceFunction = LongitudalFunction; wheelRLprop.TireLongitudalForceFunction = LongitudalFunction;

wheelFRprop.TireLateralForceFunction = LateralFunction; wheelFLprop.TireLateralForceFunction = LateralFunction; wheelRRprop.TireLateralForceFunction = LateralFunction; wheelRLprop.TireLateralForceFunction = LateralFunction;

The LateralFunction reduces the slip by a factor of 10. This has the desired effect of making the turns smoother due to tire slippage.

It is difficult to measure the exact tire friction of the actual robot, so these parameters must typically be tuned by hand until the simulated behavior closely matches the observed real-world behavior.

Making the Wheels Turn

One of the first things you might have noticed about your Corobot entity is that its tires don’t actually turn as the robot moves. This doesn’t affect the simulation behavior but it does reduce the visual realism of the scene quite a bit. In addition, you likely noticed that the wheels never turn on the robots in the MRDS simulation tutorials, either. Let’s fix that problem now.

You need to define a new WheelEntity that can keep track of its current rotation and adjust the rendering of its mesh accordingly. The following code shows how to create a new entity called RotatingWheelEntity, which inherits from WheelEntity but adds this additional functionality:

[DataContract]

public class RotatingWheelEntity : WheelEntity

{

const float rotationScale = (float)(-1.0 / (2.0 * Math.PI)); public float Rotations = 0;

public RotatingWheelEntity()

{

}

(continued)

315

www.it-ebooks.info

Part II: Simulations

(continued)

public RotatingWheelEntity(WheelShapeProperties wheelShape) : base(wheelShape)

{

}

public override void Initialize( Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)

{

base.Initialize(device, physicsEngine);

}

public override void Update(FrameUpdate update)

{

base.Update(update);

// set the wheel to the current position Wheel.State.LocalPose.Orientation =

TypeConversion.FromXNA(

xna.Quaternion.CreateFromAxisAngle( new xna.Vector3(-1, 0, 0), (float)(Rotations * 2 * Math.PI)));

// update the rotations for the next frame Rotations += (float)(Wheel.AxleSpeed *

update.ElapsedTime * rotationScale);

}

}

Add the code for this entity outside of the Corobot entity definition but within the same namespace. This entity has a public data member called Rotations. For each frame, the LocalPose of the wheel shape is set according to the current rotation of the wheel. The rotation of the wheel for the next frame is calculated by converting the wheel axle speed to radians per second and multiplying by the elapsed time. The Rotation variable serves as an encoder for the current wheel rotation.

The Render function for the WheelEntity is already set up to take the LocalPose of the wheel shape into account when rendering the mesh; all you have left to do is ensure that the Render function for each wheel is called. Add the following override to the Render method in the Corobot class to accomplish this:

public override void Render( RenderMode renderMode, MatrixTransforms transforms, CameraEntity currentCamera)

{

base.Render(renderMode, transforms, currentCamera); _wheelFL.Render(renderMode, transforms, currentCamera); _wheelFR.Render(renderMode, transforms, currentCamera); _wheelRL.Render(renderMode, transforms, currentCamera); _wheelRR.Render(renderMode, transforms, currentCamera);

}

316

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

This method first takes care of rendering the Corobot entity by calling the base.Render method. It then it takes care of rendering each WheelEntity by explicitly calling the Render method for each wheel. Remember that you must do this explicitly because the WheelEntities are not actually children of the

Corobot entity.

Additionally, at this point you need to change the definition of _wheelFR (and the others) to use RotatingWheelEntity instead of WheelEntity. The accessor functions for the wheels must be changed to return a RotatingWheelEntity instead of a WheelEntity. In addition, the initialization of the private wheel members (_wheelFR, etc.) must be changed to create a RotatingWheelEntity. The code changes for the FrontRightWheel are shown here:

RotatingWheelEntity _wheelFR;

[Category(“Wheels”)]

[DataMember]

public RotatingWheelEntity FrontRightWheel

{

get { return _wheelFR; } set { _wheelFR = value; }

}

_wheelFR = new RotatingWheelEntity(wheelFRprop);

Recompile your Corobot project and run the manifest again. You should now notice the wheels move as the robot moves around. It may be a little difficult to tell if they are moving because each wheel is just a uniform gray disc. You’ll fix that soon.

Adding Encoders

Now that each wheel has a concept of its current rotation, it is possible for you to add simulated wheel encoders. The Corobot has encoders on its front wheels. Each encoder has a resolution of 600 ticks per revolution of the wheel. Add the following two properties to access the simulated encoder values on these wheels:

[Category(“Wheels”)]

[DataMember]

public int FrontRightEncoder

{

get { return (int)(_wheelFR.Rotations * 600f); } set { _wheelFR.Rotations = (float)value / 600f; }

}

[Category(“Wheels”)]

[DataMember]

public int FrontLeftEncoder

{

get { return (int)(_wheelFL.Rotations * 600f); } set { _wheelFL.Rotations = (float)value / 600f; }

}

These properties will show up in the Simulation Editor when you display the properties of the Corobot entity so you can easily verify that the values are correct.

317

www.it-ebooks.info

Part II: Simulations

SimulatedQuadDifferentialDrive provides a way to expose the encoder values in its state. Add the following two lines of code to the UpdateStateFromSimulation method in the

SimulatedQuadDifferentialDriveService class:

_state.LeftWheel.EncoderState.CurrentReading = _entity.FrontLeftEncoder; _state.RightWheel.EncoderState.CurrentReading = _entity.FrontRightEncoder;

Making It Look Real

One might argue that the visual appearance of the entity has little bearing on the usefulness of the simulation. However, one gets tired of looking at a lot of gray boxes driving around. You can build a mesh using a graphics modeling tool such as 3D Studio Max, Maya, or Blender that provides a better visualization of the entity. This mesh displays in the simulator in place of the entity but it has no effect on the entity’s physics behavior.

It doesn’t matter which modeling tool you use to create the mesh as long as it can export to the Alias .obj format. This is a fairly universal, if somewhat basic, 3D geometry format. Each .obj file is typically accompanied by a .mtl file with the same name that specifies the material characteristics for the geometry.

It is beyond the scope of this book to discuss geometry modeling in any great detail, but here are a few lessons that were learned from building meshes for the Corobot model:

Modeling tools: The Maya modeling tool was used to generate the geometry for this model. Other modeling and CAD packages such as 3D Studio Max, SolidWorks, and Blender have been successfully used to build models for the simulation environment. Blender is a reasonable option if money is a concern. You can find more information about Blender at www.blender

.org.

Realism versus speed: Because you want the wheels to move independently from the robot body, you must make both a wheel mesh and a separate body mesh. There is usually a trade-off between the realism and visual interest of the model and the number of polygons it takes to define the model. Too many polygons makes the simulator run more slowly, especially if you have many robots or objects in the scene and you are running on a less powerful graphics card. Always endeavor to keep your simulation running at 30 frames per second or faster. Certainly, if it is running at 20 fps or slower, it becomes much less usable. Also keep in mind that the AGEIA physics engine becomes less stable if it is asked to continuously simulate steps much larger than 16.6 ms. If your simulator is running at less than 30 frames per second, then each frame represents over 33 ms and you may begin to notice physics behavior problems.

Wheel geometry: Because this model has four wheels, the wheel geometry affects the overall look of the model a great deal. For that reason, more polygons were budgeted for the wheels than for any other part of the robot model. The wheels were modeled by defining a wheel hub composed of a cylindrical tube with a disc in the center with five triangular holes. Maya’s smoothing feature was used to round the edges of the hub. Finally, a cylindrical tire was modeled to fit over the hub. The resulting tire model is shown in Figure 6-8. The right part of the figure illustrates the number of polygons that were used. The wheel doesn’t look exactly like the one on the robot but it is close enough.

318

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

Figure 6-8

Modeling the robot chassis and platform: The dimensions of the physics model are a good place to start in modeling the visual geometry. The chassis is modeled as a simple box combined with hexagonal cylinders on the front and back to give it the beveled look of the actual robot. The platform is modeled as a simple box sandwiched between two larger thin boxes. The middle box represents the batteries and circuit boards carried by the robot. A couple of cylinders and a box represent the camera in front, and a very simple representation of the IR sensor was also added to the front and back of the chassis.

Circuit boards and battery: Because a real Corobot was available, pictures were taken of the circuit boards and battery on the platform and added as a texture map to the platform box. The texture map is shown in Figure 6-9, and Figure 6-10 shows the model with the texture map applied. The simulation environment supports texture maps in a number of formats, including

.bmp and .jpg. You can also use the DirectX Texture Tool, available in the DirectX SDK, to make

.dds files that support mip-maps. A mip-map is a texture that contains multiple resolutions. The hardware automatically chooses the most appropriate resolution based on the mapping of

the texture to the screen. You might have noticed that a .dds texture with mip levels is typically used on the ground plane. If you substitute a regular .bmp or .jpg texture map with only a single resolution, you will notice shimmering pixels in the distance as the camera moves around due to aliasing of the texture map.

319

www.it-ebooks.info

Part II: Simulations

Figure 6-9

Figure 6-10

Tweaking the simulation environment: Because different modeling packages handle materials and geometry in different ways, you will likely need to iterate several times to make the model look good. For example, some modeling packages define +Z as the upward axis, in which case you would need to rotate your model to make it look right in the simulation environment. Other packages use sophisticated lighting models that don’t translate well to the .obj format, so you may have to go back and forth between the modeling tool and the simulation environment, tweaking the lighting parameters until the model looks as expected.

320

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

Adding the Custom Mesh to the Corobot Entity

If no mesh is specified for an entity in State.Assets.Mesh, the simulator generates meshes from the physics shapes in the entity. If a mesh is specified, it is used. You need to change your Corobot entity to add these custom meshes. Add the following line for each of the RotatingWheelEntities just prior to the call to Initialize:

_wheelFR = new RotatingWheelEntity(wheelFRprop); _wheelFR.State.Name = base.State.Name + “ FrontRightWheel”; _wheelFR.Parent = this;

_wheelFR.MeshRotation = new Vector3(0, 180, 0); // flip the wheel mesh

_wheelFR.State.Assets.Mesh = “CorobotWheel.obj”;

_wheelFR.Initialize(device, physicsEngine);

The simulator looks in the store\media directory by default but you can also specify any other directory inside the SDK directory structure.

After adding the wheel mesh to each of the wheels, add the Corobot mesh to the Corobot entity just before base.Initialize is called in its Initialize method:

State.Assets.Mesh = “Corobot.obj”; base.Initialize(device, physicsEngine);

Run the Corobot service. You should now see the custom mesh in place of the old physics model. To display the physics representation or combine the two, select a different rendering mode by pressing F2. Figure 6-11 shows the full Corobot model in the simulation environment.

Figure 6-11

321

www.it-ebooks.info

Part II: Simulations

Adding a Camera

The Corobot has a camera mounted on the front center of its chassis. Fortunately, a camera entity and its associated service are already provided in the SDK so it is fairly simple to add it to your entity.

Adding the Camera Entity

Add the following lines of code to the CorobotEntity nondefault constructor:

CameraEntity frontCam = new CameraEntity( 320, 240,

(float)(30.0 * Math.PI / 180.0)); frontCam.State.Name = name + “_cam”; frontCam.IsRealTimeCamera = true; // update each frame frontCam.State.Pose.Position = new Vector3(

0,

chassisDimensions.Y / 2.0f + chassisClearance - 0.01f, -chassisDimensions.Z / 2.0f);

InsertEntityGlobal(frontCam);

For this code, you first create a CameraEntity with a resolution of 320 by 240 pixels and a field of view of 30 degrees (converted to radians). You give it a name based on its parent entity and set it to be a realtime camera. Because the view from a real-time camera is rendered every frame, be careful not to add too many real-time cameras in a scene because they can greatly increase rendering time.

Finally, you give the camera a pose that positions it just a little below the center of the front of the chassis shape facing forward. Because the camera is a child of the Corobot entity, it follows its parent’s orientation and position.

Compile and build with these changes and run the Corobot manifest. When the scene is displayed, select the Camera menu item. Both the main camera and the Corobot_cam should appear in the menu. When you select the Corobot_cam, you see the simulation environment from the point of view of that camera. The 320 × 240 image is stretched to fill the display window so it may look a little blocky, and objects may be distorted if the display window has a different aspect ratio. Drive the robot around with the dashboard and verify that the camera view changes.

Adding the SimulatedWebcam Service

Just as the Corobot motors needed a SimulatedQuadDifferentialDrive service to drive them, the camera entity you just added needs a SimulatedWebCam service to retrieve frames from the camera for other services to use.

Like other simulation services, the SimulatedWebCam service is started with an entity name for a partner. It attempts to connect with the entity by making a subscription request to the simulation engine.

Add the following lines to the Corobot.manifest.xml file:

<!-- Start WebCam service --> <ServiceRecordType> <dssp:Contract>

322