[SOLVED] math assembly Robot Kinematics Scenario

Robot Kinematics Scenario
You are a Mechatronic Engineer designing a 2link SCARA selective compliance assembly robot arm type robotic arm. The robots end effector the tool attached to the distal end of the second link needs to be controlled to move in a horizontal plane through twodimensional 2D 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 robots 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 robots end effector are x2, y2 and the coordinates of the joint between link 1 and link 2 are x1, y1 .

The robot arm is fixed to a square work table link 0 that has sides of length D. The origin of the robot arm x, y0, 0 is at the centre of the work table. Since the movement of a SCARAtype 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 robots 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.
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 effector Cartesian position x2 and y2; and
2. Inverse kinematics: Given the end effector position x2 and y2, calculate the joint angles 1 and 2.
The forward kinematic equations are straightforward:
x2L2cos12L1cos1 y2L2 sin12L1 sin1
However, you want to be able to control the end effector position directly using the inverse kinematic equations:
2cos12L1L2x22y22L12L22
1tan1x2y2tan1L1L2cos2L2sin2
Solution of the inverse kinematics of a robot manipulator is usually more difficult, and more computationally expensive, than solving the forward kinematic equations. Solving the inverse kinematic equations can take excessively long for the realtime control of manipulators.
Please note the following
1. The inverse kinematic equations will have two different solutions arm configurations over almost all of the space that is reachable by the end effector.
2. If an inverse trigonometric function has a solutionthen 2n, where n is an integer, are also solutions.

3. The lengths L1 and L2 of the two links will usually be different. This will limit where the robot arm is able to reach.
4. Please look at this animation.
Specification
You are required to write a program that calculates and displays solutions to the forward and inverse kinematic equations of a twolink robot subject to constraints, for usersupplied input data. The program must comply with the following specification.
0. In this specification 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 flags on the command line. The form of program invocation is shown below. The square bracketsindicate optional flags. Thewithin indicates mutually exclusive.
.robot s1 s2 L1 L2 D S fi dr
2. The first 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 floating point number of type double , and representing either
x2 the starting x coordinate of the end effector 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 floating point number of type double , and representing either
y2 the y coordinate of the end effector 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 floating point number of type double .
L2 is the constant length of link 2 in metres, interpreted as a floating point number of type double .

D is the length of the sides of the work table in metres, interpreted as a floating point number of type double .
S is the maximum angular speed that either joint servo motor can attain, interpreted as a floating point number of type double .
S shall be positive.
If the unit of angular measure is radians, S shall be interpreted as a speed in radianssecond.
If the unit of angular measure is degrees, S shall be interpreted as a speed in degreessecond.
fi This flag determines whether forward or inverse kinematics are calculated:
f : the program shall calculate the coordinates x2, y2 of the end effector 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 effector.
Zero or one of the flags f and i shall be set.
The program shall default to calculating inverse kinematics if neither f nor i is set.

rd This flag specifies 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 flags r and d shall be set.
The angular inputs and outputs of the program shall default to radians, and angular velocities to radianssec, if neither r nor d is set.
4. Program arguments are casesensitive; 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 characters, many different 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. x1instead of numbers in any of the first six fields.
3. Entering both d and r flags.

4. Entering both f and i flags.
5. Entering invalid characters in the flags fields.
6. Invalid user input shall also include
1. Points that the arm cannot reach; and
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 programs 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 define the path between the starting arm configuration and the goal configuration.
9. Inputs shall be spaceseparated, one goal point per input line with the coordinates from the command line providing the start point for the first goal coordinates read from stdin .

Text
1
2
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 sufficient for the arm to travel from the current position to the next position without the angular speed of either joint exceeding
S . The final 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 highspeed 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 onedimensional; 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 effector 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.
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 effector 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 spacedelimited format, with the first 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 fifth 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 doesnt 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
Inverse Kinematics

Execution
1. Your submission must include a Makefile . The Makefile can be the one included in the project scaffold, or a modified version of this file.
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 inputoutput functions: fgets , scanf , fprintf
Header stdlib.h has text to numeric functions strtod and more.
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 bugcatching 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 students 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 specification will tested by Eds automated testing process. Compliance with the specification 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
Selfdocumenting 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.