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

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

A Joint with Motion Limits

One more example: See what happens when you specify a joint limit. Consider the following joint definition:

//An angular joint with limited twist and swing2 name = “Limited”;

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

angular.TwistMode = JointDOFMode.Limited; angular.Swing2Mode = JointDOFMode.Limited; angular.TwistDrive = new JointDriveProperties(

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

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

//specify a limit on twist and swing2 with an angle of PI/8, 0 restitution, and no spring

angular.LowerTwistLimit = new JointLimitProperties((float)(-Math.PI / 8), 0, new SpringProperties(5000000, 1000, 0));

angular.UpperTwistLimit = new JointLimitProperties((float)(Math.PI / 8), 0, new SpringProperties(5000000, 1000, 0));

angular.Swing2Limit = new JointLimitProperties((float)(Math.PI / 8), 0, new SpringProperties(5000000, 1000, 0));

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” + “;” + name + “-swing2” + “;”;

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

position.X += 2;

Here, you have specified joint limits on the Twist DOF and the Swing2 DOF of about 22.5 degrees in either direction. This should constrain the motion of the child entity to lie within a cone with a half-cone angle of 22.5 degrees. You’ve also specified a large spring coefficient on the JointLimitProperties, which applies a significant force to keep the joint within the joint limits.

393

www.it-ebooks.info

Part II: Simulations

Once again, run the TestBench manifest and switch the camera to Limited_cam to view this joint. Move the Limited-twist and Limited-swing2 sliders to move the joint and notice that it is constrained to the specified limits. As the sliders get significantly outside the limit angle, the physics solver can become less stable and cause the joint to oscillate. This can be improved somewhat by increasing the damping coefficient in the JointLimitProperties.

That concludes the joint experiments we’ll cover in the TestBench example. It is hoped that these examples have explained some of the most important joint properties that affect joint behavior. The TestBench provides a great way to prototype a joint to see how it is going to behave in your application. In the following sections, you’ll learn how to use joints to build a robotic arm.

Building a Simulated Robotic Arm

Now you can use your newfound joint expertise to build a real-world object. In this section, you build a simulated robotic arm modeled after the Lynx 6 robotic arm. This hobbyist arm provides six independent degrees of freedom and is available at a reasonable price point. More information about this arm, shown in Figure 8-6, is available at www.lynxmotion.com/Category.aspx?CategoryID=25.

First, you’ll learn how to write code to build the physics model of the arm by creating entities and connecting them with joints. Next, you’ll learn how to add a visual model and then write a service to control the arm. Finally, you’ll learn how to use inverse kinematics to drive the arm to a specified position.

Figure 8-6

As you can see from the figure, the arm consists of a base that swivels. The upper arm segment pivots at its connection to the base and the lower arm segment pivots at its connection to the upper arm. The wrist segment pivots at its connection to the lower arm and rotates. Finally, the gripper connects to the wrist and has the capability to open and close.

The Physical Model of the Arm Entity

Refer to the SimulatedLynxL6Arm service in the Chapter 8 directory to see how the SimulatedLynxL6ArmEntity is defined. Figure 8-7, from the Lynxmotion website, was used to define the size and orientation of each arm segment.

394

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

Figure 8-7

These dimensions are defined in SimulatedLynxL6Arm.cs at the top of the SimulatedLynxL6Arm class definition. Notice that this entity subclasses the SingleShapeEntity. This is because the arm entity itself only includes a single shape, which is the base of the arm. Each of the other segments in the arm is a separate entity joined to another entity in the arm as a child. This makes it easy to use the built-in ParentJoint that belongs to each entity to join the entities together.

The following code defines the dimensions of each part of the arm:

static float InchesToMeters(float inches) { return (float)(inches * 0.0254); }

// physical attributes of the arm static float L1 = InchesToMeters(4.75f); static float L2 = InchesToMeters(4.75f);

static float Grip = InchesToMeters(2.5f); static float L3 = InchesToMeters(5.75f) - Grip; static float L4 = 0.03f;

static float H = InchesToMeters(3f); static float G = InchesToMeters(2f);

static float L1Radius = InchesToMeters(0.7f); static float L2Radius = InchesToMeters(0.7f); static float L3Radius = InchesToMeters(0.7f); static float GripRadius = InchesToMeters(0.2f);

Because the dimensions are given on the diagram in inches but the units used by the simulator are meters, a simple utility function is used to convert the units. H specifies the height of the base, and G is the radius. The L1 entity corresponds to the upper arm, and the L2 entity is the lower arm. In Figure 8-7, the L3 segment includes both the wrist and the grippers. The entity divides this into the L3

entity, which includes the wrist to just before the grippers; an L4 entity, used to make the wrist rotate; and the Grip entity, which is 2.5 inches long. The GripRadius refers to the radius of each part of the gripper.

395

www.it-ebooks.info

Part II: Simulations

In addition to the size of each segment, you need to define the joints that will be used to join the segments. The following class defines the characteristics of each joint, as well as some run-time properties of the joint such as the target position, the current position, and the current speed of motion:

// This class holds a description of each of the joints in the arm. class JointDesc

{

public string Name;

public float Min; // minimum allowable angle public float Max; // maximum allowable angle public PhysicsJoint Joint; // Phyics Joint

public PhysicsJoint Joint2; // Alternate Physics Joint (used for gripper) public float Target; // Target joint position

public float Current; // Current joint position

public float Speed; // Rate of moving toward the target position public JointDesc(string name, float min, float max)

{

Name = name; Min = min; Max = max; Joint = null;

Joint2 = null; Current = Target = 0; Speed = 30;

}

//Returns true if the specified target is within the valid bounds public bool ValidTarget(float target)

{

return ((target >= Min) && (target <= Max));

}

//Returns true if the joint is not yet at the target position public bool NeedToMove(float epsilon)

{

if (Joint == null) return false;

return (Math.Abs(Target - Current) > epsilon);

}

//Takes one step toward the target position based on the specified time public void UpdateCurrent(double time)

{

float delta = (float)(time * Speed); if (Target > Current)

Current = Math.Min(Current + delta, Target);

else

Current = Math.Max(Current - delta, Target);

}

}

The initialized properties of each joint are the name and the minimum and maximum angles that the joint supports. In the case of the grippers, you will be using linear joints, and the minimum and maximum values represent distances. ValidTarget provides a simple way to check a target joint position to determine whether it falls within the minimum and maximum for the joint. NeedToMove indicates whether the current joint position is within a certain distance of the target position.

396

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

UpdateCurrent calculates a new Current position based on the speed of movement and the time that has elapsed.

The joints for the arm are defined in an array of descriptors as follows:

// Initialize an array of descriptions for each joint in the arm JointDesc[] _joints = new JointDesc[]

{

new JointDesc(“Base”, -180, 180), new JointDesc(“Shoulder”, -90, 90), new JointDesc(“Elbow”, -65, 115), new JointDesc(“Wrist”, -90, 90),

new JointDesc(“WristRotate”, -90, 90),

new JointDesc(“Gripper”, 0, InchesToMeters(2))

};

Notice that the minimum and maximum for the elbow joint are a little unusual due to the physical constraints of the arm, and the minimum and maximum for the gripper joint are 0 inches and 2 inches, respectively. The gripper is actually modeled using two simulated joints, one for each of the parts of the gripper. From a logical standpoint, though, the gripper position is treated as a single joint that specifies the distance between the left and right gripper.

The segment dimensions and the joint descriptions are used in the initialization constructor to programmatically build the arm entity. In the SimulatedLynxL6Arm constructor, one of the first things you’ll notice is that the physics shape for the base is not quite as tall as the actual base. This arises from a design decision to only enable one degree of freedom per joint. The point where the upper arm attaches to the base could be thought of as having two degrees of freedom: the base rotation and the shoulder joint swivel. Instead, we have defined another entity called L0Entity between the base and the L1Entity. The ParentJoint of the L0Entity is used for the base rotation, and the ParentJoint of the L1Entity is used for the shoulder joint. The height of the base is lowered to prevent the L1Entity from colliding with the base when the shoulder joint is moved.

The base state and its shape are initialized with the following code:

//The physics shape for the base is slightly lower than the actual base

//so that the upper-arm segment does not intersect it as it moves around. float baseHeight = H - L1Radius - 0.001f;

State.Name = name; State.Pose.Position = position;

State.Pose.Orientation = new Quaternion(0, 0, 0, 1); State.Assets.Mesh = “L6_Base.obj”;

MeshTranslation = new Vector3(0, 0.026f, 0);

//build the base

BoxShape = new BoxShape(new BoxShapeProperties( “Base”,

150, // mass

new Pose(new Vector3(0, baseHeight / 2, 0), new Quaternion(0, 0, 0, 1)), new Vector3(G * 2, baseHeight, G * 2)));

Notice that the shape is a box, rather than a cylinder. The only way to specify a cylinder shape in the AGEIA engine is to use a mesh that is shaped like a cylinder with a

SimplifiedConvexMeshEnvironmentEntity. The box is simpler and works fine.

397

www.it-ebooks.info

Part II: Simulations

In the spirit of full disclosure, it should be mentioned that there has been no attempt to specify accurate masses for each of the arm components. A mass of 150 Kg has been specified for the base, which is not accurate. If you wish to attach this arm to another entity in the environment, you will want to specify more accurate mass values to each entity in the arm. It is possible to make the base of the arm move as a result of the inertia of swinging the upper part of the arm quickly. The base of the arm can be made immobile by making it kinematic using the following:

State.Flags |= EntitySimulationModifiers.Kinematic;

The default pose specified for the box places it baseHeight/2 above the shape origin. This puts the origin of the base entity on the ground instead of baseHeight/2 above the ground, making it easier to position within the world. We set a value on the MeshTranslation property of the base entity, moving the mesh associated with the base up to correspond with the position of the shape.

The next part of the arm to be defined is the L0Entity. All of the remaining entities in the arm are SingleShapeSegmentEntities. As you will recall from the TestBench application earlier in this chapter, this type of entity allows its ParentJoint to be overridden. The L0Entity contains a single sphere shape, whose radius is the same as the L1Entity. The ParentJoint of this entity controls the rotation of the base. It is created as shown here:

// build and position L0 (top of the base)

SphereShape L0Sphere = new SphereShape(new SphereShapeProperties( “L0Sphere”,

50,

new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)), L1Radius));

SingleShapeSegmentEntity L0Entity =

new SingleShapeSegmentEntity(L0Sphere, position + new Vector3(0, H, 0)); L0Entity.State.Pose.Orientation = new Quaternion(0, 0, 0, 1); L0Entity.State.Name = name + “_L0”;

L0Entity.State.Assets.Mesh = “L6_L0.obj”; L0Entity.MeshTranslation = new Vector3(0, -0.02f, 0); JointAngularProperties L0Angular = new JointAngularProperties(); L0Angular.Swing1Mode = JointDOFMode.Free;

L0Angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);

EntityJointConnector[] L0Connectors = new EntityJointConnector[2]

{

new EntityJointConnector(L0Entity,

new Vector3(0,1,0), new Vector3(1,0,0), new Vector3(0, 0, 0)), new EntityJointConnector(this,

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

};

L0Entity.CustomJoint = new Joint();

L0Entity.CustomJoint.State = new JointProperties(L0Angular, L0Connectors); L0Entity.CustomJoint.State.Name = “BaseJoint”;

this.InsertEntityGlobal(L0Entity);

The L0Entity contains a single sphere. Its center is positioned H meters above the ground so that its center corresponds to the base’s center of rotation. It is important that the initial position of the base and

398

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

L0Entity is very close to their position once they are connected with a joint. If the initial position is different, then the pieces will snap together violently during the first frame of the simulation and the arm will likely topple over. A joint is defined with the Swing1 degree of freedom unlocked so that the joint will rotate freely around the Y axis. The L0Entity is inserted as a child of the base entity using the InsertEntityGlobal method because the position of the L0Entity is defined in world coordinates, rather than coordinates relative to the base entity.

After all that work, you end up with the physics model shown in Figure 8-8.

Figure 8-8

It’s not very impressive yet but it gets better. Next, you attach the upper arm segment (L1Entity) to the

L0Entity:

// build and position L1 (upper arm)

CapsuleShape L1Capsule = new CapsuleShape(new CapsuleShapeProperties( “L1Capsule”,

2,

new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)), L1Radius,

L1));

SingleShapeSegmentEntity L1Entity =

new SingleShapeSegmentEntity(L1Capsule, position + new Vector3(0, H, 0)); L1Entity.State.Pose.Orientation = new Quaternion(0, 0, 0, 1); L1Entity.State.Name = name + “_L1”;

L1Entity.State.Assets.Mesh = “L6_L1.obj”; JointAngularProperties L1Angular = new JointAngularProperties(); L1Angular.TwistMode = JointDOFMode.Free;

(continued)

399

www.it-ebooks.info

Part II: Simulations

(continued)

L1Angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 1000, 0), 100000000);

EntityJointConnector[] L1Connectors = new EntityJointConnector[2]

{

new EntityJointConnector(L1Entity,

new Vector3(0,1,0), new Vector3(0,0,1), new Vector3(0, -L1/2, 0)), new EntityJointConnector(L0Entity,

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

};

L1Entity.CustomJoint = new Joint();

L1Entity.CustomJoint.State = new JointProperties(L1Angular, L1Connectors); L1Entity.CustomJoint.State.Name = “Shoulder|-80|80|”;

L0Entity.InsertEntityGlobal(L1Entity);

This is much like the code that was used to define the L0Entity except that you are now using a capsule shape for the segment. The entity is positioned exactly H meters above the ground so that its rounded end-cap coincides with the sphere in the L0Entity. Its center of rotation will correspond to the center of the L0Entity sphere.

When the joint position is specified for the L1Entity in L1Connects[0], a position of (0, -L1/2, 0)is specified for the joint position for which this coordinate is relative to the L1Entity. A position of (0,0,0) is specified for the L0Entity because this joint will be located exactly at the sphere’s center. When the joint is created, these two points on the two entities will be constrained to be in the same place. Notice that the name you give to the joint includes a minimum and maximum value that the JointMover service uses to scale its controls, as in the TestBench sample described previously.

Finally, the L1Entity is added as a child of the L0Entity and another segment of the arm is attached.

The L2Entity is created and joined to the L1Entity in much the same way. The only difference lies in the joint connectors, which are defined as follows:

EntityJointConnector[] L2Connectors = new EntityJointConnector[2]

{

new EntityJointConnector(L2Entity,

new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -L2/2, 0)), new EntityJointConnector(L1Entity,

new Vector3(0,1,0), new Vector3(0,0,1), new Vector3(0, L1/2, 0))

};

Notice that the normal vector specified for the L2Entity connector is (1,0,0) and the normal vector specified for the L1Entity connector is (0,1,0). This joins the entities at right angles to each other.

The L3Entity and L4Entity are created and joined in much the same way. At this point the physical model looks like Figure 8-9.

400

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

Figure 8-9

It is shown in wireframe view with the wrist joint slightly bent to better demonstrate how the entities are connected. The sphere in the middle of the wrist segment is really the end-caps of the L3Entity and the

L4Entity.

All that remains is to add the two gripper entities. The gripper is modeled as two capsules that move together and apart using linear joints. Although each part of the gripper is joined to the L4Entity with its own joint, the gripper is treated logically as if it has one joint. The following code adds the left gripper to the entity:

// build and position LeftGrip

CapsuleShape LeftGripCapsule = new CapsuleShape(new CapsuleShapeProperties( “LeftGripCapsule”,

1f,

new Pose(new Vector3(0, 0, 0), new Quaternion(0, 0, 0, 1)), GripRadius,

Grip));

LeftGripCapsule.State.DiffuseColor = new Vector4(0, 0, 0, 1);

LeftGripEntity = new SingleShapeSegmentEntity(

LeftGripCapsule, position + new Vector3(0, H, 0));

//position the entity close to its final position once the joint is connected LeftGripEntity.Position = new xna.Vector3(-0.24f, 0.19f, 0.01f); LeftGripEntity.Rotation = new xna.Vector3(179.94f, -176.91f, 89.67f); LeftGripEntity.State.Name = name + “_LeftGrip”;

//use a linear joint for the left grip

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

LeftGripLinear.XDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(5000000000, 1000, 0), 100000000);

EntityJointConnector[] LeftGripConnectors = new EntityJointConnector[2]

(continued)

401

www.it-ebooks.info

Part II: Simulations

(continued)

{

new EntityJointConnector(LeftGripEntity,

new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, -Grip/2, 0)), new EntityJointConnector(L4Entity,

new Vector3(1,0,0), new Vector3(0,0,1), new Vector3(0, L4/2, GripRadius))

};

LeftGripEntity.CustomJoint = new Joint(); LeftGripEntity.CustomJoint.State =

new JointProperties(LeftGripLinear, LeftGripConnectors); LeftGripEntity.CustomJoint.State.Name = “LeftGripJoint|-0.0254|0|”;

L4Entity.InsertEntityGlobal(LeftGripEntity);

The main difference from the other joints is that this is a linear joint. The X degree of freedom is unlocked. The range of the gripper is 0 to 2 inches, so the range of this single half of the gripper is 0 to 1 inch. One other difference is that a DiffuseColor is specified for the capsule shape in this entity. This is because there is no custom mesh specified for this entity. The default capsule shape, colored dark black, is used for the visual representation. With the grippers added, the complete physics model looks like the image shown in Figure 8-10.

Figure 8-10

Finally, a camera entity is mounted just above the L4Entity so that you can get a close-up view of what the arm is manipulating:

402