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

www.it-ebooks.info

Chapter 9: Adventures in Simulation

_bodyWidth/2 + _upperLegRadius * 2, _bodyWidth/2 + _upperLegRadius, _bodyLength/2 - _upperLegRadius, 0, 0, 0, _upperLegRadius,

0, 0, 0, 1, “HexapodShoulder.obj”),

new HexapodShapeDescriptor(Shapes.Capsule, “FRUpper”, _bodyWidth/2 + _upperLegRadius * 5,

_upperLegLength/2 + _lowerLegLength + _lowerLegRadius, _bodyLength/2 - _upperLegRadius,

0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, “HexapodUpperLeg.obj”),

new HexapodShapeDescriptor(Shapes.Box, “FRLower”, _bodyWidth/2 + _upperLegRadius * 5, _lowerLegLength/2 + _lowerLegRadius, _bodyLength/2 - _upperLegRadius,

_lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius,

0, 0, 0, 1, “HexapodLowerLeg.obj”),

};

The HexapodShapeDescriptor class holds information about each shape, such as its type, its name, its position, its size, its rotation, its mass, and whether it has a visual mesh associated with it. The size of the body and the length and radius of each part of each leg are specified, and then the ShapeDescriptors array is initialized with all of the data. The body consists of a single box shape. Each leg consists of three shapes: a sphere, called the shoulder, a capsule for the upper leg, and a box for the lower leg. The positions and sizes of each shape are defined according to the size variables so that it is easy to change the size of the various parts of the entity. The physics representation for the front right leg and body is shown in Figure 9-8.

Figure 9-8

433

www.it-ebooks.info

Part II: Simulations

The sphere attached to the side of the hexapod body is the shoulder shape. It is there so you can define a vertical shoulder joint and an additional horizontal shoulder joint and have them operate independently of each other.

Five additional legs are specified similarly to the front right leg defined earlier. The shapes are placed in approximately the same position and orientation that they will end up with the joints attached. That prevents an abrupt motion in the first frame after the entity is initialized as all of the shapes snap together.

Defining the Joints

The class holding the joint information is called ParentChild because it defines the joint that joins a child entity to its parent. This class and the joint descriptors are shown in the following code:

public class ParentChild

{

public string Parent; public string Child; public string JointName;

public Vector3 ChildConnect; public Vector3 ParentConnect; public Vector3 ParentNormal; public Vector3 ParentAxis; public Vector3 ChildNormal; public Vector3 ChildAxis;

public ParentChild(string child, string parent, string jointName, Vector3 childConnect, Vector3 parentConnect, Vector3 childNormal, Vector3 childAxis, Vector3 parentNormal, Vector3 parentAxis)

{

Child = child; Parent = parent;

JointName = jointName; ChildConnect = childConnect; ParentConnect = parentConnect; ParentNormal = parentNormal; ParentAxis = parentAxis; ChildNormal = childNormal; ChildAxis = childAxis;

}

}

static Vector3 X = new Vector3(1, 0, 0); static Vector3 Y = new Vector3(0, 1, 0); static Vector3 Z = new Vector3(0, 0, 1); static Vector3 nX = new Vector3(-1, 0, 0); static Vector3 nY = new Vector3(0, -1, 0); static Vector3 nZ = new Vector3(0, 0, -1);

ParentChild[] Relationships = new ParentChild[]

{

new ParentChild(“FRShoulder”, “Hexapod”, “FRShoulder”, new Vector3(0, 0, _upperLegRadius),

new Vector3(_bodyWidth/2, 0, _bodyLength/2 - _upperLegRadius), Y, X, -X, Y),

434

www.it-ebooks.info

Chapter 9: Adventures in Simulation

new ParentChild(“FRUpper”, “FRShoulder”, “FRShoulder2”, new Vector3(0, _upperLegLength/2, 0),

new Vector3(0,0,0), Y, X, Z, -Y),

new ParentChild(“FRLower”, “FRUpper”, “FRElbow”, new Vector3(0, _lowerLegLength/2, 0),

new Vector3(0,-_upperLegLength/2,0), Y, X, Y, X)

}

The ParentChild class specifies the name of the child entity, the name of the parent entity, the name of the joint; the position of the joint relative to the child, the position of the joint relative to the parent; and the normal and axis vectors of the child, and the normal and axis vectors of the parent. This information enables each joint to be fully specified.

Just like the shapes, the joint positions are specified using the size and radius variables defined previously. This makes it easy to change the size of the legs or body without having to redefine all of the shapes and joints.

Notice that the joints are defined with a different orientation for the parent connector and the child connector. This causes the shapes to be joined at right angles and ensures that the axis of each joint is correct. The Shoulder joint moves up and down relative to the hexapod body, while the Shoulder2 joint moves backward and forward.

Building the Hexapod

Now that all the hard work has been done in defining the shapes and joints, the code that actually builds the hexapod is very simple. The first part of the method creates the shapes and wraps them with

SegmentEntities:

string _parentName = “Hexapod”;

HexapodEntity CreateHexapod(string name, Vector3 initialPosition)

{

Dictionary<string, VisualEntity> hexapodShapes = new Dictionary<string, VisualEntity>();

string prefix = name + “_”;

foreach(HexapodShapeDescriptor desc in ShapeDescriptors)

{

Shape newShape = null; switch(desc.ShapeID)

{

case Shapes.Box:

newShape = new BoxShape(new BoxShapeProperties( desc.Name + “ Shape”, (float)desc.mass, new Pose(), new Vector3((float)desc.xSize, (float)desc.ySize, (float)desc.zSize)));

break;

case Shapes.Capsule:

(continued)

435

www.it-ebooks.info

Part II: Simulations

(continued)

newShape = new CapsuleShape(new CapsuleShapeProperties( desc.Name + “ Shape”, (float)desc.mass, new Pose(), (float)desc.radius, (float)desc.ySize));

break;

case Shapes.Sphere:

newShape = new SphereShape(new SphereShapeProperties( desc.Name + “ Shape”, (float)desc.mass, new Pose(), (float)desc.radius));

break;

}

SingleShapeEntity shapeEntity = null; if(desc.Name == _parentName)

{

shapeEntity = new HexapodEntity(newShape, new Vector3( (float)desc.xPosition + initialPosition.X, (float)desc.yPosition + initialPosition.Y, (float)desc.zPosition + initialPosition.Z));

}

else

{

shapeEntity = new SegmentEntity(newShape, new Vector3( (float)desc.xPosition + initialPosition.X, (float)desc.yPosition + initialPosition.Y, (float)desc.zPosition + initialPosition.Z));

}

shapeEntity.State.Name = prefix + desc.Name; shapeEntity.State.Pose.Orientation = UIMath.EulerToQuaternion(

new xna.Vector3((float)desc.xRotation, (float)desc.yRotation, (float)desc.zRotation));

if (!string.IsNullOrEmpty(desc.mesh)) shapeEntity.State.Assets.Mesh = desc.mesh;

hexapodShapes.Add(shapeEntity.State.Name, shapeEntity);

}

_parentName is used to hold the name of the shape that becomes the top-level parent. The code goes through each defined shape and creates the shape with the specified parameters and then creates an entity to hold the shape. If the shape is the parent shape, it becomes a HexapodEntity; otherwise, it becomes a SegmentEntity. The entity is given a name based on the top-level name specified as a parameter to the CreateHexapod method, and a visual mesh is assigned if one was specified for the shape. Each entity is stored in the hexapodShapes dictionary for later retrieval.

Creating and Attaching the Joints

Once the entities have been created, joints can be created and attached to them. The following code accomplishes this task:

// now set up the Parent/Child relationships foreach (ParentChild rel in Relationships)

436

www.it-ebooks.info

Chapter 9: Adventures in Simulation

{

string[] names = rel.JointName.Split(‘;’); JointAngularProperties angular = new JointAngularProperties();

if ((names.Length > 0) && (names[0] != string.Empty))

{

angular.TwistMode = JointDOFMode.Free; angular.TwistDrive = new JointDriveProperties(

JointDriveMode.Position,

new SpringProperties(500000, 1000, 0), 1000000);

}

if ((names.Length > 1) && (names[1] != string.Empty))

{

angular.Swing1Mode = JointDOFMode.Free; angular.SwingDrive = new JointDriveProperties(

JointDriveMode.Position,

new SpringProperties(500000, 1000, 0), 1000000);

}

if ((names.Length > 2) && (names[2] != string.Empty))

{

angular.Swing2Mode = JointDOFMode.Free; if(angular.SwingDrive == null)

angular.SwingDrive = new JointDriveProperties( JointDriveMode.Position,

new SpringProperties(500000, 1000, 0), 1000000);

}

EntityJointConnector[] connectors = new EntityJointConnector[]

{

new EntityJointConnector(hexapodShapes[prefix + rel.Child], rel.ChildNormal, rel.ChildAxis, rel.ChildConnect),

new EntityJointConnector(hexapodShapes[prefix + rel.Parent], rel.ParentNormal, rel.ParentAxis, rel.ParentConnect)

};

SegmentEntity child = (SegmentEntity)hexapodShapes[prefix + rel.Child]; child.CustomJoint = new Joint();

child.CustomJoint.State = new JointProperties(angular, connectors); child.CustomJoint.State.Name = rel.JointName;

hexapodShapes[prefix + rel.Parent].InsertEntityGlobal( hexapodShapes[prefix + rel.Child]);

}

This code attempts to configure the joint based on the joint name. If it contains more than one string separated by a semicolon, then multiple degrees of freedom are unlocked. In this example, each joint has only a single degree of freedom unlocked.

After the joint properties have been defined, the child entity is retrieved from the dictionary and its CustomJoint field is set to the newly created joint. The child entity is then inserted as a child into the parent entity using InsertEntityGlobal. When the SegmentEntity is initialized, it will replace its parent joint properties with the properties of its custom joint.

437

www.it-ebooks.info

Part II: Simulations

Attaching a Camera

The last task is to attach a camera to the front of the hexapod so that you can see the world from its perspective. You use the same AttachedCameraEntity that was used with the Lynx 6 Robotic Arm simulation:

HexapodEntity retValue = (HexapodEntity)hexapodShapes[prefix + _parentName];

// add a camera

AttachedCameraEntity cam = new AttachedCameraEntity(); cam.State.Name = name + “_cam”;

// move the camera to the front of the hexapod

cam.State.Pose = new Pose(new Vector3(0, 0, -_bodyLength / 2 + 0.02f));

//adjust the near plane so that we can see objects that are close cam.Near = 0.01f;

//use a visual model with the camera

cam.State.Assets.Mesh = “WebCam.obj”;

// the camera coordinates are relative to the hexapod retValue.InsertEntity(cam);

retValue.State.Flags |= EntitySimulationModifiers.Kinematic; retValue.State.Name = name;

return retValue;

}

That’s the entire hexapod! It has a total of 18 degrees of freedom with three joints in each of its six legs. The hexapod consists of a HexapodEntity, which is the top-level parent entity, and multiple child entities, which define the camera and the legs. The HexapodEntity has a MoveTo method much like the MoveTo method defined for the Lynx 6 Robotic Arm in the previous chapter. It enables the position of each joint to be specified along with a time value, and it will cause the joints to be moved to that position over the specified time.

The HexapodEntity service creates an entire simulation environment, including sky and ground and two hexapod entities. It uses the terrain from Simulation Tutorial 5 to give the hexapods something interesting to walk around. Any other service that needs to use a hexapod entity can simply add this service as a reference, just as was done with the Corobot entity in Chapter 7.

The Hexapod Differential Drive Service

At this point, you could simply run the HexapodEntity service and see a complete simulation environment with two hexapods, but you wouldn’t have a convenient way to make them walk around. You could start the JointMover service from Chapter 8, but that really only enables you to move one joint at a time, making it fairly tedious to control 18 joints.

The HexapodDifferentialDrive service comes to the rescue. This service implements the GenericDifferentialDrive contract, so it will work with the SimpleDashboard service and you will be able to control the motion of the hexapods just as if they were differential-drive robots.

438

www.it-ebooks.info

Chapter 9: Adventures in Simulation

This service was generated with the DssNewService utility, much like the

SimulatedQuadDifferentialDrive in Chapter 6. The command line was as follows:

dssnewservice /s:HexapodDifferentialDrive

/i:”\Microsoft Robotics Studio (1.5)\bin\RoboticsCommon.dll” /alt:”http://schemas.microsoft.com/robotics/2006/05/drive.html”

This service uses the usual strategy of subscribing to the simulation engine for an entity specified as its partner. When it receives a notification that the entity has been initialized, it spawns an iterator that is used to control the legs. Two different leg movements have been included with this service. If the entity name has a “2” in it, the second leg movement method is used.

The SetDrivePowerHandler method is executed when a SetDrivePower message is received. It sets the _left and _right member variables to indicate the current power assigned to each motor in the differential drive. These power values range from 1 to 1 and they are used by the leg movement methods to control how far the legs move during each cycle.

The MoveLeg Method

Let’s look at the MoveLegs method first. It uses the default ShoulderAngle, ElbowAngle, StepAngle, and LiftAngle defined by the HexapodEntity to define the angles used to make the legs walk. The first thing it does is extend the legs straight out so that the entity will be suspended above the ground. The HexapodEntity is initially created as a kinematic object so that it won’t be moved by the physics engine until it is fully initialized. This prevents any abrupt movements from moving the entity as all the pieces come together in the first frame after it is initialized.

After 200 milliseconds, the HexapodEntity is changed from a kinematic entity to a dynamic entity and it falls to the ground. Then the legs are pulled into the neutral position to enable the hexapod to stand:

const float minDrive = 0.05f;

public IEnumerator<ITask> MoveLegs()

{

float shoulderAngle = _hexapodEntity.ShoulderAngle; float elbowAngle = _hexapodEntity.ElbowAngle; float stepAngle = _hexapodEntity.StepAngle;

float liftAngle = _hexapodEntity.LiftAngle;

// legs straight out

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 1), delegate(SuccessResult s) { },

ShowError);

// wait for the leg movement inertia to settle yield return Arbiter.Receive(

false, TimeoutPort(200), delegate(DateTime dt) { });

_hexapodEntity.DeferredTaskQueue.Post(

new Task<VisualEntity>(_hexapodEntity, DeferredDrop));

(continued)

439

www.it-ebooks.info

Part II: Simulations

(continued)

// pull the legs in to the default position

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90, shoulderAngle, elbowAngle,

90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, 2), delegate(SuccessResult s) { }, ShowError);

Next, the method goes into an endless loop to move the legs according to the current values of _left and _right. The rightStepAngle is the maximum step angle multiplied by the _right motor power. As the right motor power is increased, the angle that the legs are moved forward and backward also increases and the speed of the entity increases. Each side of the hexapod is independently controlled, just as a differential drive would enable the hexapod to be steered in any direction and even driven in reverse.

If both _right and _left are near 0, the entity doesn’t need to move its legs at all, and the method waits 100 milliseconds and then tries again. If the legs do need to move, then a four-phase leg movement is started. First, the FrontRight, RearRight, and MiddleLeft legs are lifted and moved forward or backward according to the step angle. Next, these legs are dropped back to the ground in their new position. Next, the MiddleRight, FrontLeft, and RearLeft legs are lifted and moved forward or backward according to the step angle. At the same time, the previous three legs are drawn back to their original position. It is this motion that moves the entity relative to the ground. In the last phase, the second set of three legs is again brought back to the ground. When phase one starts again, these legs are brought back to their original position and the entity moves again:

// endless loop to make the legs move while (_hexapodEntity != null)

{

float stepTime = 0.1f;

shoulderAngle = _hexapodEntity.ShoulderAngle; elbowAngle = _hexapodEntity.ElbowAngle; stepAngle = _hexapodEntity.StepAngle; liftAngle = _hexapodEntity.LiftAngle;

float rightStepAngle = stepAngle * _right; float leftStepAngle = stepAngle * _left;

// no movement necessary

if ((Math.Abs(rightStepAngle) < minDrive) && (Math.Abs(leftStepAngle) < minDrive))

{

yield return Arbiter.Receive(

false, TimeoutPort(100), delegate(DateTime now) { }); continue;

}

// Raise FR, RR, ML

440

www.it-ebooks.info

Chapter 9: Adventures in Simulation

yield return Arbiter.Choice(_hexapodEntity.MoveTo(

90 + rightStepAngle, shoulderAngle + liftAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + rightStepAngle, shoulderAngle + liftAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle + liftAngle, elbowAngle, 90, shoulderAngle, elbowAngle, stepTime), delegate(SuccessResult s) { },

ShowError);

// Lower FR, RR, ML

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90 + rightStepAngle, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + rightStepAngle, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, stepTime), delegate(SuccessResult s) { },

ShowError);

// Raise MR, FL, FR

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90, shoulderAngle, elbowAngle,

90 + rightStepAngle, shoulderAngle + liftAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle + liftAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle + liftAngle, elbowAngle, stepTime), delegate(SuccessResult s) { },

ShowError);

// Lower MR, FL, FR

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90, shoulderAngle, elbowAngle,

90 + rightStepAngle, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle, elbowAngle, stepTime), delegate(SuccessResult s) { },

ShowError);

}

}

This is a relatively efficient walk algorithm and it works well with the differential-drive model. One problem is that sometimes only three legs are on the ground, which can reduce the stability and friction of the entity.

441

www.it-ebooks.info

Part II: Simulations

The MoveLegs2 Method

Another leg movement algorithm is shown in the MoveLegs2 method. This method follows the same initialization sequence as the first method except that the hexapod is never brought to a standing position. It lies on the ground with its legs extended. It uses a three-phase movement to move along the ground like an animal with flippers. It first raises its legs above the ground in unison and moves them forward or backward according to the values of _left and _right. Then it lowers its legs back to the ground in their new positions. Finally, it draws its legs back to their original position, and the body of the hexapod is dragged across the ground:

// Raise all legs

yield return Arbiter.Choice(_hexapodEntity.MoveTo(

90 + rightStepAngle, shoulderAngle - liftAngle, elbowAngle,

90 + rightStepAngle, shoulderAngle - liftAngle, elbowAngle,

90 + rightStepAngle, shoulderAngle - liftAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle - liftAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle - liftAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle - liftAngle, elbowAngle, stepTime), delegate(SuccessResult s) { },

ShowError);

// Lower all legs

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90 + rightStepAngle, shoulderAngle, elbowAngle, 90 + rightStepAngle, shoulderAngle, elbowAngle, 90 + rightStepAngle, shoulderAngle, elbowAngle, 90 + leftStepAngle, shoulderAngle, elbowAngle, 90 + leftStepAngle, shoulderAngle, elbowAngle,

90 + leftStepAngle, shoulderAngle, elbowAngle, stepTime), delegate(SuccessResult s) { },

ShowError);

// Back to neutral position

yield return Arbiter.Choice(_hexapodEntity.MoveTo( 90, shoulderAngle, elbowAngle,

90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle, 90, shoulderAngle, elbowAngle,

90, shoulderAngle, elbowAngle, stepTime), delegate(SuccessResult s) { }, ShowError);

This algorithm is surprisingly efficient in the simulator, although it would probably result in a much shorter life span for a real-world hexapod because it is constantly banged against the ground.

You can run the Hexapod simulation environment by typing Hexapod in the \Microsoft Robotics Studio (1.5) directory. It starts the Hexapod manifest, which in turn starts the HexapodEntity service and two copies of the HexapodDifferentialDrive service, along with the SimpleDashboard service. The Hexapod simulation environment is shown in Figure 9-9.

442