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

www.it-ebooks.info

Chapter 15: Using a Robotic Arm

respective axis directions, although the origin should be located at the center of the base. Ignore the coordinate system for now and assume that the robot is in the X-Y plane. Note that rotations are measured counterclockwise around a joint.

As you traverse the arm from the base to the tip, the angles of the joints add up to determine the angle of each link measured with respect to the horizontal. In Figure 15-3, the shoulder is about 110 degrees with respect to the base, the elbow is at 95 degrees with respect to the upper arm, and the wrist is at −30 degrees with respect to the lower arm. (The diagram is not drawn to scale.)

Consider each link as a simple straight line that is the hypotenuse of a right-angled triangle. The contribution of each link to the total depth and height are the horizontal and vertical edges of this triangle and can be calculated based on the angle of the link, α, and its length, L. Figure 15-4 shows the side-on view of a single link, which might be the upper arm, lower arm, or hand.

Link Length, L

Height L sin( )

Link Angle,

Depth L cos( )

Figure 15-4

For the L6 arm in the position in Figure 15-3, the calculations are shown in the following table:

663

www.it-ebooks.info

Part IV: Robotics Hardware

To determine the angles for each of the links, you need to take into account the way that the servo is fixed to the respective links and translate the servo angles to link angles. For example, when the shoulder servo has a setting of zero degrees, it is standing upright, i.e., the upper arm (L1) is at 90 degrees. A summary of the servo orientations and directions of travel is given in the following table:

*If you look at the physical mounting of the elbow, you can see that the screws are offset so that it actually makes a 70-degree angle between the upper arm and lower arm when it is in the zero position. However, the software adjusts for this difference, so if you request an angle of zero for the elbow servo, it moves to make a 90-degree angle between the upper and lower arms. This offset is necessary because if the upper arm moved all the way to a servo setting of +90, it would hit the cross bracing in the lower arm. It is free to move to 90 in the opposite direction, although this is not advisable. In addition, the simulation uses a direction of rotation for the elbow that is opposite to the real arm, but this is hidden from you by the arm service.

The depth and height show the position of the tip relative to the base. The height immediately translates to the Y coordinate in 3D space. However, the base can also rotate, so to obtain the X and Z coordinates you use the depth and the angle of base rotation. The depth corresponds to the radius of rotation measured on the ground (looking down at the arm from above).

To complicate matters, the base servo starts with the arm facing away from the SSC-32 controller board and it moves clockwise for increasing servo angles. If you are sitting beside the robot with the SSC-32 on your right, which is the view that first appears in the simulation, the X coordinate is always negative. Unlike the simulation, the real L6 cannot rotate by 180 degrees in either direction.

Combining all of these calculations results in the (X, Y, Z) position of the tip, given all of the servo angles. The code to do this is in a routine called ForwardKinematics, which is in Lynx6Arm.cs in the folder ProMRDS\Chapter15\Lynxmotion\Lynx6Arm. There is no point in reproducing the code here. If you are interested you can read through it.

Inverse Kinematics

Unless you have a very expensive arm, it is not possible to instruct it to go to a particular location in 3D space. Instead, you must calculate the values of all of the joint angles that correspond to the desired pose. This is called inverse kinematics, and you can find books with entire chapters dedicated to this topic.

As noted in Chapter 8, there are spreadsheets available on the Lynxmotion website for performing the forward and inverse kinematics calculations.

664

www.it-ebooks.info

Chapter 15: Using a Robotic Arm

The Lynx6Arm service developed for this chapter was based on the original MRDS V1.0 code from the Lynxmotion website. In order to simplify the code, the Lynx Inverse Kinematics service was dropped and replaced by direct calculations inside the Lynx6Arm service itself. The algorithm was taken from Hoon Hong’s ik2.xls spreadsheet for inverse kinematics posted on the Lynxmotion website. This is much simpler and easier to understand (and debug).

Providing a general service to solve the kinematics equations is a good idea, but every articulated robot has a different design, and it is a difficult problem to generalize. For example, a humanoid robot is a very different configuration from a robotic arm, but it still needs to solve inverse kinematics equations to make it walk.

If you followed the forward kinematics discussion, then you can imagine the inverse kinematics as the opposite set of calculations. Unfortunately, in general, there can be more than one possible solution or no solution at all.

Multiple solutions to reaching a particular 3D point in space occur when you have a wrist. You can tilt your wrist up or down, but the tips of your fingers can still be at the same place. To resolve this issue, you must also specify the tilt angle of the wrist. This is also important when using a gripper because you often want to have the gripper horizontal when it picks up an object. That means four pieces of information are required: X, Y, Z, and P, where P is the wrist/hand angle.

There might be no solution if you have specified a point that is outside the reach of the robot. Even close to the robot, there can be places that it cannot reach. For example, the Lynx 6 would have a hard time trying to scratch itself — for the same reason that you cannot touch your elbow with your fingers (the elbow on the same arm as your hand — not the other arm, that’s cheating!).

Even if you do find a solution to the equations, the arm might be unstable if placed in that position. The Lynx6Arm service places some constraints on the servo angles to try to avoid overtaxing the servos. However, it is still possible to damage the arm if you are not careful.

Setting Up a Lynxmotion L6 Arm

The Lynx 6 Robotic Arm Combo Kit for PC available from Lynxmotion is a relatively inexpensive kit that the authors use to demonstrate many of the principles of robotic arms. For full details see www

.lynxmotion.com/Category.aspx?CategoryID=25.

The L6 kit has six degrees of freedom: Base Rotate, Shoulder, Elbow, Wrist, Wrist Rotate, and Gripper. The somewhat cheaper L5 kit does not include the Wrist Rotate, but we recommend that you save your pennies and buy the L6 because the extra degree of freedom is useful. Figure 15-5 shows the L6 arm. The SSC-32 controller board is at the right-hand side and you can see the serial port socket.

665

www.it-ebooks.info

Part IV: Robotics Hardware

Figure 15-5

Building the Lynxmotion L6 Arm

Building the arm will take you several hours. The assembly instructions are available online at the Lynxmotion website, and it might be wise to read them before you decide to buy an arm. As you build, consider the following:

The pieces are made out of lexan, which is very tough. They are laser cut, but the cuts don’t go all the way through in some places. Pushing out the smaller holes can be difficult, and the pieces have very tight tolerances. You should carefully drill out the holes with the correct size drill bit, and you might want to use a small file to trim a little off the various tongues before fitting them into their mating slots.

An SSC-32 servo controller is supplied as part of the kit. It is already assembled so you don’t need to do any soldering. It can handle up to 32 servos via a serial port connection with a PC. You can see the circuit board on the right-hand side in Figure 15-5. A very good manual is available for the SSC-32, and you should read it in conjunction with this chapter. It explains the format of the serial commands that can be sent to the arm. This information isn’t necessary, but it gives you a better idea of the capabilities of the SSC-32.

Although the L6 arm comes with some sophisticated control software called RIOS (robotic arm interactive operating system), the objective of this book is to show you how to control it using MRDS. The Lynx6Arm service on the book’s website (www.proMRDS.com or www.wrox.com) offers a simple interface for the arm with the advantage that you have the source code.

LynxTerm is basic software that enables you to manipulate the robot’s joints individually. The Motion Recorder service included in this chapter provides similar functionality for moving the joints. You can use either of these to test your arm once you have built it, although the assembly instructions refer to LynxTerm.

Starting the Lynxmotion L6 Arm

Of course, once you’ve built the arm you’ll want to try it out. Before you do, you might want to consider the following:

Make sure that you have plenty of clear space around the robot. It might do unexpected things if you enter a bad value for a parameter or make a coding mistake.

666

www.it-ebooks.info

Chapter 15: Using a Robotic Arm

Starting up the arm frequently results in it jerking. This happens because the L6 has no feedback from the servos. Consequently, when the SSC-32 powers up, it assumes that all of the servos are in their centered positions. In general, this is a bad assumption. The first time you try to move a servo, it might jump rapidly from its current position to the new position that you request.

Therefore, try to hold up the arm when you start it up so that it does not have as far to travel. You can move the linkages carefully while it is powered off without damaging the arm.

The arm has a tendency to oscillate if you move it too fast or try to pick up very heavy loads. There are springs that can be attached between the elbow and the shoulder to try to compensate for the load (look carefully at Figure 15-5 just behind the upper arm), but these add extra load and might even exacerbate the problem for some arm positions.

However, the springs do stop the arm from slumping to the ground when the power is turned off. This helps to protect the arm, and minimizes jerking when the arm is initialized because it is more likely to be near its starting position. It’s a good idea to reset the arm to the default position every time you use it before turning it off.

To make the robot more stable, it is highly advisable to screw it to a board or table. Unfortunately, this makes it difficult to move around if you want to take it somewhere to do a demo. Based on the length of the arm, a board 60cm by 90cm is more than big enough to cover the whole area that the arm can reach.

Controlling the L6 Arm

The code to control the L6 arm is not included with MRDS. A version of the code can be downloaded from the Lynxmotion website, but the companion websites for this book (www.wrox.com and www.ProMRDS.com) contain a heavily modified version that is used in the examples. This new version is used with the permission of Lynxmotion. All of the code is contained in the folder

ProMRDS\Chapter15\Lynxmotion\Lynx6Arm.

There are two parts to the code: the Lynx6Arm service and the SSC32 service. The simulated Lynx 6 robotic arm is in the Chapter 8 folder.

Before you attempt to use the software, you need to connect your arm to a serial port on your PC and make the appropriate change to the code. The SSC32 service has COM1 hard-coded into the

SerialPortConfig class in SSC32State.cs:

public class SerialPortConfig

{

//Serial port configuration parameters

//Baud rate is 115.2k with both jumpers private int _baudRate = 115200; private Parity _parity = Parity.None; private int _dataBits = 8;

private StopBits _stopBits = StopBits.One;

//IMPORTANT: This is the COM port that will be used by the arm.

//It is hard-coded here. There should be a config file associated

//with this service, but this has not been implemented yet. private string _portName = “COM1”;

private int _timeout = 1000;

667

www.it-ebooks.info

Part IV: Robotics Hardware

Note that there is a timeout value of one second. Don’t touch the other serial port settings because they are set in the firmware on the SSC-32 controller board.

Using the Generic Arm Contract

The Lynx6Arm service implements the MRDS generic Articulated Arm contract. This contract is used by the examples to communicate with the arm. It is also implemented in the simulator, so the same code works on both the simulator and the real arm in most cases.

The generic Articulated Arm contract included with MRDS has only a small number of requests. You can find the code under MRDS in samples\Common\ArticulatedArmState.cs and

ArticulatedArmTypes.cs.

The following requests are available for the generic contract in addition to the usual requests such as

Get, Drop, and Subscribe:

SetJointTargetPose: Sets the pose for a single joint

SetJointTargetVelocity: Sets the velocity for a single joint

GetEndEffectorPose: Gets the pose of the end effector (using forward kinematics)

SetEndEffectorPose: Sets the pose of the end effector (changes all joints as necessary using inverse kinematics)

The standard set of requests might not be suitable for some applications. For example, there are no explicit requests to control the opening/closing of the gripper, although it can just be considered another joint. Humanoid robots might not have grippers, but a gripper is a fundamental feature of an arm — without a gripper an arm would be useless!

Setting Joint Angles

For basic operations, these requests are sufficient. In fact, you can control the arm using only SetJointTargetPose if you know all of the relevant joint positions for the trajectory of the arm. If the arm will be performing some repetitive task, you can just record the joint positions using the Motion Recorder service developed for this chapter and then replay them.

The Lynx6Arm service treats the gripper as the last joint in the set, but limits the angle to 35 degrees because the gripper servo is a very lightweight servo, and it could become overloaded if the gripper mechanism were driven too far. In addition, be very careful when closing the gripper on an object so that you do not try to drive it too far.

There are no timing parameters for SetJointTargetPose so you are restricted to using whatever value is implemented in the service itself. SetJointTargetVelocity can be used to specify the maximum speed for joint motions. Its name might suggest that it starts a joint moving at a specified speed, but this is not the case.

The firmware on the SSC-32 has been designed so that a sequence of joint motion commands can be sent in quick succession and it will execute all of them. However, this is not an ideal approach. Unless the

668

www.it-ebooks.info

Chapter 15: Using a Robotic Arm

Time parameter is the same for all of the joints in a sequence, they will move to their destinations at different speeds. This is equivalent to Asynchronous PTP in KUKA terminology.

Using GetEndEffectorPose

You can use GetEndEffectorPose to obtain the 3D coordinates of the EOA and the tilt angle of the wrist. Note that the wrist tilt angle is not the same as the angle of the wrist servo angle. The wrist tilt is the accumulation of all of the angles for all the joints and specifies how the wrist is tilted with respect to a horizontal line.

When you call GetEndEffectorPose, the Lynx6Arm service calls the SSC-32 service to query the arm. This means that the values returned should be current. This operation is done synchronously. In other words, the SSC-32 service sends a request to the SSC-32 and then waits for a reply. However, it does not guarantee that the arm is physically located at the corresponding location. If the arm’s motion is restricted for any reason, the servo settings might not be reached. The SSC-32 is only telling you how it is pulsing the servos, not whether the servos are at the desired positions, which it has no way to know.

The EOA is at the tip of the gripper, but to pick up an object you need to position the gripper so that the object is at the TCP. There is a difference of about an inch (a couple of centimeters). It is a simple matter to change the kinematics code to calculate the TCP rather than the EOA. Open Lynx6Arm.cs, locate the following code, and reduce the value of L3:

// Not required

//float Grip = Conversions.InchesToMeters(3.5f); // Length of gripper

//NOTE: The gripper is basically in two parts because it contains

//the wrist rotate joint which is NOT at the same place as the

//wrist joint.

//Measurements are:

//Wrist joint to rotate plate = 2.25”

//Rotate place to tip of fingers = 3.5”

//The actual length of the “hand” varies slightly depending on

//whether the gripper is open or closed.

//L3 measures the distance to the tip of the fingers or End Of Arm (EOA).

//A Tool Center Point (TCP) would be shorter than this.

//The rubber fingers are just over 1” long, so to keep the grip action

//happening on the fingers, then L3 needs to be shorter by about 0.5”.

If you have an older Lynx 6 arm, you will have a yellow base instead of a black one. This base is a different height so you also need to modify the value of H. Note that the base height is measured up to the center of the shoulder servo.

The SetEndEffectorPose request performs the inverse kinematics calculations. You specify both the position and orientation (wrist tilt) of the end effector.

669

www.it-ebooks.info

Part IV: Robotics Hardware

The SSC-32 has a command that sets multiple joints in motion at the same time but with an overall Time parameter, and SetEndEffectorPose uses this command. (Speeds can still be specified individually, and the motion will take the longer of the times specified or the time based on the speed.) This is basically the equivalent of the KUKA Synchronous PTP mode of operation.

One consequence of this is that motions specified using SetEndEffectorPose are usually much smoother than setting all of the joint angles (via multiple calls to SetJointTargetPose).

Using the Dashboard to Control the Arm

By now you should be familiar with the Dashboard. It includes a panel for controlling an arm and it will work with the Lynx6Arm service.

The generic arm contract requests do not allow for setting multiple joints simultaneously. KUKA created a new generic contract with a request called SetMultipleJointAngles for just this purpose. However, the KUKA implementation is not backwardly compatible with the existing generic arm contract, so standard programs such as the Dashboard do not work with the KUKA simulator.

To try using the Dashboard, run the following command from a MRDS Command Prompt window (remembering that it has to be on one line):

dsshost -port:50000 -tcpport:50001 -manifest:”ProMRDS/Config/Lynxmotion.Lynx6Arm.manifest.xml” -manifest:”ProMRDS/Config/Dashboard.manifest.xml”

For your convenience, a batch command file called LynxL6Arm.cmd is also provided.

When the Dashboard starts up, connect to localhost. You should see the lynx6arm in the services list. Make sure that you have enabled the articulated arm display under Tools Options and connect to the arm. This will enumerate the joints and list them for you. To make it easier to understand, the joint names include the joint number. This is the servo number on the SSC-32. The display should look like Figure 15-6.

Figure 15-6

670

www.it-ebooks.info

Chapter 15: Using a Robotic Arm

You can select a joint by clicking it. Enter a value for the angle and then click the Set Angle button. The joint will move to the specified angle. When you are first experimenting, use fairly small angles until you know how the arm will behave. You can use both positive and negative angles. If you click the Get Pose button, the current X, Y, Z coordinates of the EOA will display.

Clearly, moving the arm around in this fashion is tedious. The example is simply included here to illustrate that the generic arm contract works. An alternative way to control the arm is using one of the VPL programs in Chapter 12 and an Xbox game controller. The two VPL programs for robotic arms in Chapter 12 use the thumbsticks and buttons in different ways. This is deliberate, and highlights the difference between adjusting servo angles and specifying the position of the end effector. Some people will find one way more intuitive than the other, but in some situations both approaches are useful. Playing with an Xbox controller is a very good way to get a practical understanding of the different approaches to moving an arm.

Recording Motions

The next step beyond the Dashboard is to use a more sophisticated application to control the arm. This chapter includes a service called the Motion Recorder. The service can be used with either the simulated arm or a real arm, so even if you don’t have a Lynxmotion L6 arm you can still experiment with the simulator. This section explains how the Motion Recorder works and how to use it.

In the MRDS bin folder you will find a batch file called MotionRecorder.cmd. Run this batch file to start the Motion Recorder.

The Motion Recorder service enables you to move the arm around either by moving joints individually or by specifying a pose. You can save the arm positions to a file and then later replay the sequence of commands. The service GUI looks like Figure 15-7.

Figure 15-7

671

www.it-ebooks.info

Part IV: Robotics Hardware

The purpose of the Motion Recorder is to help you develop applications. It could be improved in a lot of ways, but it is only intended as a simple tool.

Using the Simulated Lynx Arm

You will notice some similarities between the Motion Recorder and the GUI for the Lynx arm simulation in Chapter 8. In fact, it can be used with the simulator if you want. To use the simulator, edit the manifest in the ProMRDS\Config directory called MotionRecorder.manifest.xml. At the bottom of the manifest you will see that the simulated arm is commented out (this is highlighted in the following code). Remove the commenting and instead comment out the real arm service, which is just above the simulated one.

<?xml version=”1.0” ?> <Manifest

xmlns=”http://schemas.microsoft.com/xw/2004/10/manifest.html”

xmlns:dssp=”http://schemas.microsoft.com/xw/2004/10/dssp.html”

>

<CreateServiceList> <ServiceRecordType>

<dssp:Contract>http://schemas.tempuri.org/2007/11/ motionrecorder.html</dssp:Contract>

</ServiceRecordType> <!-- Use the real arm -->

<ServiceRecordType> <dssp:Contract>http://schemas.coroware.com/2007/11/

lynx6arm.html</dssp:Contract> </ServiceRecordType>

<!-- Use the simulated arm <ServiceRecordType>

<dssp:Contract>http://schemas.tempuri.org/2007/10/

SimulatedLynxL6Arm.html</dssp:Contract> </ServiceRecordType>

-->

</CreateServiceList> </Manifest>

Now when you run the Motion Recorder, the simulator will start. You can play around as much as you like without potentially damaging your Lynx 6 arm.

Inside the Motion Recorder Service

The following is a brief description of how the Motion Recorder works. The user interface should be fairly intuitive. Three main files make up the application:

MotionRecorder.cs: The main service where most of the action happens

MotionRecorderTypes.cs: State, operations port, classes for communication, etc.

MotionRecorderUI.cs: Windows Form for the user interface

The MotionRecorderTypes.cs file should not require any explanation, as it is quite short.

672