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

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

or the Referee service from the previous chapter. Its purpose is to create a very long box called TestBenchEntity and then to create a number of test joints connected to SingleShapeEntities so that you can experiment with various joint parameters. It also creates a camera pointed at each joint so that it is very easy to switch to a view that shows the joint of interest.

Subclassing Entity Types to Add Custom Joints

Before getting into the details about the joints created in the PopulateWorld method, it is important to define a new class, which you’ll use for the segments of articulated entities. This class inherits

from SingleShapeEntity and is called SingleShapeSegmentEntity. It adds an additional property called CustomJoint, which is of type Joint. The entire definition of this class is shown here:

///<summary>

///Defines a new entity type that overrides the ParentJoint with

///custom joint properties. It also handles serialization and

///deserialization properly.

///</summary>

[DataContract]

public class SingleShapeSegmentEntity : SingleShapeEntity

{

private Joint _customJoint;

[DataMember]

public Joint CustomJoint

{

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

}

///<summary>

///Default constructor

///</summary>

public SingleShapeSegmentEntity() { }

///<summary>

///Initialization constructor

///</summary>

///<param name=”shape”></param>

///<param name=”initialPos”></param>

public SingleShapeSegmentEntity(Shape shape, Vector3 initialPos) : base(shape, initialPos)

{

}

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

{

base.Initialize(device, physicsEngine);

// update the parent joint to match our custom joint parameters if (_customJoint != null)

(continued)

383

www.it-ebooks.info

Part II: Simulations

(continued)

{

If(ParentJoint != null) PhysicsEngine.DeleteJoint((PhysicsJoint)ParentJoint);

// restore the entity pointers in _customJoint after deserialization if (_customJoint.State.Connectors[0].Entity == null)

_customJoint.State.Connectors[0].Entity = FindConnectedEntity( _customJoint.State.Connectors[0].EntityName, this);

if (_customJoint.State.Connectors[1].Entity == null) _customJoint.State.Connectors[1].Entity = FindConnectedEntity(

_customJoint.State.Connectors[1].EntityName, this);

ParentJoint = _customJoint; PhysicsEngine.InsertJoint((PhysicsJoint)ParentJoint);

}

}

VisualEntity FindConnectedEntity(string name, VisualEntity me)

{

//find the parent at the top of the hierarchy while (me.Parent != null)

me = me.Parent;

//now traverse the hierarchy looking for the name return FindConnectedEntityHelper(name, me);

}

VisualEntity FindConnectedEntityHelper(string name, VisualEntity me)

{

if (me.State.Name == name) return me;

foreach (VisualEntity child in me.Children)

{

VisualEntity result = FindConnectedEntityHelper(name, child); if (result != null)

return result;

}

return null;

}

///<summary>

///Override the base PreSerialize method to properly serialize joints

///</summary>

public override void PreSerialize()

{

base.PreSerialize();

PrepareJointsForSerialization();

}

}

384

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

This entity takes advantage of the fact that the simulation engine creates a fixed joint between a child and parent entity to join them together. In its override of the Initialize method, it first calls base.Initialize, which will, among other things, create the ParentJoint, which joins this entity to its parent entity if it is a child entity. If the CustomJoint has been defined, the Initialize method deletes the rigid ParentJoint that was automatically created and creates a new ParentJoint based on the properties of the CustomJoint. It is then very easy to define custom joints between child and parent entities simply by specifying the properties of CustomJoint.

This class also has some special code to handle proper serialization and deserialization of joints. A joint contains two connectors that each have a reference to the entity that is connected. When the

SingleShapeEntity entity is serialized, either to be copied to disk or sent in a message to a service on another node, these entity references cannot be serialized; therefore, when a segment entity is deserialized, its entity references must be restored. This is done by overriding the PreSerialize method with a new method that calls PrepareJointsForSerialization after calling the base class

PreSerialize.

PrepareJointsForSerialization is a method defined on VisualEntity that uses reflection to traverse all the fields in the entity to search for joints. When it finds a joint, it writes the name of the entity referenced by each EntityJointConnector into the EntityName field of each connector. This property is correctly serialized and it provides the information necessary to restore the entity connection after deserialization.

When a SingleShapeSegmentEntity is deserialized and then initialized, it eventually executes the code in the Initialize method override that checks whether either of the entity connectors is null.

If so, the FindConnectedEntity method is used to traverse to the parent entity and then through all of the children entities to find an entity with the name contained in the EntityName property. When the entity is found, the reference to that entity is restored in the connector.

The SingleShapeSegmentEntity provides a SingleShapeEntity that supports custom

ParentJoints and proper serialization and deserialization of joint objects within the entity. It is a simple matter to add the relevant code to other entity types by subclassing them to give them this same functionality.

Adding Joints to the Test Bench

Now you are ready to define a test bench to experiment with the various joint properties. In the next few sections, you’ll add five different types of joints to the test bench so that you can see how they work.

In the PopulateWorld method in TestBench.cs, you begin by creating sky and ground entities as usual. Then you create a box entity that is 50 meters long. You will attach other entities to this box at two-meter intervals.

385

www.it-ebooks.info

Part II: Simulations

A Joint with One Angular Degree of Freedom

The first joint is the simplest possible revolute joint, with one degree of freedom:

// Simple single DOF joint name = “1DOF”;

segment = NewSegment(position + new Vector3(0, 0.5f, 0), name); angular = new JointAngularProperties();

angular.TwistMode = JointDOFMode.Free;

angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 100000, 0), 100000000);

connectors = new EntityJointConnector[2]; connectors[0] = new EntityJointConnector(

segment,

new Vector3(0,1,0), new Vector3(1,0,0),

new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y/2 - segment.CapsuleShape.State.Radius, 0));

connectors[1] = new EntityJointConnector( benchEntity,

new Vector3(0,1,0), new Vector3(1,0,0),

new Vector3(position.X, position.Y + 0.25f, position.Z));

segment.CustomJoint = new Joint();

segment.CustomJoint.State = new JointProperties(angular, connectors); segment.CustomJoint.State.Name = name + “-twist”; benchEntity.InsertEntityGlobal(segment);

AddCamera(position, name); position.X += 2;

The name variable is the base name for the segment, joint, and camera that will be created. You begin by creating a new SingleShapeSegmentEntity at the position defined by the position variable but raised up by half a meter so that its button rests on the top of the testbench.

Because this will be a revolute joint, you define a new JointAngularProperties variable and free the TwistMode degree of freedom. Recall from Figure 8-1 that the TwistMode degree of freedom is associated with the local axis.

Now you define the drive characteristics that will move the joint around this degree of freedom. The JointDriveProperties specify that you will be changing the position of the joint, rather than its velocity. They also specify the spring properties of the joint and the maximum torque that can be applied to the joint to force it to move to its target position. The spring properties enable a spring coefficient and a damping coefficient to be specified. The spring coefficient describes the amount of force that will be applied to the joint to move it toward its target position proportional to the distance from that target position. The damping coefficient roughly represents the friction of the joint to move. You’ll examine the effect of lowering the damping coefficient in the second joint you create.

Next, the joint connectors are defined. The first connector attaches to the SingleShapeSegment entity you just created. In addition to the entity, the connector allows the local axis, the normal axis, and the connection point to be specified. In this case, you define the local axis to lie along the X axis (1,0,0) and the normal axis to lie along the Y axis (0,1,0). This leaves the binormal axis to lie along the Z axis (0,0,1) even though it is not specified here. The final parameter is the location on the segment entity where the

386

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

joint is attached. The coordinates specified here are relative to the origin of the segment entity. You specify the bottom-most point of the capsule shape.

The next connector references the TestBenchEntity. It defines the normal and local axes the same way. Later you’ll examine what happens if they are defined differently in the two connectors. The connection point is specified on the top surface of the testbench with X and Z coordinates according to the current position variable.

Now you create a Joint object and assign it to the CustomJoint property. You create a new JointProperties object using the angular joint properties and the joint connectors that you’ve already defined and assign it to CustomJoint.State. You give the joint a name and then insert the

SingleShapeSegmentEntity as a child of the TestBenchEntity. You use InsertEntityGlobal because the coordinates you have given the segment entity are defined in world space, not relative to the

TestBenchEntity.

Finally, create a camera that will look right at the joint you’ve just created. Run the TestBench manifest by typing the following in the MRDS command window. Alternatively, you can set the TestBench project as the StartUp project and then press F5 to run it.

C:\>cd “Microsoft Robotics Studio (1.5)”

C:\Microsoft Robotics Studio (1.5)>testbench

You should see a simulation window open with a scene similar to the one shown in Figure 8-2.

Figure 8-2

Press the F8 key once to switch the main camera to the 1DOF_cam camera to get a view of the segment entity you just created and attached. The TestBench manifest also runs the JointMover service, which can be used to move the joints you’ve specified. The user interface for this service is shown in Figure 8-3.

387

www.it-ebooks.info

Part II: Simulations

Figure 8-3

Each slider on the Joint Mover window corresponds to a degree of freedom in a joint on the entity specified in the Entity Name box. For scenes with multiple jointed entities, you can type a new entity name in the Entity Name box and press the change button to select the new entity. The sliders will reconfigure to match the joints in the new entity.

You can use the Suspend checkbox to set the top-level parent of the entity to kinematic and raise it off the ground a bit. This can be useful to test the joint movement of an entity that rests on the ground.

Slide the 1DOF-twist slider on the Joint Mover window to move the joint back and forth, as shown in Figure 8-4.

Figure 8-4

388

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

As you move the slider back and forth, the joint rotates around the X axis between 180 degrees and 180 degrees. Note that if you move the slider quickly, the motion of the entity lags slightly behind.

A Joint with a Smaller Damping Coefficient

For the next experiment, you’ll create a joint with a much smaller damping coefficient on the drive to see how that affects the behavior of the joint. Here is the code for the “SmallDamper” joint:

// A joint with a very small damping coefficient on the drive name = “SmallDamper”;

segment = NewSegment(position + new Vector3(0, 0.5f, 0), name); angular = new JointAngularProperties();

angular.TwistMode = JointDOFMode.Free;

angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);

connectors = new EntityJointConnector[2]; connectors[0] = new EntityJointConnector(

segment,

new Vector3(0, 1, 0), new Vector3(1, 0, 0),

new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

connectors[1] = new EntityJointConnector( benchEntity,

new Vector3(0, 1, 0), new Vector3(1, 0, 0),

new Vector3(position.X, position.Y + 0.25f, position.Z));

segment.CustomJoint = new Joint();

segment.CustomJoint.State = new JointProperties(angular, connectors); segment.CustomJoint.State.Name = name + “-twist”; benchEntity.InsertEntityGlobal(segment);

AddCamera(position, name); position.X += 2;

This joint definition is identical to the previous 1DOF joint except for the damping coefficient specified in the SpringProperties of the TwistDrive. The damping coefficient used is 100 instead of the previous value of 100000.

Run the TestBench manifest again and select the “SmallDamper_cam” camera to look at the second joint. Move the SmallDamper-twist slider in the Joint Mover window to move the joint in the same way as you moved the first joint. You should notice that the joint is much more responsive and crisp in its movements. There is little or no lag between the movement of the slider and the movement of the joint. The damping coefficient can be a useful parameter to model the friction of the joint. Be careful not to make this value too large, though. A large damping coefficient relative to the spring coefficient can mean that the joint will never quite reach the target position when it is moved.

389

www.it-ebooks.info

Part II: Simulations

A Six Degree of Freedom Joint

Next, you’ll define a joint with all six degrees of freedom unlocked. The code for this joint is shown here:

// A joint with all 6 DOF free name = “6DOF”;

segment = NewSegment(position + new Vector3(0, 0.5f, 0), name); angular = new JointAngularProperties();

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

JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000); angular.SwingDrive = new JointDriveProperties(

JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);

linear = new JointLinearProperties(); linear.XMotionMode = JointDOFMode.Free;

linear.XDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);

linear.YMotionMode = JointDOFMode.Free;

linear.YDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);

linear.ZMotionMode = JointDOFMode.Free;

linear.ZDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);

connectors = new EntityJointConnector[2]; connectors[0] = new EntityJointConnector(

segment,

new Vector3(0, 1, 0), new Vector3(1, 0, 0),

new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

connectors[1] = new EntityJointConnector( benchEntity,

new Vector3(0, 1, 0), new Vector3(1, 0, 0),

new Vector3(position.X, position.Y + 0.25f, position.Z));

segment.CustomJoint = new Joint();

segment.CustomJoint.State = new JointProperties(angular, connectors); segment.CustomJoint.State.Linear = linear; segment.CustomJoint.State.Name =

name + “-twist” + “;” + name + “-swing1” + “;” + name + “-swing2” + “;” + name + “-X|-2|2|” + “;” + name + “-Y|-2|2|” + “;” + name + “-Z|-2|2|” + “;” ;

benchEntity.InsertEntityGlobal(segment); AddCamera(position, name);

position.X += 2;

390

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

Some parts of this code require a little more explanation. When the angular properties are defined, all three angular degrees of freedom are freed but only two drives are defined: a TwistDrive and a SwingDrive. The AGEIA physics engine applies the SwingDrive characteristics to drive both the Swing1 DOF and the Swing2 DOF, even if they are both freed at the same time. This differs from the linear properties, for which a separate drive is allowed for each of the X, Y, and Z degrees of freedom.

When the JointProperties object is created, you pass the angular properties and the connectors as parameters and then assign the linear properties later.

The JointMover service determines how to map sliders to the joint by the name of the joint. This joint needs six different sliders, so six different names are provided in the joint name, separated by semicolons. There should be a name for each degree of freedom that is free or limited in the joint, in the order of Twist, Swing1, Swing2, X, Y, and Z degrees of freedom. Furthermore, the minimum and maximum values of each slider can be specified with two optional floating-point parameters following the name and separated with the “|” character. The default minimum and maximum values for each slider are –180 and 180, respectively. The only requirement that the simulation engine imposes on the joint name is that it must be unique relative to all the other joint names.

Run the TestBench manifest again and select the “6DOF_cam” camera to look at the third joint. Six different sliders appear in the Joint Mover window with the 6DOF name prefix to control each

free degree of freedom. Experiment by moving the sliders, especially the linear DOF sliders. The 6DOF joint provides a good opportunity to understand the physics visualization for joints. Click Render Physics to put the simulator in physics visualization mode. Use the 6DOF-Y slider to move the

SingleShapeSegmentEntity up off of the TestBenchEntity. You can see the frame for each connection point, with a line joining them that represents the linear positional offset, as shown in Figure 8-5.

Figure 8-5

391

www.it-ebooks.info

Part II: Simulations

The frame displayed on the TestBenchEntity shows the point where the connection is made, as well as the orientation of the connection. When viewed on the screen, the red vector represents the local axis, the green vector represents the normal axis, and the blue vector represents the binormal axis. The frame displayed at the base of the capsule shape represents the connection point for the child entity. This frame is rotated by the angular rotation of the joint and offset by the linear position of the joint. If you look closely, you can see that there are actually two frames shown at the bottom of the capsule shape. One frame represents the target position, while the other frame represents the current position. Because the damping coefficient is set very low for this joint, the two frames are almost always aligned. With a higher damping coefficient, you would see one frame lag behind the other as the joint is moved.

A Joint with Different Connector Orientations

Up until now, you’ve always specified the orientation of each connector to be the same. What happens if they are different? This sounds like a job for the TestBench! Here is the code for a single DOF joint that has a different orientation for each connector:

// Simple single DOF joint with differing connector orientations name = “Orientation”;

segment = NewSegment(position + new Vector3(0, 0.5f, 0), name); angular = new JointAngularProperties();

angular.Swing1Mode = JointDOFMode.Free;

angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 100, 0), 100000000);

connectors = new EntityJointConnector[2]; connectors[0] = new EntityJointConnector(

segment,

new Vector3(0, 0, 1), new Vector3(0, 1, 0),

new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

connectors[1] = new EntityJointConnector( benchEntity,

new Vector3(0, 1, 0), new Vector3(1, 0, 0),

new Vector3(position.X, position.Y + 0.25f, position.Z));

segment.CustomJoint = new Joint();

segment.CustomJoint.State = new JointProperties(angular, connectors); segment.CustomJoint.State.Name = name + “-swing1”; benchEntity.InsertEntityGlobal(segment);

AddCamera(position, name); position.X += 2;

In this joint definition, you have specified the local axis of the child connector to align with the Y axis, and the normal axis to align with the Z axis. The parent connector follows the convention that the other joints have used, with the local axis aligned with the X axis and the normal axis aligned with the Y axis. Start the TestBench manifest and switch to the Orientation_cam camera to see what the result of this joint definition is. Switch to the physics view and you will see that the simulation engine has oriented the child entity so that the axes of each of the connectors line up. A difference in connector orientation means a difference in orientation between the joined entities.

392