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

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

Just to make things more interesting, 10 dominos have been added to the scene. These dominos are SingleShapeEntities consisting of a single box shape. Their visual mesh provides a texture map for each face to make them more interesting.

Pressing the Reset button causes the currently executing motion sequence to stop, resetting the position of the dominos and the arm to their initial position using the following code:

// Place the dominos back in their initial positions public void ResetDominos()

{

for (int i = 0; i < DominoCount; i++)

{

Dominos[i].State.Pose = new Pose(InitialDominoPosition(i)); // Don’t move the domino until its Update method is executing

Dominos[i].DeferredTaskQueue.Post(new Task<int>(i, DeferredSetDominoPose));

}

}

// Restore the arm to its initial position public void ResetArmPosition()

{

_l6Arm.State.Pose = new Pose(new Vector3(0, 0, 0)); // Don’t set the pose until the Update method executes

_l6Arm.DeferredTaskQueue.Post(new Task(DeferredSetArmPose));

// wait for the arm motion to settle before making it non-kinematic Activate(Arbiter.Receive(false, TimeoutPort(500), delegate(DateTime now)

{

_l6Arm.DeferredTaskQueue.Post(new Task(ReleaseArm));

}));

}

//Returns the initial position of the requested domino Vector3 InitialDominoPosition(int i)

{

return new Vector3(InchesToMeters(2.5f) + i * 0.011f, 0, 0);

}

//Create and place DominoCount dominos

public IEnumerator<ITask> CreateDominos()

{

for(int i=0; i<DominoCount; i++)

{

Dominos[i] = new Domino(InitialDominoPosition(i), i); SimulationEngine.GlobalInstancePort.Insert(Dominos[i]);

}

yield break;

}

// This method sets the pose of a domino in its update method void DeferredSetDominoPose(int i)

(continued)

413

www.it-ebooks.info

Part II: Simulations

(continued)

{

Dominos[i].PhysicsEntity.SetPose(Dominos[i].State.Pose);

}

//Sets the arm pose in its update method void DeferredSetArmPose()

{

_l6Arm.PhysicsEntity.IsKinematic = true; _l6Arm.PhysicsEntity.SetPose(_l6Arm.State.Pose);

}

//Restores the arm to non-kinematic

void ReleaseArm()

{

_l6Arm.PhysicsEntity.IsKinematic = false;

}

Notice that the dominos are moved back to their original positions by posting a task to each of their deferred task queues. The task will execute during their Update method when it is safe to call

PhysicsEntity.SetPose to move them.

It is similar for the arm except you must also deal with the inertia of the arm. When the arm entity is moved quickly, its segments act like a pendulum, storing the inertia, and the physics engine causes the arm to continue moving during the next few frames. You can prevent this effect by setting the base of the arm as kinematic before you move it. After it is moved, the code waits for half a second before setting the arm back to its non-kinematic state.

Pressing the Park Arm button executes two calls to MoveTo as shown in the following code:

// This method puts the arm into its parked position public IEnumerator<ITask> ParkArm()

{

// Raise the grippers away from the working surface

yield return Arbiter.Choice(_l6Arm.MoveTo(0, 0, 80, 0, 0, 0.05f, 2), delegate(SuccessResult s) { },

ShowError);

// Move to the parked position

yield return Arbiter.Choice(_l6Arm.MoveTo(0, 80, -56, -75.2f, 0, 0.05f, 3), delegate(SuccessResult s) { },

ShowError);

}

The sequence of events in this code snippet is as follows:

414

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

3. The first delegate executes if the response message was a SuccessResult. Otherwise, the second delegate executes and prints the error information to the console.

6. Finally, the first delegate executes if the response message was a SuccessResult or the error message is printed.

Stacking yield return statements in an iterator method like this guarantees that the arm motions will be executed one after another in the proper order with no wasteful spin-waiting on the CPU.

The first MoveTo command raises the gripper nearly vertical, and the second one puts it into the parked position shown in Figure 8-14.

Figure 8-14

Clicking the Random Move button causes the arm to be moved to a random position. Random values for each of the parameters are chosen without regard to what is valid and then passed to MoveToPosition. This continues until MoveToPosition returns a SuccessResult, meaning the position specified was valid. Whenever MoveToPosition is called, the parameters on the Winforms window are updated.

Both the Reverse Dominos button and the Topple Dominos button execute a sequence of arm moves. The first sequence causes the arm to pick up the dominos one by one and put them back down again on the other side of the arm. The Topple Dominos button causes the arm to pick up the dominos one by one

415

www.it-ebooks.info

Part II: Simulations

and lay them out in a pattern. The dominos are then knocked down in classic fashion. The code for the Topple Dominos motion is shown here:

//This method executes a series of arm movements that cause the arm to pick up

//dominos from one side and put them down in a pattern on the other side and

//then the arm causes the dominos to fall.

public IEnumerator<ITask> ToppleDominos()

{

_moveSequenceActive = true;

for (int i = 0; i < DominoCount; i++)

{

Vector3 src = InitialDominoPosition(DominoCount - i - 1);

// move the arm into position to grasp the domino

yield return CheckAndMove(final.X, 0.05f, final.Z - 0.04f, -80, 0, 0, 1); yield return CheckAndMove(final.X, 0.05f, final.Z - 0.04f, -80, 0, 0, 0.5f);

// pause for dramatic effect

yield return Arbiter.Receive(false, TimeoutPort(500), delegate(DateTime now) { });

yield return CheckAndMove(final.X, 0.05f, final.Z + 0.06f, -80, 0, 0, 0.3f); yield break;

}

//This method calculates the wrist rotation angle that will orient the wrist to

//the specified angle relative to the X axis.

416

www.it-ebooks.info

Chapter 8: Simulating Articulated Entities

float AdjustWrist(float angle, float x, float z)

{

float val = angle + RadiansToDegrees((float)(Math.Atan2(-z, -x))) + 90; while (val < -90)

val += 180;

while (val > 90) val -= 180;

return val;

}

// check for early termination of the motion and call MoveToPosition ITask CheckAndMove(

float x, float y, float z, float gripAngle, float gripRotation, float grip, float time)

{

if (_moveSequenceActive)

{

return Arbiter.Choice(

MoveToPosition(x, y, z, gripAngle, gripRotation, grip, time), delegate(SuccessResult s) { },

ShowError);

}

else

{

return new Task(DoNothing);

}

}

void DoNothing()

{

}

// The destination positions for the dominos in the topple movement Vector3[] ToppleDominoPosition = new Vector3[]

{

new Vector3(-0.11f,0,0.125f), new Vector3(-0.12f,0,0.10f), new Vector3(-0.125f,0,0.075f), new Vector3(-0.12f,0,0.05f), new Vector3(-0.11f,0,0.025f), new Vector3(-0.10f,0,0.00f), new Vector3(-0.095f,0,-0.025f), new Vector3(-0.10f,0,-0.05f), new Vector3(-0.11f,0,-0.075f), new Vector3(-0.12f,0,-0.10f)

};

The CheckAndMove method ensures that the sequence is still active. If the Reset button is clicked, _moveSequenceActive becomes false and a null task is returned instead of the

MoveToPosition task. The ToppleDominoPosition array specifies where the arm will place each domino (see Figure 8-15).

417

www.it-ebooks.info

Part II: Simulations

Look at the calls to the AdjustWrist method. This method calculates the angle that the gripper will make relative to the X axis and then adjusts that angle by rotating the wrist so that the domino is placed at the specified angle.

Have some fun putting the arm through its paces. You can change the pattern of the dominos by editing the ToppleDominoPosition array or you can define entirely new motion sequences. As you debug a new motion sequence, watch the console output for messages indicating that a requested motion is invalid or set a breakpoint on the ShowError method.

Figure 8-15

Summary

This chapter explored the world of joints and articulated entities. You learned how to join two entities together with a joint and became familiar with the many properties of a joint. The TestBench sample service provided a way to examine the behavior of different joint types and to prototype new joints.

The SimulatedLynxL6Arm service showed how to build a robotic arm entity with six degrees of freedom. The arm service included a user interface and inverse kinematics code that made it easy to define new motion sequences for the arm to perform.

Chapter 9 includes additional examples of articulated entities in the simulation environment.

418

www.it-ebooks.info

Part II: Simulations

Figure 9-1

The Sumo package is a great place to get started with writing and modifying MRDS services because most of the infrastructure has already been provided and a complete sumo strategy can be implemented by making just a few changes to the existing code.

This section explains how to use the simulated sumo services and how to implement a new sumo player service with a unique strategy.

SimulatedSumo

In a sumo competition, two robots are placed within a circular sumo ring where they attempt to push each other outside the ring. Each sumo match consists of three rounds that last, at most, one minute each. When a robot leaves the sumo ring, either under its own power or by being pushed out, the round ends and the remaining robot scores one point. If neither robot leaves the ring after one minute, the round is a tie and neither robot scores a point. At the end of three rounds, the robot with the most points wins

the match.

The sumobot uses its front bumpers to sense when it has made contact with its opponent, and it

uses the webcam to locate its opponent. The infrared sensors on the underside are used to determine when the sumobot is getting close to the edge of the sumo ring. Note in Figure 9-1 that on the outside edge

of the circular ring is a band that is painted a different color and has a different reflectivity than the interior of the ring. The IR sensors on each robot can detect when the robot is over this region, and the robot can adjust its course so that it doesn’t drive out of the ring unless it is being pushed by the other sumobot.

The Sumo package is available from the Microsoft Robotics website (www.microsoft.com/robotics) under Downloads. When you download and install the package, it installs the source code for the SimulatedIRobotLite service and the SumoPlayer service to samples\simulation\competitions\ SimulatedSumoServices. The binaries for these services are installed in the bin directory along with the binaries for the SumoReferee service.

The Sumo robot used with this package is built with an iRobot Create, an onboard computer running Windows CE, and a webcam. You can find the instructions for building this platform at http://msdn2

.microsoft.com/en-us/robotics/bb403184.aspx.

420

www.it-ebooks.info

Chapter 9: Adventures in Simulation

Figure 9-2 shows the complete hardware platform alongside its simulation model.

Figure 9-2

You can see that the sumobot uses an iRobot Create for its motor platform. The Create has two powered wheels and uses a differential drive to steer. In addition, there are left-front and right-front bumper sensors and four IR sensors on the underside. The ICOP eBox 2300 processor that runs Windows CE is mounted on the transparent platform just above the payload bay. A webcam is mounted just above that to provide vision capability for the sumobot.

The Simulated Sumo Referee

The SimulatedSumoReferee service constructs the simulated sumo ring environment, places the competitors in the sumo ring, and then runs the match and determines the winner. The user interface for the SimulatedSumoReferee service is shown in Figure 9-3. (Never mind that the skinny cartoon referee looks more like a soccer referee than a sumo referee.)

Figure 9-3

421

www.it-ebooks.info

Part II: Simulations

You can run the SimulatedSumoReferee service by typing sumoreferee from the c:\Microsoft Robotics Studio (1.5) directory. This runs the sumoreferee.cmd file in the bin directory, installed along with the software in this book. This command file starts the simulatedsumoreferee manifest installed with the Sumo package described previously.

When the SimulatedSumoReferee service runs, it starts the simulation engine and builds the sumo ring environment, which consists of a sumo ring, a ground plane, and an overhead light. No sky is used in this simulation environment. The Sumo Referee user interface displays a drop-down box listing all the available sumo players on the system, identified by their contracts. The sample sumo player provided with the sumo package is called “samplesumo,” and the sumo player that you develop in this section and which is provided with the book software is called “cyclone.” In addition to the sumo players, the following options are available:

Refresh: This causes the list of players to refresh in case a new player has been installed on the system.

Continuous: Selecting this option causes the referee to pick two players at random and start the match. When the first match is complete, two more players are selected and another match starts.

None: This prevents a player from being selected. This is handy when you want to observe the behavior of your sumobot without the annoyance of another crazed robot trying to push it out of the ring. Of course, a match of None against None is not very exciting to watch.

Qualify: The qualifying round was used in the MEDC competition to enable developers to show that they had a functioning sumobot service prior to being admitted to the hardware round.

A sumobot is specified for the first player and “Qualify” is selected for the second player. The referee then starts a match with a sumobot opponent that does not move but patiently waits to be pushed out of the ring.

Try playing a match by selecting two players and then clicking the Start Match button. The Referee service takes care of placing the robots and starting each round. It also keeps track of the time and the score. At the end it declares a winner or a tie. A sumo round in progress is shown in Figure 9-4.

Figure 9-4

422