Задани на лабораторные работы. ПРК / Professional Microsoft Robotics Developer Studio
.pdf
www.it-ebooks.info
Chapter 6: Extending the MRDS Visual Simulation Environment
pasted into the environment using the Simulation Editor. It also occurs when an entire scene is loaded into the simulator. Here are the Corobot constructors:
///<summary>
///Default constructor used when this entity is deserialized
///</summary>
public CorobotEntity()
{
}
///<summary>
///Initialization constructor used when this entity is built
///programmatically.
///</summary>
///<param name=”initialPos”></param>
public CorobotEntity(string name, Vector3 initialPos)
{
base.State.Name = name; base.State.Pose.Position = initialPos;
}
The nondefault constructor enables the entity name and initial position to be specified. The only action the constructor takes is to modify its state with the passed parameters. The parameters do not need to be initialized when the default constructor is called during deserialization because the entity state will be restored to the value it had when the entity was serialized.
Next, you need to define some dimensions. Add the following code to the top of the CorobotEntity class:
private static float InchesToMeters(float inches)
{
return (float)(inches * 2.54 / 100.0);
}
static float mass = 3.63f; // kg
static float chassisClearance = InchesToMeters(1.5f); static float wheelGap = InchesToMeters(3f / 8f); static float wheelWidth = InchesToMeters(2.2f); static float wheelDiameter = InchesToMeters(4.75f); static float wheelMass = 0.1f; // kg
static float platformClearance = InchesToMeters(5.75f);
static Vector3 platformDimensions = new Vector3( InchesToMeters(11.0f), InchesToMeters(3.0f), InchesToMeters(8.5f));
static Vector3 chassisDimensions = new Vector3( platformDimensions.X - 2 * wheelWidth - 2 * wheelGap, InchesToMeters(2.5f),
platformDimensions.Z);
static Vector3 wheelFRPosition = new Vector3( chassisDimensions.X / 2.0f + wheelGap + wheelWidth / 2.0f, wheelDiameter / 2.0f,
-InchesToMeters(5.75f - 2.125f));
(continued)
283
www.it-ebooks.info
Part II: Simulations
(continued)
static Vector3 wheelFLPosition = new Vector3( -wheelFRPosition.X,
wheelFRPosition.Y,
wheelFRPosition.Z);
static Vector3 wheelRRPosition = new Vector3( wheelFRPosition.X,
wheelFRPosition.Y, -wheelFRPosition.Z);
static Vector3 wheelRLPosition = new Vector3( -wheelFRPosition.X,
wheelFRPosition.Y, -wheelFRPosition.Z);
The motor box is called the chassis and the upper box containing the processor is called the platform. The height of the chassis above the ground is defined by chassisClearance, and the height of the platform is defined by platformClearance. The width of the chassis is calculated to be just wide enough to allow the outer edge of the wheels to be even with the sides of the platform. The position of the front right wheel is calculated and the positions of the other three wheels are derived from it. This gives us enough information to specify the basic physics shapes that make up the physics model of the Corobot.
The wheels are more than just shapes — they are full entities with their own meshes and physics shapes. You need a place to store all four wheel entities when you create them. Add this code just before the constructors:
// instance variables WheelEntity _wheelFR; WheelEntity _wheelFL; WheelEntity _wheelRR; WheelEntity _wheelRL;
[Category(“Wheels”)]
[DataMember]
public WheelEntity FrontRightWheel
{
get { return _wheelFR; } set { _wheelFR = value; }
}
[Category(“Wheels”)]
[DataMember]
public WheelEntity FrontLeftWheel
{
get { return _wheelFL; } set { _wheelFL = value; }
}
[Category(“Wheels”)]
[DataMember]
public WheelEntity RearRightWheel
{
get { return _wheelRR; } set { _wheelRR = value; }
}
284
www.it-ebooks.info
Chapter 6: Extending the MRDS Visual Simulation Environment
[Category(“Wheels”)]
[DataMember]
public WheelEntity RearLeftWheel
{
get { return _wheelRL; } set { _wheelRL = value; }
}
The Category attribute just groups these properties together so that they are better organized in the Simulation Editor view. Each WheelEntity property has the [DataMember] attribute. This attribute tells the proxy generator that this is a property that must be serialized when the entity is sent to a remote node or when it is saved to disk. Only public properties can be marked with this attribute. You’ll test the entity later to ensure that it can be properly serialized and deserialized.
These wheel entities and chassis and platform shapes are created in the Initialize method. Add the following code to override the base class Initialize method:
public override void Initialize( xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
{
try
{
// chassis
BoxShapeProperties chassisDesc = new BoxShapeProperties( “chassis”,
mass / 2.0f,
new Pose(new Vector3( 0,
chassisClearance + chassisDimensions.Y / 2.0f, 0)),
chassisDimensions);
chassisDesc.Material =
new MaterialProperties(“chassisMaterial”, 0.0f, 0.5f, 0.5f);
BoxShape chassis = new BoxShape(chassisDesc); chassis.State.Name = “ChassisShape”; base.State.PhysicsPrimitives.Add(chassis);
// platform
BoxShapeProperties platformDesc = new BoxShapeProperties( “platform”,
mass / 2.0f,
new Pose(new Vector3( 0,
platformClearance + platformDimensions.Y / 2.0f, 0)),
platformDimensions);
(continued)
285
www.it-ebooks.info
Part II: Simulations
(continued)
platformDesc.Material = chassisDesc.Material; BoxShape platform = new BoxShape(platformDesc); platform.State.Name = “PlatformShape”; base.State.PhysicsPrimitives.Add(platform);
The first thing the Initialize method does is create two box shapes to represent the chassis and the platform. The mass of both objects is assumed to be the same, so the total mass is split between them. They are given the dimensions and position specified. These positions are relative to the origin of the entity. Both box shapes are given the same material definition, which specifies a restitution of 0 and mid-range static and dynamic friction. After each shape is created, it is added to the
PhysicsPrimitives list:
base.CreateAndInsertPhysicsEntity(physicsEngine); base.PhysicsEntity.SolverIterationCount = 128;
When the physics entity is created, it passes all of the objects in the PhysicsPrimitives list to the physics engine, which creates its own representation for them based on the attributes specified in the BoxShape objects. The SolverIterationCount determines how many iterations the physics engine will use to resolve constraints on the entity, such as contact points or joints. The higher the count, the more accurate the results become. A value of 128 is probably overkill because the AGEIA documentation states that the default value is 4 and AGEIA developers have never needed to use a value higher than 30. This code follows the example of the DifferentialDriveEntity in the SDK in setting this value to 128:
// Wheels
WheelShapeProperties wheelFRprop = new WheelShapeProperties( “FrontRightWheel”, wheelMass, wheelDiameter / 2.0f); WheelShapeProperties wheelFLprop = new WheelShapeProperties( “FrontLeftWheel”, wheelMass, wheelDiameter / 2.0f); WheelShapeProperties wheelRRprop = new WheelShapeProperties( “RearRightWheel”, wheelMass, wheelDiameter / 2.0f); WheelShapeProperties wheelRLprop = new WheelShapeProperties( “RearLeftWheel”, wheelMass, wheelDiameter / 2.0f);
wheelFRprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed; wheelFLprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed; wheelRRprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed; wheelRLprop.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
wheelFRprop.InnerRadius = 0.7f * wheelDiameter / 2.0f; wheelFLprop.InnerRadius = 0.7f * wheelDiameter / 2.0f; wheelRRprop.InnerRadius = 0.7f * wheelDiameter / 2.0f; wheelRLprop.InnerRadius = 0.7f * wheelDiameter / 2.0f;
wheelFRprop.LocalPose = new Pose(wheelFRPosition); wheelFLprop.LocalPose = new Pose(wheelFLPosition); wheelRRprop.LocalPose = new Pose(wheelRRPosition); wheelRLprop.LocalPose = new Pose(wheelRLPosition);
286
www.it-ebooks.info
Chapter 6: Extending the MRDS Visual Simulation Environment
Next, the WheelShapeProperties for each wheel are specified. The OverrideAxleSpeed flag tells the physics engine not to calculate the axle speed based on motor torque and friction. Instead, you specify the axle speed each frame. This turns out to be a better way to simulate the types of motors that typically drive wheeled robots. The LocalPose of each shape is useful when an entity contains multiple shapes. The LocalPose specifies the position and orientation of each shape relative to the origin of the entity. In the preceding code, each pose is initialized with a position vector and the orientation part of the pose defaults to 0.
_wheelFR = new WheelEntity(wheelFRprop); _wheelFR.State.Name = base.State.Name + “ FrontRightWheel”; _wheelFR.Parent = this;
_wheelFR.Initialize(device, physicsEngine);
_wheelFL = new WheelEntity(wheelFLprop); _wheelFL.State.Name = base.State.Name + “ FrontLeftWheel”; _wheelFL.Parent = this;
_wheelFL.Initialize(device, physicsEngine);
_wheelRR = new WheelEntity(wheelRRprop); _wheelRR.State.Name = base.State.Name + “ RearRightWheel”; _wheelRR.Parent = this;
_wheelRR.Initialize(device, physicsEngine);
_wheelRL = new WheelEntity(wheelRLprop); _wheelRL.State.Name = base.State.Name + “ RearLeftWheel”; _wheelRL.Parent = this;
_wheelRL.Initialize(device, physicsEngine);
After the WheelShapeProperties have been initialized, you can create the WheelEntities. Each WheelEntity is given a name based on the parent entity name.
The parent reference of each WheelEntity is set to the CorobotEntity. The WheelEntity uses this reference in an unusual way. You can see this code in the Initialize method of the WheelEntity in samples\entities\entities.cs. Instead of calling CreateAndInsertPhysicsEntity, the WheelEntity calls InsertShape on its parent’s PhysicsEntity. This adds the WheelEntity shape to the set of shapes that makes up the parent. As far as the physics engine is concerned, the wheel shapes are just part of the parent entity. This reduces the amount of computation required by the physics engine because it doesn’t have to calculate the interactions between the wheels and the chassis and platform shapes. It assumes that they are rigidly joined.
You must explicitly call the Initialize method for each WheelEntity because these entities have not been inserted into the parent entity as children using the InsertEntity method.
287
www.it-ebooks.info
Part II: Simulations
Child Entity or Not?
When you make an entity a child of another entity, and both entities have physics shapes in them, the child is joined to the parent with a joint. The properties of that joint are contained in the ParentJoint property of the child entity. (Joints are discussed in Chapter 7). The Initialize method of each child entity is called from the base class Initialize method. Likewise, the Update and Render methods are automatically called from the corresponding methods in the parent. In some cases, it is not desirable to have the child entity joined to the parent with a joint, such as in the case of the WheelEntities above. In this case, the WheelEntities are not added as children to the parent and Initialize, Update, and Render are explicitly called from the corresponding methods in the parent. Both methods of dealing with dependent entities are acceptable.
base.Initialize(device, physicsEngine);
}
catch (Exception ex)
{
// clean up
if (PhysicsEntity != null) PhysicsEngine.DeleteEntity(PhysicsEntity);
HasBeenInitialized = false; InitError = ex.ToString();
}
}
Finally, you call the base.Inialize method, which, among other things, loads any meshes or textures associated with the entity. If no mesh was specified in State.Assets.Mesh, simple meshes are constructed from the shapes in the entity.
Now that you have the Initialize method completed, only one thing remains to add the Corobot entity into the simulation environment. Add the following lines of code to the Start method just after the code inserting the giant box into the simulation environment:
// create a Corobot SimulationEngine.GlobalInstancePort.Insert(
new CorobotEntity(“Corobot”, new Vector3(0, 0, 0)));
This creates a new CorobotEntity with the name “Corobot” at the simulation origin. Compile and run the service. You should see something similar to what is shown in Figure 6-5.
288
www.it-ebooks.info
Chapter 6: Extending the MRDS Visual Simulation Environment
Figure 6-5
You can test your new entity by going into the Simulation Editor and selecting it by holding down the Ctrl key while pressing the right mouse button with the mouse pointer on the entity. You can examine the fields in the EntityState and each of the WheelEntities to verify they are correct. You can even press Ctrl+X and then Ctrl+V to cut and paste the entity back into the simulator to verify that serialization and deserialization of the entity work properly.
You still need to add some properties to the entity to make it compatible with the
DifferentialDriveEntity. The MotorTorqueScaling property scales the speed of the motors to model the gear ratio on the physical robot. The IsEnabled property allows other services to enable or disable the drive. Add the following properties to the CorobotEntity class:
bool _isEnabled;
///<summary>
///True if drive mechanism is enabled
///</summary>
[DataMember]
[Description(“True if the drive mechanism is enabled.”)] public bool IsEnabled
{
get { return _isEnabled; } set { _isEnabled = value; }
}
float _motorTorqueScaling;
///<summary>
///Scaling factor to apply to motor torque requests
///</summary>
[DataMember]
(continued)
289
www.it-ebooks.info
Part II: Simulations
(continued)
[Description(“Scaling factor to apply to motor torgue requests.”)] public float MotorTorqueScaling
{
get { return _motorTorqueScaling; } set { _motorTorqueScaling = value; }
}
Next, you initialize the MotorTorqueScaling in the nondefault constructor. The value set here is an arbitrary value. You’ll reexamine this value later in the section “Tuning MotorTorqueScaling.”
_motorTorqueScaling = 20f;
The CurrentHeading property simulates a compass sensor. It returns the heading of the entity based on
State.Pose:
public float CurrentHeading
{
get
{
// return the axis angle of the quaternion
xna.Vector3 euler = UIMath.QuaternionToEuler(State.Pose.Orientation); // heading is the rotation about the Y axis.
return xna.MathHelper.ToRadians(euler.Y);
}
}
The next few methods enable explicit control of the speed of each motor by setting the left and right target velocities. If one of these methods is called, then any current DriveDistance or RotateDegrees commands are terminated with an error:
float _leftTargetVelocity; float _rightTargetVelocity;
public void SetMotorTorque(float leftWheel, float rightWheel)
{
ResetRotationAndDistance();
SetAxleVelocity(
leftWheel * _motorTorqueScaling, rightWheel * _motorTorqueScaling);
}
public void SetVelocity(float value)
{
ResetRotationAndDistance(); SetVelocity(value, value);
}
///<summary>
///Sets angular velocity on the wheels
///</summary>
///<param name=”left”></param>
///<param name=”right”></param>
public void SetVelocity(float left, float right)
{
290
www.it-ebooks.info
Chapter 6: Extending the MRDS Visual Simulation Environment
ResetRotationAndDistance();
if (_wheelFR == null || _wheelFL == null) return;
left = ValidateWheelVelocity(left); right = ValidateWheelVelocity(right);
//v is in m/sec - convert to an axle speed
//2Pi(V/2PiR) = V/R
SetAxleVelocity(
left / _wheelFR.Wheel.State.Radius, right / _wheelFL.Wheel.State.Radius);
}
private void SetAxleVelocity(float left, float right)
{
_leftTargetVelocity = left; _rightTargetVelocity = right;
}
const float MAX_VELOCITY = 20.0f;
const float MIN_VELOCITY = -MAX_VELOCITY;
float ValidateWheelVelocity(float value)
{
if (value > MAX_VELOCITY) return MAX_VELOCITY; if (value < MIN_VELOCITY) return MIN_VELOCITY;
return value;
}
All that remains are the DriveDistance and RotateDegrees methods and their associated helper functions and variables:
Pose _startPoseForDriveDistance; double _distanceToTravel;
SuccessFailurePort _driveDistancePort = null;
public void DriveDistance( float distance,
float power,
SuccessFailurePort responsePort)
{
//reset drivedistance or rotatedegrees commands not yet completed ResetRotationAndDistance();
//keep track of the response port for when we complete the request _driveDistancePort = responsePort;
//handle negative distances
if (distance < 0)
(continued)
291
www.it-ebooks.info
Part II: Simulations
(continued)
{
distance = -distance; power = -power;
}
_startPoseForDriveDistance = State.Pose; _distanceToTravel = distance; SetAxleVelocity(
power * _motorTorqueScaling, power * _motorTorqueScaling);
}
//DriveDistance and RotateDegrees variables Queue<double> progressPoints = new Queue<double>(); const int averageKernel = 6;
const int decelerateThreshold = 6;
const float twoPI = (float)(2 * Math.PI);
//RotateDegrees variables
double _targetRotation = double.MaxValue; double _currentRotation = 0;
double _previousHeading = 0;
const float acceptableRotationError = 0.005f; SuccessFailurePort _rotateDegreesPort = null;
public void RotateDegrees( float degrees,
float power,
SuccessFailurePort responsePort)
{
//reset drivedistance or rotatedegrees commands not yet completed ResetRotationAndDistance();
//keep track of the response port for when we complete the request _rotateDegreesPort = responsePort;
_targetRotation = xna.MathHelper.ToRadians(degrees); _currentRotation = 0;
_previousHeading = CurrentHeading;
if (degrees < 0) SetAxleVelocity(
power * _motorTorqueScaling, -power * _motorTorqueScaling);
else
SetAxleVelocity(
-power * _motorTorqueScaling, power * _motorTorqueScaling);
}
void ResetRotationAndDistance()
{
progressPoints.Clear(); _distanceToTravel = 0;
292