编写程序控制机器人的末端电子装置

9/5/2019 MTRX1702 – Assessments

Robot Kinematics Scenario

You are a Mechatronic Engineer designing a 2-link SCARA (selective compliance assembly robot arm) type robotic arm. The robot’s end eector (the tool attached to the distal end of the second link) needs to be controlled to move in a horizontal plane through two-dimensional (2-D) space. You are tasked with writing a program that can generate the data necessary for the robot arm to follow paths between several points.

A diagram of the robot’s two links is shown below. Link 1 (orange) is of length L1 and link 2 (green) is of length L2. Rotation of each link through a joint angle θ1 or θ2 relative to the previous link is driven by a servo motor located at the appropriate joint. The coordinates of the robot’s end eector are (x2, y2) and the coordinates of the joint between link 1 and link 2 are (x1, y1) .

The robot arm is xed to a square work table (link 0) that has sides of length D. The origin of the robot arm (x, y) = (0, 0) is at the centre of the work table. Since the movement of a SCARA-type arm is very fast the entire work table is surrounded by a safety barrier, a vertical wall constructed from sheets of clear polycarbonate. The wall prevents anyone from reaching inside the robot’s work space when it is operating. The diagram shows how the robot appears when viewed from above; the safety barrier is represented by the thick black lines.

https://edstem.org/courses/3530/assessments/5898

1/8

9/5/2019 MTRX1702 – Assessments

Robot Arm Kinematic Equations

Kinematics is the branch of mechanics that is concerned with the motion of objects without regard to the forces which cause the motion. There are two ways of writing the kinematic equations that describe your robot:

1. Forward kinematics: Given the joint angles θ1 and θ2, calculate the end eector Cartesian position x2 and y2; and

2. Inverse kinematics: Given the end eector position x2 and y2, calculate the joint angles θ1 and θ2.

The forward kinematic equations are straightforward:

x2 = L2 ⋅ cos(θ1 +θ2)+L1 ⋅cos(θ1) y2 = L2 ⋅sin(θ1 +θ2)+L1 ⋅sin(θ1)

However, you want to be able to control the end eector position directly using the inverse kinematic equations:

θ2 = cos−1(x2 + y2 − L21 − L2) 2⋅L1 ⋅L2

θ1 = tan−1(y2) − tan−1( L2 ⋅ sin(θ2) ) x2 L1 + L2 ⋅cos(θ2)

Solution of the inverse kinematics of a robot manipulator is usually more dicult, and more computationally expensive, than solving the forward kinematic equations. Solving the inverse kinematic equations can take excessively long for the real-time control of manipulators.

Please note the following

1. The inverse kinematic equations will have two dierent solutions (arm congurations) over almost all of the space that is reachable by the end eector.

2. If an inverse trigonometric function has a solution θ then θ ± 2nπ, where n is an integer, are also solutions.

3. The lengths L1 and L2 of the two links will usually be dierent. This will limit where the robot arm is able to reach.

4. Please look at this animation.

Specication

You are required to write a program that calculates and displays solutions to the forward and

inverse kinematic equations of a two-link robot subject to constraints, for user-supplied input data.

https://edstem.org/courses/3530/assessments/5898 2/8

9/5/2019 MTRX1702 – Assessments

The program must comply with the following specication.

0. In this specication the word “shall” indicates a mandatory requirement and the word “should” indicates a feature that is desirable but not mandatory.

Command line arguments

1. The program shall accept six mandatory inputs as ordered command line arguments, plus a number of optional inputs, as ags on the command line. The form of program invocation is shown below. The square brackets [] indicate optional ags. The | within indicates mutually exclusive.

./robot <s1> <s2> <L1> <L2> <D> <S> [-f|-i] [-d|-r]

2. The rst six arguments are necessary for the program to run; they shall be in the order shown. The remaining two arguments are optional and can be in either order.

3. The command line arguments shall have the following meanings

<s1> is one start coordinate, interpreted as a oating point number of type double , and representing either

<x2> the starting x coordinate of the end eector in metres, if calculating inverse kinematics, or

<theta1> the starting angle of link 1 relative to the work table, if calculating forward kinematics.

<s2> is the second start coordinate, interpreted as a oating point number of type double , and representing either

<y2> the x coordinate of the end eector in metres, if calculating inverse kinematics, or

<theta2> the angle of link 2 relative to link 1, if calculating forward kinematics.

<L1> is the (constant) length of link 1 in metres, interpreted as a oating point number of type

double .

<L2> is the (constant) length of link 2 in metres, interpreted as a oating point number of type

double .

<D> is the length of the sides of the work table in metres, interpreted as a oating point

number of type double .

<S> is the maximum angular speed that either joint servo motor can attain, interpreted as a

oating point number of type double . <S> shall be positive.

https://edstem.org/courses/3530/assessments/5898 3/8

9/5/2019

MTRX1702 – Assessments

If the unit of angular measure is radians, <S> shall be interpreted as a speed in radians/second.

If the unit of angular measure is degrees, <S> shall be interpreted as a speed in degrees/second.

[-f|-i] This ag determines whether forward or inverse kinematics are calculated:

-f : the program shall calculate the coordinates (x2, y2) of the end eector from the joint

coordinates (q1, q2), or

-i : the program shall calculate the joint coordinates (q1, q2) from the coordinates (x2,

y2) of the end eector.

Zero or one of the ags -f and -i shall be set.

The program shall default to calculating inverse kinematics if neither -f nor -i is set.

[-r|-d] This ag species the unit of measure for angular quantities.

-r : input angles shall be interpreted as radians, and all output angles shall be printed

as radians.

-d : input angles shall be interpreted as degrees, and all output angles shall be printed as degrees.

Zero or one of of the ags -r and -d shall be set.

The angular inputs and outputs of the program shall default to radians, and angular

velocities to radians/sec, if neither -r nor -d is set.

4. Program arguments are case-sensitive; that is -F is not the same as -f .

5. The program shall validate user input, and recover gracefully from illegal input. Since a user might accidentally enter any incorrect character(s), many dierent errors are possible including but not limited to:

1. Failing to enter exactly six numerical values after ./robot

2. Entering characters or symbols (e.g. x1 ) instead of numbers in any of the rst six elds. 3. Entering both -d and -r ags.

4. Entering both -f and -i ags.

5. Entering invalid characters in the ags elds.

6. Invalid user input shall also include

1. Points that the arm cannot reach; and

https://edstem.org/courses/3530/assessments/5898 4/8

9/5/2019 MTRX1702 – Assessments

2. Points that would cause any part of the arm to breach the walls around the table. 7. On detection of illegal input, the following error message shall be displayed to stderr :

Error: Illegal input!

Program Input

8. The pair of starting coordinates [(x2, y2) or (θ1, θ2) depending on the program’s mode of operation] read from stdin shall be used to calculate a sequence of intermediate points [(x1, y1) and (x2, y2) or angles θ1 and θ2, again depending on the mode of operation] that dene the path between the starting arm conguration and the goal conguration.

9. Inputs shall be space-separated, one goal point per input line with the coordinates from the command line providing the start point for the rst goal coordinates read from stdin .

10. The path between the current start point and the next goal point shall be divided into a number of 0.10 second time steps, with the number of steps being sucient for the arm to travel from the current position to the next position without the angular speed of either joint exceeding <S> . The nal point in a sequence shall be the goal point before continuing to any new goal point coordinates read from stdin .

11. The sequence of point ends when an End Of File signal is received or an empty line is input. Simplifying assumption

12. Since a SCARA arm is optimised for high-speed positioning, acceleration and deceleration of the joint motors is very high. You may therefore assume that

1. The dynamics of the arm are negligible and, as a consequence,

2. Both joints travel at a constant angular speed no greater than <S> in the 0.1 second intervals between successive intermediate points

3. The two arm links can be assumed to be one-dimensional; that is, straight lines.

Path constraints

13. The paths generated by the program shall satisfy a number of constraints:

1. All points (x, y) along the end eector path shall be reachable by the robot arm.

2. The robot arm shall not collide with itself. That is, the magnitude of the angle θ2 must not exceed ±175 degrees so that link 2 does not collide with link 1.

Text

1 2

45 45 00

https://edstem.org/courses/3530/assessments/5898 5/8

9/5/2019 MTRX1702 – Assessments

3. No part of the robot arm shall collide with its environment. That is, the positions (x1, y1) of the ‘knuckle’ and (x2, y2) of the end eector must not lie beyond the edges of the work table.

4. The movement between points shall not exceed the provided speed limit. Program Output

14. The results shall be displayed in a space-delimited format, with the rst column containing the time in seconds. Subsequent columns depend on the chosen mode. If the mode is

1. Forward kinematics: The second and third columns shall contain x1 and y1 whilst the fourth and fth column shall contain x2 and y2. Output values shall be in metres.

2. Inverse kinematics: The second and third columns shall contain θ1 and θ2. The angles shall be in radians or degrees to match the command line arguments.

15. The values in each column shall be displayed to 4 decimal places.

16. The elapsed time shall be continuous over the complete path. That is, time doesn’t restart with each successive goal coordinate.

17. Output may be displayed after each coordinate line is entered or may be displayed once all coordinate lines have been entered.

Examples of Program Invocation Forward kinematics

$./robot 0 0 3 2 10 100 -d -f

45 45

0.0000

0.1000

0.2000

0.3000

0.4000

0.5000

0.6000

0.7000

0.8000

0.9000

1.0000

00

1.1000

1.2000

1.3000

1.4000

1.5000

1.6000

1.7000

1 8000

3.0000 0.0000 5.0000

2.9544 0.5209 4.8338

2.8191 1.0261 4.3512

2.5981 1.5000 3.5981

2.2981 1.9284 2.6454

2.0268 2.2118 1.8525

2.1671 2.0745 2.2543

2.0981 2.1443 2.0544

2.1329 2.1097 2.1547

2.1155 2.1271 2.1046

2.1213 2.1213 2.1213

2.4575 1.7207 3.1415

2.7189 1.2679 4.0045

2.8978 0.7765 4.6298

2.9886 0.2615 4.9582

2.9971 -0.1309 4.9895

2.9993 0.0654 4.9974

2.9998 -0.0327 4.9993

3 0000 0 0164 4 9998

0.0000

1.2050

2.3116

3.2321

3.8980

4.2042

4.0726

4.1439

4.1096

4.1271

4.1213

3.6001

2.7999

1.7765

0.6088

-0.3052

0.1527

-0.0764

0 0382

6/8

https://edstem.org/courses/3530/assessments/5898

9/5/2019

1.8000 3.0000

1.9000 3.0000

2.0000 3.0000

Inverse Kinematics

0.0164

-0.0082

0.0000

MTRX1702 – Assessments

4.9998 0.0382

5.0000 -0.0191

5.0000 0.0000

$./robot 3 4 3 2 10 100 -d -i 23

0.0000 53.1301

0.1000 43.1301

0.2000 33.1301

0.3000 23.1301

0.4000 22.3647

0.5000 22.7474

0.6000 22.5561

0.7000 22.6518

0.8000 22.6039

0.9000 22.6199

1 2.5

1.0000 29.9540

1.1000 26.2869

1.2000 28.1205

1.3000 27.2037

1.4000 27.6621

1.5000 27.4329

1.6000 27.5093

0.0000

10.0000

20.0000

30.0000

40.0000

50.0000

60.0000

70.0000

80.0000

90.0000

100.0000

110.0000

120.0000

117.9465

118.9732

118.4599

118.6310

Execution

1. Your submission must include a Makefile . The Makefile can be the one included in the project scaold, or a modied version of this le.

2. Your code will be compiled by running the command make in the root directory of your submission.

3. The executable that is created must be called robot . It must be created in the root directory of your submission.

Libraries

You may use any functions in the C11 Standard Library. You may not use any external sources of code, regardless of licensing.

The following libraries are likely to be most useful:

Header <stdio.h> has input/output functions: fgets() , scanf() , fprintf()

Header <stdlib.h> has text to numeric functions strtod() and more. https://edstem.org/courses/3530/assessments/5898 7/8

9/5/2019 MTRX1702 – Assessments

Header <math.h> has mathematical functions: sin() , cos() , tan() and their inverses Header <ctype.h> has character functions: isdigit() , isalpha() , isspace() , tolower() Header <assert.h> has the bug-catching macro assert()

Academic Honesty

This assignment is to represent individual work. You are not to collaborate with your peers, this is an individual assignment. You may discuss your approach to the problem with other students, but you may not share code, look at another student’s code, or allow another student to look at your code. Similar code will be treated with suspicion. All submissions will be checked for similarity.

Marking

Compliance with the specication will tested by Ed’s automated testing process. Compliance with the specication is worth 75% of this assignment.

The remaining 25% of the assignment is awarded for quality of your code. Things that will be assessed include, but are not limited to:

Appropriate and consistent formatting and style

Project layout

Good coding practices (e.g. no global variables or “magic numbers”) Self-documenting code, including appropriate commenting Appropriate use of functions

Late Submission

Late submissions will be penalised at a rate of 5% of the value of the assignment per day or part thereof. Submissions that are more than 10 days late will not be accepted. The last submission is the one that will be assessed, regardless of whether an earlier one has a greater value in test cases solved.

https://edstem.org/courses/3530/assessments/5898 8/8