Manipulator Kinematics 6n4l42
This document was ed by and they confirmed that they have the permission to share it. If you are author or own the copyright of this book, please report to us by using this report form. Report 3b7i
Overview 3e4r5l
& View Manipulator Kinematics as PDF for free.
More details w3441
- Words: 9,116
- Pages: 24
UFMF5P-20-3: R OBOTS M ECHANICS
Coursework 1: Manipulator Kinematics
Queron Williams - 10031755 November 14, 2013
1. I NTRODUCTION This document outlines the methods used to derive both the forward kinematics and inverse kinematics for a 5 Degrees Of Freedom Lynxmotion robot arm. From this we will then create a Matlab model of the arm. The model will allow us to perform tasks such as plotting the workspace and planning movement sequences. Finally results from the model will be tested on the robot.
2. F ORWARD K INEMATICS Forward kinematics enable us to work out the final position of the end effector relative to the base link of the robot. To do this we must first know the relation between each link of the robot arm.
2.1. KINEMATIC STRUCTURE OF LYNXMOTION 5DOF A RM The Lynxmotion arm has 5 Degrees Of Freedom. The configuration of the ts in the arm are shown below. The robot consists of 6 links labelled G 0 − G 6 . These Links are connected through ts j 1 − j 5 . For each pair of successive ts there is a common perpendicular distance l 1 − l 5 .
Figure 2.1: t and Link structure of Lynxmotion robot arm
10031755
Queron Williams
1
Each of these t then has axes added. The z-axis is chosen to lie along the t axis of each t. The x-axis is perpendicular to the z-axis of the current t and the z-axis of the next t. The y-axis is chosen to complete the right hand coordinate frame for each t.
Figure 2.2: Axis structure of Lynxmotion robot arm Some of the links (l 5 ) in the diagram actually have a link length of zero on the real robot however they have been show for clarity.
2.2. D ENAVIT-H ARTENBERG PARAMETERS Denavit-Hartenberg (DH) parameters are used to describe the translation between each set of axis in the diagram. there are two types of DH notation, these are called Proximal and Distal. For this example we will be using the Proximal method. The derived DH parameters for the Lynxmotion arm are as follows: Link 1 2 3 4 5 E
a i −1 0 0 l2 l3 0 0
αi −1 0 90 0 0 -90 0
θi θ1 θ2 θ3 θ4 θ5 0
di l1 0 0 0 l4 l5
2.3. T RANSFORMATION M ATRICES Transformation matrices are used to describe the transformation between 2 coordinate frames in a robot. They are made up of both a rotation and translation matrix. R
T =
0
0
T 0 1
(2.1)
There is a transformation matrix that describes any possible transformation when using the proximal method. cosθi −si nθi 0 a i −1 si nθ cosα i i −1 cosθi cosαi −1 −si nαi −1 −si nαi −1 d i i −1 Ti = (2.2) cosαi −1 d i si nθi si nαi −1 cosθi si nαi −1 cosαi −1 0 0 0 1
10031755
Queron Williams
2
This can be used to create a transformation matrix for each t or the robot arm. This is accomplished by substituting values from each line of the DH table into the above matrix. The transformation matrix for each separate t is shown below. cosθ1 −si nθ1 0 0 si nθ cosθ1 0 0 1 0 (2.3) T1 = 0 0 1 l1 0 0 0 1 1
T2 =
2
T3 =
3
T4 =
4
T5 =
cosθ2 0 si nθ2 0
−si nθ2 0 cosθ2 0
0 −1 0 0
0 0 0 1
cosθ3 si nθ3 0 0
−si nθ3 cosθ3 0 0
0 l2 0 0 1 0 0 1
cosθ4 si nθ4 0 0
−si nθ4 cosθ4 0 0
0 l3 0 0 1 0 0 1
cosθ5 0 −si nθ5 0
5
TE =
1 0 0 0
−si nθ5 0 −cosθ5 0
0 0 1 l4 0 0 0 1
0 1 0 0
0 0 0 0 1 l5 0 1
(2.4)
(2.5)
(2.6)
(2.7)
(2.8)
Once we have found each links transformation matrix we can multiply them together to find a complete transformation matrix from the base to the end effector. 0
0
TE =
c 234 ∗ c 1 ∗ c 5 − s 1 ∗ s 5 c 1 ∗ s 5 + c 234 ∗ c 5 ∗ s 1 s 234 ∗ c 5 0
TE =0 T11 T22 T33 T44 T55 TE
(2.9)
−s 234 ∗ c 1 −s 234 ∗ s 1 c 234 0
c 1 ∗ (L3 ∗ c 23 + L2 ∗ c 2 − L4 ∗ s 234 − L5 ∗ s 234 ) s 1 ∗ (L3 ∗ c 23 + L2 ∗ c 2 − L4 ∗ s 234 − L5 ∗ s 234 ) L1 + L3 ∗ s 23 + L2 ∗ s 2 + L4 ∗ c 234 + L5 ∗ c 234 1 (2.10)
Queron Williams
3
−c 5 ∗ s 1 − c 234 ∗ c 1 ∗ s 5 c 1 ∗ c 5 − c 234 ∗ s 1 ∗ s 5 −s 234 ∗ s 5 0
Where: c i = cosθi s i = si nθi c 23 = cos(θ2 + θ3 ) s 23 = si n(θ2 + θ3 ) c 234 = cos(θ2 + θ3 + θ4 ) s 234 = si n(θ2 + θ3 + θ4 )
10031755
2.4. M ODELLING IN MATLAB To model the robot arm in Matlab we are using the previously calculated transformation matrix to find the position of each t then drawing a lime between each t. For all simulations I will be using link lengths measured from the actual robot. These link lengths are: L1 = 6.9cm, L2 = 9.4cm, L3 = 10.8cm, L4 = 8.5cm, L5 = 0cm.
Figure 2.3: MATLAB model of robot arm with all ts set to 0 degrees
2.5. T ESTING F ORWARD K INEMATICS To test that this model works for many different sets of given theta values I have repeated this with the robot in 3 different Cartesian positions as shown below.
Figure 2.4: MATLAB model angles: θ1 = 0, θ2 = 70, θ3 = −20, θ4 = −90, θ5 = 0,
10031755
Queron Williams
4
Figure 2.5: MATLAB model angles: θ1 = −45, θ2 = 50, θ3 = −40, θ4 = −110, θ5 = 0,
Figure 2.6: MATLAB model with angles: θ1 = −90, θ2 = 30, θ3 = −60, θ4 = −130, θ5 = 0,
3. W ORKSPACE A NALYSIS A robots workspace describes all possible positions that the robot can reach with its end effector. In order to accurately plot the workspace the range of possible angles for each t were measured from the robot. These were as follows : θ1 = -90 to 90 , θ2 = 0 to 135 , θ3 = 0 to -135 , θ4 = 0 to -180. θ5 has not been changed as its angle does not affect the final position of the end effector.
3.1. P LOTTING THE W ORKSPACE The Forward kinematic model we have created for the Lynxmotion robot arm can be used to easily plot its workspace. This is done by iterating each t through its range of motion. If we do this for all ts we will get every possible configuration the arm can be in. For each set of t angles we use the forward kinematics to calculate the
10031755
Queron Williams
5
Cartesian position of the end effector. We can then graph the output of the Cartesian points to show which areas can be reached by the robot.
3.2. R ESULTS Whilst plotting the workspace I have varied the colours of the points based on the angle of each of the ts θ2 −θ4 . The red channel represents the value of θ2 , green shows θ3 and blue shows θ4 . This helps give some indication of the angles requited to achieve any given target position.
Figure 3.1: Cross-section of workspace across the XZ plane The red towards the bottom of the workspace shows that θ2 (shoulder) must be further forward for the arm to reach below Zero targets. The green shows that θ3 (elbow) must be outstretched to reach the outer edges of the workspace and the area above/behind. The blue shows that θ4 (wrist) must be bent downwards in order to reach areas closer to the center of the workspace. In reality the robots workspace would be slightly more restricted than the plot shows due to physical constraints of the robot. The Lynxmotion arm would not be able to reach below Z=0 as it is mounted to a flat surface.
10031755
Queron Williams
6
Figure 3.2: Side view of the workspace
Figure 3.3: Top view of the workspace
Figure 3.4: Back view of the workspace
Figure 3.5: Isometric view of the workspace
4. I NVERSE K INEMATICS Often in robotics tasks it is useful to be able to give a robot arm a Cartesian target position and calculate the θ values required to reach the target. In order to achieve this we must be able to solve the Inverse Kinematics for the robot.
4.1. D ERIVING THE I NVERSE K INEMATICS The easiest way to solve inverse kinematics is using the geometric method. This involves breaking the problem down into multiple 2D problems and solving each separately. For the Lynxmotion arm we will seperate the soloution into two seperate problems. The first problem will be to solve θ1 as viewed from above, only considering the XY plane. The second problem is to solve θ2 , θ3 and θ4 . This is done by considering the arm as a 2D problem in a new XZ plane, where X is the hypotenuse formed by the target position in the previous XY plane. To solve the IK we will be given a target x, y, z, pitch and roll for the end effector.
10031755
Queron Williams
7
4.1.1. F INDING θ1
Y 5Y 5 4Y
4 3 θ1 4X
1/2
5X
X
Figure 4.1: Robot arm as viewed in XY plane θ1 can be easily solved using the trigonometric function atan 2 as shown below.
θ1 = at an2(5 y , 5x )
(4.1)
4.1.2. F INDING θ3
Z θ2 5
4
4Z
θ3 3 3Z 2
θ2 3X
4X
X
Figure 4.2: Robot arm as viewed in XZ plane To find θ2 and θ3 we need to first know the position of t 4 on the XZ plane. This means that we need to calculate
10031755
Queron Williams
8
t 4’s position (X 4 and Z4 ). We can do this as we know the target position, link 4’s length and the target pitch ( we will call ϕ)
4x = 5x − l 4 c ϕ
(4.2)
4z = 5z − l 4 s ϕ
(4.3)
To find the X value we need only sum the lengths of the adjacent sides of the two right-angled triangles formed by l 2 &θ2 and l 3 &θ3 . For Z it is the same but using the opposite sides.
4x = l 3 c 23 + l 2 c 2
(4.4)
4z = l 3 s 23 + l 2 s 2
(4.5)
Where c i = cosθi , s i = si nθi , c 23 = cos(θ2 + θ3 ), s 23 = si n(θ2 + θ3 ). The soloution to θ3 can be computed from summation of squaring both equations 4.4 and 4.5
42x
+ 42z
2 42x = l 32 c 23 + l 22 c 22 + 2l 3 l 2 c 23 c 2
(4.6)
2 42z = l 32 s 23 + l 22 s 22 + 2l 3 l 2 s 23 s 2
(4.7)
2 2 = l 32 (c 23 + s 23 ) + l 22 (c 22 + s 22 ) + 2l 3 l 2 (c 23 c 2 + s 23 s 2 )
(4.8)
Because c 2 θi + s 2 θi = 1, equation 4.8 can be rearanged and simplified as follows:
42x
+ 42z
42x + 42z = l 32 + l 22 + 2l 3 l 2 (c 23 c 2 + s 23 s 2 )
(4.9)
42x + 42z = l 32 + l 22 + 2l 3 l 2 (c(θ2 + θ3 )c 2 + s(θ2 + θ3 )s 2 )
(4.10)
= l 32 + l 22 + 2l 3 l 2 (1/2(c(2θ2 + θ3 ) + c 3 ) + 1/2(c 3 − c(2θ2 + θ3 ))) 42x + 42z = l 32 + l 22 + 2l 3 l 2 c 3
(4.11) (4.12)
Rearranged for c(θ3 ):
cθ3 =
−l 22 − l 32 + 42x + 42z 2l 2 l 3
(4.13)
The acos() function can cause inaccuracies at small values of θ, to get around this problem we will use atan2() this requires sθ3 and cθ3 but sine we know c 2 θi + s 2 θi = 1 we can simply rearrange to get sθ3 .
v à ! u 2 2 2 2 2 u −l − l + 4 + 4 x z 2 3 sθ3 = ±t1 − 2l 2 l 3
(4.14)
The final atan2() solution can now be created, note that the ± in the formula will allow us to get both possible solutions for this t.
v à !2 u 2 2 2 2 2 2 2 2 u −l 2 − l 3 + 4x + 4z −l − l + 4x + 4z θ3 = At an2 ±t1 − , 2 3 2l 2 l 3 2l 2 l 3
10031755
Queron Williams
(4.15)
9
4.1.3. F INDING θ2 Now that we have θ3 , we can use equation 4.4 and 4.5 to calculate θ2 . To do this we multiply equation 4.4 by cθ2 and the equation 4.5 by sθ2
c 2 4x = l 3 c(θ2 + θ3 )c 2 + l 2 c 22
(4.16)
s 2 4z = l 3 s(θ2 + θ3 )s 2 + l 2 s 22
(4.17)
c 2 4x + s 2 4z = l 3 (c(θ2 + θ3 )c 2 + s(θ2 + θ3 )s 2 ) + l 2 (c 22 + s 22 )
(4.18)
c 2 4x + s 2 4z = l 3 (1/2(c(2θ2 + θ3 ) + c 3 ) + 1/2(c 3 − c(2θ2 + θ3 ))) + l 2 (c 22 + s 22 )
(4.19)
c 2 4x + s 2 4z = l 3 c 3 + l 2 (c 22 + s 22 )
(4.20)
c 2 4x + s 2 4z = l 3 c 3 + l 2
(4.21)
We repeat this process but this time multiply equation 4.4 by −sθ2 and the equation 4.5 by cθ2
−s 2 4x = −l 3 c(θ2 + θ3 )s 2 − l 2 c 2 s 2
(4.22)
c 2 4z = l 3 s(θ2 + θ3 )c 2 + l 2 s 2 c 2
(4.23)
−s 2 4x + c 2 4z = l 3 s(θ2 + θ3 )c 2 − l 3 c(θ2 + θ3 )s 2
(4.24)
−s 2 4x + c 2 4z = l 3 s 3
(4.25)
Now we multiply both sides of equation 4.21 by −s(θ2 ), and both sides of equation 4.25 by c(θ2 ): before adding the resultant equations:
c 2 42x + s 2 4z 4x = 4x (l 3 c 3 + l 2 )
(4.26)
−s 2 4x 4z + c 2 42z = l 3 s 3 4z
(4.27)
c 2 (42x + 42z ) = 4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
(4.28)
This is then rearranged for c(θ2 ):
cθ2 =
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
(4.29)
42x + 42z
We know c 2 θi + s 2 θi = 1 so we can rearrange to get s(θ2 ):
s
µ
sθ2 = ± 1 −
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
¶2
(4.30)
42x + 42z
This can be used to give us the final atan2 solution for θ2 , as before the ± in the formula will allow us to get both possible solutions for this t.
à s
θ2 = At an2 ± 1 −
10031755
µ
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z 42x + 42z
Queron Williams
¶2
,
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z 42x + 42z
!
(4.31)
10
4.1.4. F INDING θ3 Now that we have θ2 and θ3 we can easily calculate θ4 using ϕ:
θ4 = ϕ − (θ2 + θ3 ) − 90
(4.32)
This will have to be repeated using the alternate θ2 and θ3 values to get both solutions for θ4 .
4.2. T ESTING THE I NVERSE K INEMATICS To test my inverse kinematics I have used the end effector target positions I acquired during my forward kinematic tests. I have then solved the inverse kinematics for these targets but specifying a pitch of 45 degrees with respect to ground. θ5 has been ignored as it does not affect the position of the end effector, only orientation. For the first set of theta angles (θ1 = 0, θ2 = 70, θ3 = −20, θ4 = −90) Matlab produces this target:
End efector position:
x= 15.62
y= 0.00
z= 30.52
And with a target pitch of 45 degrees it gives these solutions:
Solution 1: Solution 2: >>
1= 0.00 1= 0.00
2= 54.12 2= 68.63
3= 13.56 4= -112.68 3= -13.56 4= -100.06
I then plotted these 2 solutions to check them against the original angles and make sure the target position and pitch were right.
Figure 4.3: Test 1 input
Figure 4.4: Test 1 solution 1
Figure 4.5: Test 1 solution 2
This process was repeated for a second set of theta angles and a pitch of 45 degrees. Matlab produced:
input angles: 1= -22.50 2= 60.00 3= -30.00 4= -100.00 End efector position: x= 20.36 y= -8.43 z= 23.35 Solution 1: 1= -22.50 2= 12.93 3= 37.58 4= -95.51 Solution 2: 1= -22.50 2= 53.21 3= -37.58 4= -60.63 This was then plotted against the original.
Figure 4.6: Test 2 input
10031755
Figure 4.7: Test 2 solution 1
Queron Williams
Figure 4.8: Test 2 solution 2
11
The process was repeated once more for a final set of theta angles and a pitch of 0 degrees. Matlab produced:
input angles: 1= -45.00 2= 50.00 3= -40.00 4= -110.00 End efector position: x= 17.71 y= -17.71 z= 14.50 Solution 1: 1= -45.00 2= 2.95 3= 51.41 4= -144.37 Solution 2: 1= -45.00 2= 52.29 3= -51.41 4= -90.87 This was then plotted against the original.
Figure 4.9: Test 3 input
Figure 4.10: Test 3 solution 1
Figure 4.11: Test 3 solution 2
5. R EAL W ORLD A PPLICATION In order to test the model I will use it to perform a task with the Lynxmotion robot arm. The Lynxmotion robot arm uses a serial servo controller. I have written the following functions to make real time control of the arm easy: • updatet(); allows me to open the serial port and sends values directly to a servo. This function also deals with mapping required t angles to the correct pulse width for each t. this allows for the robot to be updated in real time as the program runs. • solveIK(); this function is provided with the target x/y/z/pitch/roll and peforms all of the IK calculations, returning the required θ vales for each t to reach the target. • UpdateArm(); this function gives a set of target positions to solveIK() then es each of the returned θ values out to the correct t udatet(). It then waits for the movement to be completed before returning and allowing the next command to run. The use of these functions allows for easy task planning with very little code. To create a task you need to specify the starting variables for x/y/z/pitch. Each move only requires one variable to be updated, followed by the UpdateArm() command. The example below shows the code to draw a 10cm*10cm square in the XZ plane:
x=10; y=0; z=10; pitch=0; gripper=0; %gripper is open time=2; %time for each move to take (2 seconds) UpdateArm(x,y,z,pitch,gripper,time); %move to start point x=20; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 1 z=20; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 2 x=10;
10031755
Queron Williams
12
UpdateArm(x,y,z,pitch,gripper,time); %move along edge 3 z=10; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 4 Whilst performing a task matlab produces a log file showing the target position and t angles at each time the arm is updated. This can be used for debugging or manually checking output from the IK.
x= x= x= x= x=
10.00 20.00 20.00 10.00 10.00
y= y= y= y= y=
0.00 0.00 0.00 0.00 0.00
z= z= z= z= z=
10.00 10.00 20.00 20.00 10.00
pitch= pitch= pitch= pitch= pitch=
0.00 0.00 0.00 0.00 0.00
1=0.00 1=0.00 1=0.00 1=0.00 1=0.00
2=168.87 3=-162.03 4=-96.84 2=74.61 3=-108.12 4=-56.49 2=81.48 3=-60.86 4=-110.62 2=137.50 3=-98.82 4=-128.68 2=168.87 3=-162.03 4=-96.84
By putting the angles from each line of the log file into the forward kinematics it can be seen in figure 5.1-5.4 that the arm accurately draws the square.
Figure 5.1: Matlab, Frame1 of sequence
Figure 5.2: Matlab, Frame2 of sequence
Figure 5.3: Matlab, Frame3 of sequencee
Figure 5.4: Matlab, Frame4 of sequence
This is also matched by the physical arm as shown in figure 5.5-5.8 A more complex pick and place task was also performed. This involved picking up a pen and drawing a circle. a log file for this task is included in the appendix, a video can be seen at http://youtu.be/rd4mS6ZGti4 . This task was performed well however it did not produce a perfect circle. I found that this was due to a hardware limitation of the robot, there is too much friction in the base rotation servo so it cannot keep θ1 accurate.
10031755
Queron Williams
13
Figure 5.5: Video, Frame1 of sequence
Figure 5.6: Video, Frame2 of sequence
Figure 5.7: Video, Frame3 of sequencee
Figure 5.8: Video, Frame4 of sequence
6. C ONCLUSION In conclusion I have shown how to derive the forward kinematics for the Lynxmotion robot arm using the Proximal method. If i had more time i would like to look into using the Distal method as it seams that this is a more standardised method. I have also produced a model of the robot arm and used this to test and that the forward kinematics are correct. The forward kinematics have then been used to accurately plot the robots workspace, showing what areas are within reach and also indicating required t angels to reach the target. I have also shown how to derive the inverse kinematics using the geometric method. The geometric method is simpler for smaller robot arms but can become difficult when working with larger robot arms. If I was to repeat this task on a robot arm with more degrees of freedom I would try and use the algebraic method. The Inverse kinematics were then modelled and tested in a variety of positions using the Forwards kinematics to check each set of results, including multiple solutions. Finally i have used this model to plan a task. The output of each stage of the task have been checked using the forward kinematics. The task has then been executed on the physical robot and its output is shown to match with what was expected from the forward kinematics. The most difficult problem whilst planning the task was dealing with the hardware design of the robot arm. Often things would work in the model but not on the physical arm. this was because the robot arm often did not have the range of motion needed or would collide with its self before reaching its target. Overall I feel that I have produced a stable and reliable model of the robot arm. Whilst completing this task i have gained a good understanding of both forward / inverse kinematics and why these tools can be useful for deg robotic systems.
10031755
Queron Williams
14
A. MATLAB C ODE - MAIN . M 1 2 3 4 5 6 7 8 9
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% Queron Williams %%%%%%%%%%% %%%%%%%%%% Robotic Mechanics %%%%%%%%%% %%%%%%%%%%%% October 2013 %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% f i r s t c l e a r everything clc clear a l l close a l l
10 11 12 13 14 15
%% s t a t i c varables and gl o ba l s global rad2deg deg2rad L1 L2 L3 L4 L5 global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b deg2rad = pi /180 ; % multiplying by t h i s w i l l convert degrees to rads rad2deg = 180/ pi ; % multiplying by t h i s w i l l convert degrees to rads
16 17 18 19 20 21 22
%% Program options PlotAxis = 0; PlotLinks = 1 ; PlotWorkspace = 0 ; InverseKinematics = 0 ; executePath = 1 ;
23 24 25 26 27 28 29 30 31 32 33 34 35 36
%% v a r i a b l e s such as l i n k length and theta ’ s %l i n k lengths L1 = 6 . 9 ; L2 = 9 . 4 ; L3 = 1 0 . 8 ; L4 = 8 . 5 ; L5 = 0 ; % thetas theta1 = 00 * deg2rad ; %−90 to 90 theta2 = 137.50 * deg2rad ; % around −10 to 90 theta3 = −98.82 * deg2rad ; theta4 = −128.68 * deg2rad ; theta5 = 0 * deg2rad ;
37 38 39 40 41 42 43 44 45
%% DH %l i n k DH1 = DH2 = DH3 = DH4 = DH5 = DHE =
table ai −1 [ 0, [ 0, [ L2 , [ L3 , [ 0, [ 0,
alphai −1 0, ( pi /2) , 0, 0, −( pi /2) , 0,
d L1 , 0, 0, 0, L4 , L5 ,
theta theta1 ] ; theta2 ] ; theta3 ] ; theta4 ] ; theta5 ] ; 0];
46 47 48 49
%% transformation matrix ’ s % c a l c u l a t e the transformation matrix f o r each l i n k BL = get_dh_matrix ( [ 0 , 0 , 0 , 0 ] ) ; %Base Link
10031755
Queron Williams
15
50 51 52 53 54 55
T01 T12 T23 T34 T45 T5E
= = = = = =
get_dh_matrix (DH1) ; get_dh_matrix (DH2) ; get_dh_matrix (DH3) ; get_dh_matrix (DH4) ; get_dh_matrix (DH5) ; get_dh_matrix (DHE) ;
56 57 58 59 60 61 62
% calculate T02 = T01 * T03 = T02 * T04 = T03 * T05 = T04 * T0E = T05 *
the t o t a l transformation matrix T12 ; T23 ; T34 ; T45 ; T5E ;
63 64 65 66 67 68 69 70 71 72 73 74 75 76
%% %% %% hold on grid on ; y l a b e l ( ’Y (cm) ’ ) , x l a b e l ( ’X(cm) ’ ) , z l a b e l ( ’Z(cm) ’ ) daspect ( [ 1 1 1 ] ) %keep aspect r a t i o the same %view ([ −1 , −1 ,1])%isometric view ( [ 0 , − 1 , 0 ] )%side %view ( [ − 1 , 0 , 0 ] )%back %a x i s auto a x i s ([ −10 25 −0 25 −5 2 5 ] ) ; %manualy s e t a x i s %a x i s ([ −30 30 −30 30 −15 4 0 ] ) ; %manualy s e t a x i s
77 78 79 80 81 82 83 84 85 86 87 88 89 90
i f PlotLinks >= 1 OOx = [ 0 ; T01 ( 1 , 4 ) ; T02 ( 1 , 4 ) ; T03 ( 1 , 4 ) ; OOy = [ 0 ; T01 ( 2 , 4 ) ; T02 ( 2 , 4 ) ; T03 ( 2 , 4 ) ; OOz = [ 0 ; T01 ( 3 , 4 ) ; T02 ( 3 , 4 ) ; T03 ( 3 , 4 ) ; plot3 (OOx,OOy,OOz, ’b ’ , ’ LineWidth ’ , PlotLinks ) t e x t (BL( 1 , 4 ) ,BL( 2 , 4 ) ,BL( 3 , 4 ) , ’ L0 ’ ) ; t e x t ( T01 ( 1 , 4 ) , T01 ( 2 , 4 ) , T01 ( 3 , 4 ) , ’ L1 ’ ) ; t e x t ( T02 ( 1 , 4 ) , T02 ( 2 , 4 ) , T02 ( 3 , 4 ) , ’ L2 ’ ) ; t e x t ( T03 ( 1 , 4 ) , T03 ( 2 , 4 ) , T03 ( 3 , 4 ) , ’ L3 ’ ) ; t e x t ( T04 ( 1 , 4 ) , T04 ( 2 , 4 ) , T04 ( 3 , 4 ) , ’ L4 ’ ) ; t e x t ( T05 ( 1 , 4 ) , T05 ( 2 , 4 ) , T05 ( 3 , 4 ) , ’ L5 ’ ) ; t e x t ( T0E ( 1 , 4 ) ,T0E ( 2 , 4 ) ,T0E ( 3 , 4 ) , ’LE ’ ) end
T04 ( 1 , 4 ) ; T05 ( 1 , 4 ) ; T0E ( 1 , 4 ) ] ; T04 ( 2 , 4 ) ; T05 ( 2 , 4 ) ; T0E ( 2 , 4 ) ] ; T04 ( 3 , 4 ) ; T05 ( 3 , 4 ) ; T0E ( 3 , 4 ) ] ; % t h i s p l o t s the p vector
91 92 93 94 95 96 97 98 99 100
i f P l o t A x i s == 1 t r p l o t (BL , ’ frame ’ , t r p l o t ( T01 , ’ frame ’ t r p l o t ( T02 , ’ frame ’ t r p l o t ( T03 , ’ frame ’ t r p l o t ( T04 , ’ frame ’ t r p l o t ( T05 , ’ frame ’ t r p l o t ( T0E , ’ frame ’ end
, , , , , ,
’0 ’ , ’1 ’ ’2 ’ ’3 ’ ’4 ’ ’5 ’ ’e ’
, , , , , ,
’ color ’ , ’ color ’ ’ color ’ ’ color ’ ’ color ’ ’ color ’ ’ color ’
, , , , , ,
[1 ,0 ,0]) ; [0.6 ,0 ,0.3]) ; [0.3 ,0 ,0.6]) ; [0 ,0 ,1]) ; [0 ,0.3 ,0.6]) ; [0 ,0.6 ,0.3]) ; [0 ,1 ,0]) ;
101
10031755
Queron Williams
16
102 103 104 105 106 107 108 109 110 111 112 113 114 115
i f PlotWorkspace == 1 X = []; Y = []; Z = []; C = [] ,[]; f o r i = −90:2:90 , f p r i n t f ( ’ at i t e r a t i o n : %d \n ’ , i ) %progress report 4 workspace plot f o r j =0:10:135 , f o r k= −135:10:0 , f o r l = −180:5:0 , theta1= i * deg2rad ; theta2= j * deg2rad ; theta3=k * deg2rad ; theta4= l * deg2rad ;
116 117 118 119 120 121 122
DH1 DH2 DH3 DH4 DH5 DHE
= = = = = =
[ [ [ [ [ [
0, 0, L2 , L3 , 0, 0,
0, ( pi /2) , 0, 0, −( pi /2) , 0,
L1 , 0, 0, 0, L4 , L5 ,
T01 T02 T03 T04 T05 T0E
= = = = = =
get_dh_matrix (DH1) ; T01 * get_dh_matrix (DH2) ; T02 * get_dh_matrix (DH3) ; T03 * get_dh_matrix (DH4) ; T04 * get_dh_matrix (DH5) ; T05 * get_dh_matrix (DHE) ;
theta1 ] ; theta2 ] ; theta3 ] ; theta4 ] ; theta5 ] ; 0];
123 124 125 126 127 128 129 130
C( end+1 ,1) = 1−( j /135) ; %s e t red value from j o i n t 2 C( end , 2 ) = ( ( k+135) /135) ;%s e t green value from j o i n t 3 C( end , 3 ) = ( l /−180) ; %s e t blue value from j o i n t 4 X( end+1) = T0E ( 1 , 4 ) ; Y ( end+1) = T0E ( 2 , 4 ) ; Z( end+1) = T0E ( 3 , 4 ) ;
131 132 133 134 135 136 137
end
138
end
139
end
140
end s c a t t e r 3 (X , Y , Z, 2 0 ,C, ’ f i l l e d ’ ) ; hold on
141 142 143 144 145
end
146 147
%hold o f f
148 149 150 151
i f InverseKinematics == 1 %phi = 91+ theta2+theta3+theta4 ; %comment in f o r same phi as input phi = 4 5; %comment in to manualy s e t wrwtg angles
152
10031755
Queron Williams
17
[ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( T0E ( 1 , 4 ) ,T0E ( 2 , 4 ) ,T0E ( 3 , 4 ) , phi ) ; f p r i n t f ( ’ input angles : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , theta1 * rad2deg , theta2 * rad2deg , theta3 * rad2deg , theta4 * rad2deg ) f p r i n t f ( ’End e f e c t o r position : \ t x= %.2 f \ t y= %.2 f \ t z= %.2 f \n ’ , T0E ( 1 , 4 ) , T0E ( 2 , 4 ) , T0E ( 3 , 4 ) ) f p r i n t f ( ’ Solution 1 : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a ) f p r i n t f ( ’ Solution 2 : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2b , IKtheta3b , IKtheta4b )
153
154
155
156
157
158 159
end
160 161 162 163 164 165 166 167 168
i f executePath == 2 %simple square path x =10; y =0; z =10; pitch =0; gripper =0; %gripper i s open time =2; %time f o r each move to take (2 seconds ) UpdateArm( x , y , z , pitch , gripper , time ) ; %move to s t a r t point
169
x =20; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 1
170 171 172
z =20; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 2
173 174 175
x =10; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 3
176 177 178
z =10; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 4
179 180 181 182
end
183 184 185 186 187 188 189 190
i f executePath == 1 %pick and place task path pathX = 11; pathY = 0 ; pathZ = 5 ; pathWRWTG = −90; pathGripper = 0 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
191 192 193
pathZ = 2 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
194 195 196
pathGripper = 45; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
197 198 199
pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
10031755
Queron Williams
18
200
pathX = 15; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
201 202 203
f o r i = −90:10:0 , pathWRWTG = i ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end
204 205 206 207 208
pathX = 21; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
209 210 211
pathY = 5 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
212 213 214
pathZ = 4 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
215 216 217 218
f o r i =0 : 10 :360 , pathX = 21 + ( 5 * sin ( i * deg2rad ) ) ; pathY = ( 5 * cos ( i * deg2rad ) ) ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end pathY = 0 ; pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
219 220 221 222 223 224 225 226 227
pathX = 15; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
228 229 230
f o r i =0: −10: −90 , pathWRWTG = i ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end
231 232 233 234 235
pathX = 11; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
236 237 238
pathZ = 2 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
239 240 241
pathGripper = 0 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
242 243 244
pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
245 246 247 248
end
10031755
Queron Williams
19
B. MATLAB C ODE - SOLVE IK. M 1
2
function [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( xTarget , yTarget , zTarget , wrwtgTarget ) global rad2deg deg2rad L1 L2 L3 L4 IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b
3
IKtheta1 = atan2 ( yTarget , xTarget ) * rad2deg ; %wrwtg = 90+ theta2+theta3+theta4 ; wrwtg = wrwtgTarget ; z = zTarget −(L4 * sin ( wrwtg * deg2rad ) )−L1 ; XYhyp = s q r t ( ( xTarget ^2) +( yTarget ^2) ) −(L4 * cos ( wrwtg * deg2rad ) ) ;
4 5 6 7 8 9
IKcostheta3 = ( ( XYhyp^2) +( z ^2) −(L2^2) −(L3^2) ) / ( 2 * L2 * L3 ) ;
10 11
IKtheta3a = atan2 (+ s q r t (1 −( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ; IKtheta3b = atan2(− s q r t (1 −( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ;
12 13 14
IKcostheta2a = ( ( XYhyp * ( L2+(L3 * cos ( IKtheta3a * deg2rad ) ) ) ) +( z * L3 * sin ( IKtheta3a * deg2rad ) ) ) / ( ( XYhyp^2) +( z ^2) ) ; IKcostheta2b = ( ( XYhyp * ( L2+(L3 * cos ( IKtheta3b * deg2rad ) ) ) ) +( z * L3 * sin ( IKtheta3b * deg2rad ) ) ) / ( ( XYhyp^2) +( z ^2) ) ;
15
16
17
IKtheta2a = atan2 (+ s q r t (1 −( IKcostheta2a ^2) ) , IKcostheta2a ) * rad2deg ; IKtheta2b = atan2 (+ s q r t (1 −( IKcostheta2b ^2) ) , IKcostheta2b ) * rad2deg ;
18 19 20
IKtheta4a = wrwtg−( IKtheta2a+IKtheta3a ) −90; IKtheta4b = wrwtg−(IKtheta2b+IKtheta3b ) −90;
21 22 23
end
10031755
Queron Williams
20
C. MATLAB C ODE - U PDATE J OINT. M 1 2 3 4 5 6 7 8 9
function updatet ( j o i n t , t a r g e t , SpeedOrTime , SpeedOrTimeAmount ) p e r s i s t e n t f i r s t ; %check i f t h i s i s the f i r s t use p e r s i s t e n t arm ; i f isempty ( f i r s t ) delete ( i n s t r f i n d a l l ) ; %i f f i r s t use c l e a r any previous s e r i a l objects arm = s e r i a l ( ’COM13 ’ , ’ BaudRate ’ , 115200 , ’ Terminator ’ , ’CR/LF ’ ) ; fopen (arm) ; %open new s e r i a l port f i r s t = 1; end
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
switch j o i n t ; %depending on j o i n t , convert degrees to pulse width case 0 t a r g e t = round (map( t a r g e t , −90 ,90 ,500 ,2500) ) ; %checked case 1 t a r g e t = round (map( t a r g e t ,0 ,180 ,750 ,2250) ) ; %checked case 2 t a r g e t = round (map( t a r g e t , −180 ,0 ,2500 ,500) ) ; %checked case 3 t a r g e t = round (map( t a r g e t , −180 ,0 ,500 ,2500) ) ; %checked case 4 t a r g e t = round (map( t a r g e t , −90 ,90 ,500 ,2500) ) ; %checked case 5 t a r g e t = round (map( t a r g e t , −90 ,90 ,500 ,2500) ) ; %checked end %send pulse width to servo f p r i n t f (arm , s p r i n t f ( ’#%d P %d %c %d ’ , j o i n t , t a r g e t , SpeedOrTime , SpeedOrTimeAmount) )
27 28
end
D. MATLAB C ODE - U PDATE A RM . M 1 2 3
4 5 6 7 8 9 10 11
function UpdateArm( x , y , z , wrwtg , gripper , time ) global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b ; [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( x , y , z , wrwtg ) ; Updatet ( 0 , IKtheta1 , ’ t ’ , ( time * 1000) ) ; Updatet ( 1 , IKtheta2b , ’ t ’ , ( time * 1000) ) ; Updatet ( 2 , IKtheta3b , ’ t ’ , ( time * 1000) ) ; Updatet ( 3 , IKtheta4b , ’ t ’ , ( time * 1000) ) ; Updatet ( 4 , gripper , ’ t ’ ,500 ) ; Updatet ( 5 , 0 , ’ t ’ ,500 ) ; f p r i n t f ( ’ x= %.2 f \ t y= %.2 f \ t z= %.2 f \ t pitch= %.2 f \ t ’ , x , y , z , wrwtg ) ; f p r i n t f ( ’ 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2b , IKtheta3b , IKtheta4b ) ;
12
delay ( time ) ;
13 14
end
10031755
Queron Williams
21
E. MATLAB C ODE - GET _ DH _ MATRIX . M 1 2 3
4
5 6
function T = get_dh_matrix ( parameters ) T = [ ( cos ( parameters ( 1 , 4 ) ) ) , −( sin ( parameters ( 1 , 4 ) ) ) , 0 , ( parameters ( 1 , 1 ) ) ; ( sin ( parameters ( 1 , 4 ) ) ) * ( cos ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 4 ) ) ) * ( cos ( parameters ( 1 , 2 ) ) ) , −( sin ( parameters ( 1 , 2 ) ) ) , −( sin ( parameters ( 1 , 2 ) ) ) * ( parameters ( 1 , 3 ) ) ; ( sin ( parameters ( 1 , 4 ) ) ) * ( sin ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 4 ) ) ) * ( sin ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 2 ) ) ) * ( parameters (1 ,3) ) ; 0 ,0 ,0 ,1]; end
F. MATLAB C ODE - MAP. M 1 2 3
function output = map( x , in_min , in_max , out_min , out_max ) output = ( x − in_min ) * ( out_max − out_min ) / ( in_max − in_min ) + out_min ; end
G. MATLAB C ODE - DELAY. M 1 2 3 4 5 6 7
function delay ( seconds ) % function pause the program % seconds = delay time in seconds tic ; while toc < seconds end end
10031755
Queron Williams
22
H. L OG FILE FOR COMPLEX TASK x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x=
11.00 11.00 11.00 11.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 21.00 21.00 21.00 21.00 21.87 22.71 23.50 24.21 24.83 25.33 25.70 25.92 26.00 25.92 25.70 25.33 24.83 24.21 23.50 22.71 21.87 21.00 20.13 19.29 18.50 17.79 17.17 16.67 16.30 16.08 16.00 16.08 16.30 16.67
10031755
y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y=
0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 5.00 5.00 5.00 4.92 4.70 4.33 3.83 3.21 2.50 1.71 0.87 0.00 -0.87 -1.71 -2.50 -3.21 -3.83 -4.33 -4.70 -4.92 -5.00 -4.92 -4.70 -4.33 -3.83 -3.21 -2.50 -1.71 -0.87 -0.00 0.87 1.71 2.50
z= 5.00 z= 2.00 z= 2.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00
pitch= -90.00 1= 0.00 2= 86.56 3= -101.49 4= -165.07 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58 pitch= -90.00 1= 0.00 2= 88.77 3= -87.70 4= -181.07 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52 pitch= -80.00 1= 0.00 2= 73.09 3= -70.55 4= -172.53 pitch= -70.00 1= 0.00 2= 82.08 3= -83.26 4= -158.82 pitch= -60.00 1= 0.00 2= 90.06 3= -95.04 4= -145.01 pitch= -50.00 1= 0.00 2= 96.88 3= -106.05 4= -130.83 pitch= -40.00 1= 0.00 2= 102.21 3= -116.27 4= -115.94 pitch= -30.00 1= 0.00 2= 105.45 3= -125.54 4= -99.91 pitch= -20.00 1= 0.00 2= 105.66 3= -133.50 4= -82.16 pitch= -10.00 1= 0.00 2= 101.63 3= -139.51 4= -62.12 pitch= 0.00 1= 0.00 2= 92.57 3= -142.71 4= -39.86 pitch= 0.00 1= 0.00 2= 61.83 3= -103.54 4= -48.29 pitch= 0.00 1= 13.39 2= 59.07 3= -99.22 4= -49.85 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= 12.69 2= 37.67 3= -90.83 4= -36.84 pitch= 0.00 1= 11.69 2= 34.72 3= -84.56 4= -40.16 pitch= 0.00 1= 10.44 2= 31.83 3= -78.51 4= -43.32 pitch= 0.00 1= 8.99 2= 29.09 3= -72.84 4= -46.24 pitch= 0.00 1= 7.38 2= 26.59 3= -67.75 4= -48.84 pitch= 0.00 1= 5.64 2= 24.46 3= -63.43 4= -51.03 pitch= 0.00 1= 3.81 2= 22.82 3= -60.12 4= -52.70 pitch= 0.00 1= 1.92 2= 21.77 3= -58.03 4= -53.75 pitch= 0.00 1= 0.00 2= 21.41 3= -57.31 4= -54.10 pitch= 0.00 1= -1.92 2= 21.77 3= -58.03 4= -53.75 pitch= 0.00 1= -3.81 2= 22.82 3= -60.12 4= -52.70 pitch= 0.00 1= -5.64 2= 24.46 3= -63.43 4= -51.03 pitch= 0.00 1= -7.38 2= 26.59 3= -67.75 4= -48.84 pitch= 0.00 1= -8.99 2= 29.09 3= -72.84 4= -46.24 pitch= 0.00 1= -10.44 2= 31.83 3= -78.51 4= -43.32 pitch= 0.00 1= -11.69 2= 34.72 3= -84.56 4= -40.16 pitch= 0.00 1= -12.69 2= 37.67 3= -90.83 4= -36.84 pitch= 0.00 1= -13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= -13.74 2= 43.38 3= -103.42 4= -29.96 pitch= 0.00 1= -13.69 2= 46.01 3= -109.47 4= -26.53 pitch= 0.00 1= -13.17 2= 48.37 3= -115.17 4= -23.21 pitch= 0.00 1= -12.15 2= 50.42 3= -120.36 4= -20.06 pitch= 0.00 1= -10.60 2= 52.10 3= -124.90 4= -17.20 pitch= 0.00 1= -8.53 2= 53.38 3= -128.63 4= -14.75 pitch= 0.00 1= -5.99 2= 54.26 3= -131.42 4= -12.84 pitch= 0.00 1= -3.09 2= 54.77 3= -133.14 4= -11.63 pitch= 0.00 1= -0.00 2= 54.94 3= -133.72 4= -11.21 pitch= 0.00 1= 3.09 2= 54.77 3= -133.14 4= -11.63 pitch= 0.00 1= 5.99 2= 54.26 3= -131.42 4= -12.84 pitch= 0.00 1= 8.53 2= 53.38 3= -128.63 4= -14.75
Queron Williams
23
x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= >>
17.17 17.79 18.50 19.29 20.13 21.00 21.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 11.00 11.00 11.00 11.00
y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y=
3.21 3.83 4.33 4.70 4.92 5.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00
z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z=
4.00 4.00 4.00 4.00 4.00 4.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 2.00 2.00 8.00
pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch=
0.00 1= 10.60 0.00 1= 12.15 0.00 1= 13.17 0.00 1= 13.69 0.00 1= 13.74 0.00 1= 13.39 0.00 1= 0.00 0.00 1= 0.00 0.00 1= 0.00 -10.00 1= 0.00 -20.00 1= 0.00 -30.00 1= 0.00 -40.00 1= 0.00 -50.00 1= 0.00 -60.00 1= 0.00 -70.00 1= 0.00 -80.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00
2= 52.10 3= -124.90 4= -17.20 2= 50.42 3= -120.36 4= -20.06 2= 48.37 3= -115.17 4= -23.21 2= 46.01 3= -109.47 4= -26.53 2= 43.38 3= -103.42 4= -29.96 2= 40.58 3= -97.16 4= -33.42 2= 61.83 3= -103.54 4= -48.29 2= 92.57 3= -142.71 4= -39.86 2= 92.57 3= -142.71 4= -39.86 2= 101.63 3= -139.51 4= -62.12 2= 105.66 3= -133.50 4= -82.16 2= 105.45 3= -125.54 4= -99.91 2= 102.21 3= -116.27 4= -115.94 2= 96.88 3= -106.05 4= -130.83 2= 90.06 3= -95.04 4= -145.01 2= 82.08 3= -83.26 4= -158.82 2= 73.09 3= -70.55 4= -172.53 2= 62.98 3= -56.47 4= -186.52 2= 88.77 3= -87.70 4= -181.07 2= 79.07 3= -110.48 4= -148.58 2= 79.07 3= -110.48 4= -148.58 2= 88.77 3= -87.70 4= -181.07
(a video can be seen at http://youtu.be/rd4mS6ZGti4)
10031755
Queron Williams
24
Coursework 1: Manipulator Kinematics
Queron Williams - 10031755 November 14, 2013
1. I NTRODUCTION This document outlines the methods used to derive both the forward kinematics and inverse kinematics for a 5 Degrees Of Freedom Lynxmotion robot arm. From this we will then create a Matlab model of the arm. The model will allow us to perform tasks such as plotting the workspace and planning movement sequences. Finally results from the model will be tested on the robot.
2. F ORWARD K INEMATICS Forward kinematics enable us to work out the final position of the end effector relative to the base link of the robot. To do this we must first know the relation between each link of the robot arm.
2.1. KINEMATIC STRUCTURE OF LYNXMOTION 5DOF A RM The Lynxmotion arm has 5 Degrees Of Freedom. The configuration of the ts in the arm are shown below. The robot consists of 6 links labelled G 0 − G 6 . These Links are connected through ts j 1 − j 5 . For each pair of successive ts there is a common perpendicular distance l 1 − l 5 .
Figure 2.1: t and Link structure of Lynxmotion robot arm
10031755
Queron Williams
1
Each of these t then has axes added. The z-axis is chosen to lie along the t axis of each t. The x-axis is perpendicular to the z-axis of the current t and the z-axis of the next t. The y-axis is chosen to complete the right hand coordinate frame for each t.
Figure 2.2: Axis structure of Lynxmotion robot arm Some of the links (l 5 ) in the diagram actually have a link length of zero on the real robot however they have been show for clarity.
2.2. D ENAVIT-H ARTENBERG PARAMETERS Denavit-Hartenberg (DH) parameters are used to describe the translation between each set of axis in the diagram. there are two types of DH notation, these are called Proximal and Distal. For this example we will be using the Proximal method. The derived DH parameters for the Lynxmotion arm are as follows: Link 1 2 3 4 5 E
a i −1 0 0 l2 l3 0 0
αi −1 0 90 0 0 -90 0
θi θ1 θ2 θ3 θ4 θ5 0
di l1 0 0 0 l4 l5
2.3. T RANSFORMATION M ATRICES Transformation matrices are used to describe the transformation between 2 coordinate frames in a robot. They are made up of both a rotation and translation matrix. R
T =
0
0
T 0 1
(2.1)
There is a transformation matrix that describes any possible transformation when using the proximal method. cosθi −si nθi 0 a i −1 si nθ cosα i i −1 cosθi cosαi −1 −si nαi −1 −si nαi −1 d i i −1 Ti = (2.2) cosαi −1 d i si nθi si nαi −1 cosθi si nαi −1 cosαi −1 0 0 0 1
10031755
Queron Williams
2
This can be used to create a transformation matrix for each t or the robot arm. This is accomplished by substituting values from each line of the DH table into the above matrix. The transformation matrix for each separate t is shown below. cosθ1 −si nθ1 0 0 si nθ cosθ1 0 0 1 0 (2.3) T1 = 0 0 1 l1 0 0 0 1 1
T2 =
2
T3 =
3
T4 =
4
T5 =
cosθ2 0 si nθ2 0
−si nθ2 0 cosθ2 0
0 −1 0 0
0 0 0 1
cosθ3 si nθ3 0 0
−si nθ3 cosθ3 0 0
0 l2 0 0 1 0 0 1
cosθ4 si nθ4 0 0
−si nθ4 cosθ4 0 0
0 l3 0 0 1 0 0 1
cosθ5 0 −si nθ5 0
5
TE =
1 0 0 0
−si nθ5 0 −cosθ5 0
0 0 1 l4 0 0 0 1
0 1 0 0
0 0 0 0 1 l5 0 1
(2.4)
(2.5)
(2.6)
(2.7)
(2.8)
Once we have found each links transformation matrix we can multiply them together to find a complete transformation matrix from the base to the end effector. 0
0
TE =
c 234 ∗ c 1 ∗ c 5 − s 1 ∗ s 5 c 1 ∗ s 5 + c 234 ∗ c 5 ∗ s 1 s 234 ∗ c 5 0
TE =0 T11 T22 T33 T44 T55 TE
(2.9)
−s 234 ∗ c 1 −s 234 ∗ s 1 c 234 0
c 1 ∗ (L3 ∗ c 23 + L2 ∗ c 2 − L4 ∗ s 234 − L5 ∗ s 234 ) s 1 ∗ (L3 ∗ c 23 + L2 ∗ c 2 − L4 ∗ s 234 − L5 ∗ s 234 ) L1 + L3 ∗ s 23 + L2 ∗ s 2 + L4 ∗ c 234 + L5 ∗ c 234 1 (2.10)
Queron Williams
3
−c 5 ∗ s 1 − c 234 ∗ c 1 ∗ s 5 c 1 ∗ c 5 − c 234 ∗ s 1 ∗ s 5 −s 234 ∗ s 5 0
Where: c i = cosθi s i = si nθi c 23 = cos(θ2 + θ3 ) s 23 = si n(θ2 + θ3 ) c 234 = cos(θ2 + θ3 + θ4 ) s 234 = si n(θ2 + θ3 + θ4 )
10031755
2.4. M ODELLING IN MATLAB To model the robot arm in Matlab we are using the previously calculated transformation matrix to find the position of each t then drawing a lime between each t. For all simulations I will be using link lengths measured from the actual robot. These link lengths are: L1 = 6.9cm, L2 = 9.4cm, L3 = 10.8cm, L4 = 8.5cm, L5 = 0cm.
Figure 2.3: MATLAB model of robot arm with all ts set to 0 degrees
2.5. T ESTING F ORWARD K INEMATICS To test that this model works for many different sets of given theta values I have repeated this with the robot in 3 different Cartesian positions as shown below.
Figure 2.4: MATLAB model angles: θ1 = 0, θ2 = 70, θ3 = −20, θ4 = −90, θ5 = 0,
10031755
Queron Williams
4
Figure 2.5: MATLAB model angles: θ1 = −45, θ2 = 50, θ3 = −40, θ4 = −110, θ5 = 0,
Figure 2.6: MATLAB model with angles: θ1 = −90, θ2 = 30, θ3 = −60, θ4 = −130, θ5 = 0,
3. W ORKSPACE A NALYSIS A robots workspace describes all possible positions that the robot can reach with its end effector. In order to accurately plot the workspace the range of possible angles for each t were measured from the robot. These were as follows : θ1 = -90 to 90 , θ2 = 0 to 135 , θ3 = 0 to -135 , θ4 = 0 to -180. θ5 has not been changed as its angle does not affect the final position of the end effector.
3.1. P LOTTING THE W ORKSPACE The Forward kinematic model we have created for the Lynxmotion robot arm can be used to easily plot its workspace. This is done by iterating each t through its range of motion. If we do this for all ts we will get every possible configuration the arm can be in. For each set of t angles we use the forward kinematics to calculate the
10031755
Queron Williams
5
Cartesian position of the end effector. We can then graph the output of the Cartesian points to show which areas can be reached by the robot.
3.2. R ESULTS Whilst plotting the workspace I have varied the colours of the points based on the angle of each of the ts θ2 −θ4 . The red channel represents the value of θ2 , green shows θ3 and blue shows θ4 . This helps give some indication of the angles requited to achieve any given target position.
Figure 3.1: Cross-section of workspace across the XZ plane The red towards the bottom of the workspace shows that θ2 (shoulder) must be further forward for the arm to reach below Zero targets. The green shows that θ3 (elbow) must be outstretched to reach the outer edges of the workspace and the area above/behind. The blue shows that θ4 (wrist) must be bent downwards in order to reach areas closer to the center of the workspace. In reality the robots workspace would be slightly more restricted than the plot shows due to physical constraints of the robot. The Lynxmotion arm would not be able to reach below Z=0 as it is mounted to a flat surface.
10031755
Queron Williams
6
Figure 3.2: Side view of the workspace
Figure 3.3: Top view of the workspace
Figure 3.4: Back view of the workspace
Figure 3.5: Isometric view of the workspace
4. I NVERSE K INEMATICS Often in robotics tasks it is useful to be able to give a robot arm a Cartesian target position and calculate the θ values required to reach the target. In order to achieve this we must be able to solve the Inverse Kinematics for the robot.
4.1. D ERIVING THE I NVERSE K INEMATICS The easiest way to solve inverse kinematics is using the geometric method. This involves breaking the problem down into multiple 2D problems and solving each separately. For the Lynxmotion arm we will seperate the soloution into two seperate problems. The first problem will be to solve θ1 as viewed from above, only considering the XY plane. The second problem is to solve θ2 , θ3 and θ4 . This is done by considering the arm as a 2D problem in a new XZ plane, where X is the hypotenuse formed by the target position in the previous XY plane. To solve the IK we will be given a target x, y, z, pitch and roll for the end effector.
10031755
Queron Williams
7
4.1.1. F INDING θ1
Y 5Y 5 4Y
4 3 θ1 4X
1/2
5X
X
Figure 4.1: Robot arm as viewed in XY plane θ1 can be easily solved using the trigonometric function atan 2 as shown below.
θ1 = at an2(5 y , 5x )
(4.1)
4.1.2. F INDING θ3
Z θ2 5
4
4Z
θ3 3 3Z 2
θ2 3X
4X
X
Figure 4.2: Robot arm as viewed in XZ plane To find θ2 and θ3 we need to first know the position of t 4 on the XZ plane. This means that we need to calculate
10031755
Queron Williams
8
t 4’s position (X 4 and Z4 ). We can do this as we know the target position, link 4’s length and the target pitch ( we will call ϕ)
4x = 5x − l 4 c ϕ
(4.2)
4z = 5z − l 4 s ϕ
(4.3)
To find the X value we need only sum the lengths of the adjacent sides of the two right-angled triangles formed by l 2 &θ2 and l 3 &θ3 . For Z it is the same but using the opposite sides.
4x = l 3 c 23 + l 2 c 2
(4.4)
4z = l 3 s 23 + l 2 s 2
(4.5)
Where c i = cosθi , s i = si nθi , c 23 = cos(θ2 + θ3 ), s 23 = si n(θ2 + θ3 ). The soloution to θ3 can be computed from summation of squaring both equations 4.4 and 4.5
42x
+ 42z
2 42x = l 32 c 23 + l 22 c 22 + 2l 3 l 2 c 23 c 2
(4.6)
2 42z = l 32 s 23 + l 22 s 22 + 2l 3 l 2 s 23 s 2
(4.7)
2 2 = l 32 (c 23 + s 23 ) + l 22 (c 22 + s 22 ) + 2l 3 l 2 (c 23 c 2 + s 23 s 2 )
(4.8)
Because c 2 θi + s 2 θi = 1, equation 4.8 can be rearanged and simplified as follows:
42x
+ 42z
42x + 42z = l 32 + l 22 + 2l 3 l 2 (c 23 c 2 + s 23 s 2 )
(4.9)
42x + 42z = l 32 + l 22 + 2l 3 l 2 (c(θ2 + θ3 )c 2 + s(θ2 + θ3 )s 2 )
(4.10)
= l 32 + l 22 + 2l 3 l 2 (1/2(c(2θ2 + θ3 ) + c 3 ) + 1/2(c 3 − c(2θ2 + θ3 ))) 42x + 42z = l 32 + l 22 + 2l 3 l 2 c 3
(4.11) (4.12)
Rearranged for c(θ3 ):
cθ3 =
−l 22 − l 32 + 42x + 42z 2l 2 l 3
(4.13)
The acos() function can cause inaccuracies at small values of θ, to get around this problem we will use atan2() this requires sθ3 and cθ3 but sine we know c 2 θi + s 2 θi = 1 we can simply rearrange to get sθ3 .
v à ! u 2 2 2 2 2 u −l − l + 4 + 4 x z 2 3 sθ3 = ±t1 − 2l 2 l 3
(4.14)
The final atan2() solution can now be created, note that the ± in the formula will allow us to get both possible solutions for this t.
v à !2 u 2 2 2 2 2 2 2 2 u −l 2 − l 3 + 4x + 4z −l − l + 4x + 4z θ3 = At an2 ±t1 − , 2 3 2l 2 l 3 2l 2 l 3
10031755
Queron Williams
(4.15)
9
4.1.3. F INDING θ2 Now that we have θ3 , we can use equation 4.4 and 4.5 to calculate θ2 . To do this we multiply equation 4.4 by cθ2 and the equation 4.5 by sθ2
c 2 4x = l 3 c(θ2 + θ3 )c 2 + l 2 c 22
(4.16)
s 2 4z = l 3 s(θ2 + θ3 )s 2 + l 2 s 22
(4.17)
c 2 4x + s 2 4z = l 3 (c(θ2 + θ3 )c 2 + s(θ2 + θ3 )s 2 ) + l 2 (c 22 + s 22 )
(4.18)
c 2 4x + s 2 4z = l 3 (1/2(c(2θ2 + θ3 ) + c 3 ) + 1/2(c 3 − c(2θ2 + θ3 ))) + l 2 (c 22 + s 22 )
(4.19)
c 2 4x + s 2 4z = l 3 c 3 + l 2 (c 22 + s 22 )
(4.20)
c 2 4x + s 2 4z = l 3 c 3 + l 2
(4.21)
We repeat this process but this time multiply equation 4.4 by −sθ2 and the equation 4.5 by cθ2
−s 2 4x = −l 3 c(θ2 + θ3 )s 2 − l 2 c 2 s 2
(4.22)
c 2 4z = l 3 s(θ2 + θ3 )c 2 + l 2 s 2 c 2
(4.23)
−s 2 4x + c 2 4z = l 3 s(θ2 + θ3 )c 2 − l 3 c(θ2 + θ3 )s 2
(4.24)
−s 2 4x + c 2 4z = l 3 s 3
(4.25)
Now we multiply both sides of equation 4.21 by −s(θ2 ), and both sides of equation 4.25 by c(θ2 ): before adding the resultant equations:
c 2 42x + s 2 4z 4x = 4x (l 3 c 3 + l 2 )
(4.26)
−s 2 4x 4z + c 2 42z = l 3 s 3 4z
(4.27)
c 2 (42x + 42z ) = 4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
(4.28)
This is then rearranged for c(θ2 ):
cθ2 =
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
(4.29)
42x + 42z
We know c 2 θi + s 2 θi = 1 so we can rearrange to get s(θ2 ):
s
µ
sθ2 = ± 1 −
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z
¶2
(4.30)
42x + 42z
This can be used to give us the final atan2 solution for θ2 , as before the ± in the formula will allow us to get both possible solutions for this t.
à s
θ2 = At an2 ± 1 −
10031755
µ
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z 42x + 42z
Queron Williams
¶2
,
4x (l 3 c 3 + l 2 ) + l 3 s 3 4z 42x + 42z
!
(4.31)
10
4.1.4. F INDING θ3 Now that we have θ2 and θ3 we can easily calculate θ4 using ϕ:
θ4 = ϕ − (θ2 + θ3 ) − 90
(4.32)
This will have to be repeated using the alternate θ2 and θ3 values to get both solutions for θ4 .
4.2. T ESTING THE I NVERSE K INEMATICS To test my inverse kinematics I have used the end effector target positions I acquired during my forward kinematic tests. I have then solved the inverse kinematics for these targets but specifying a pitch of 45 degrees with respect to ground. θ5 has been ignored as it does not affect the position of the end effector, only orientation. For the first set of theta angles (θ1 = 0, θ2 = 70, θ3 = −20, θ4 = −90) Matlab produces this target:
End efector position:
x= 15.62
y= 0.00
z= 30.52
And with a target pitch of 45 degrees it gives these solutions:
Solution 1: Solution 2: >>
1= 0.00 1= 0.00
2= 54.12 2= 68.63
3= 13.56 4= -112.68 3= -13.56 4= -100.06
I then plotted these 2 solutions to check them against the original angles and make sure the target position and pitch were right.
Figure 4.3: Test 1 input
Figure 4.4: Test 1 solution 1
Figure 4.5: Test 1 solution 2
This process was repeated for a second set of theta angles and a pitch of 45 degrees. Matlab produced:
input angles: 1= -22.50 2= 60.00 3= -30.00 4= -100.00 End efector position: x= 20.36 y= -8.43 z= 23.35 Solution 1: 1= -22.50 2= 12.93 3= 37.58 4= -95.51 Solution 2: 1= -22.50 2= 53.21 3= -37.58 4= -60.63 This was then plotted against the original.
Figure 4.6: Test 2 input
10031755
Figure 4.7: Test 2 solution 1
Queron Williams
Figure 4.8: Test 2 solution 2
11
The process was repeated once more for a final set of theta angles and a pitch of 0 degrees. Matlab produced:
input angles: 1= -45.00 2= 50.00 3= -40.00 4= -110.00 End efector position: x= 17.71 y= -17.71 z= 14.50 Solution 1: 1= -45.00 2= 2.95 3= 51.41 4= -144.37 Solution 2: 1= -45.00 2= 52.29 3= -51.41 4= -90.87 This was then plotted against the original.
Figure 4.9: Test 3 input
Figure 4.10: Test 3 solution 1
Figure 4.11: Test 3 solution 2
5. R EAL W ORLD A PPLICATION In order to test the model I will use it to perform a task with the Lynxmotion robot arm. The Lynxmotion robot arm uses a serial servo controller. I have written the following functions to make real time control of the arm easy: • updatet(); allows me to open the serial port and sends values directly to a servo. This function also deals with mapping required t angles to the correct pulse width for each t. this allows for the robot to be updated in real time as the program runs. • solveIK(); this function is provided with the target x/y/z/pitch/roll and peforms all of the IK calculations, returning the required θ vales for each t to reach the target. • UpdateArm(); this function gives a set of target positions to solveIK() then es each of the returned θ values out to the correct t udatet(). It then waits for the movement to be completed before returning and allowing the next command to run. The use of these functions allows for easy task planning with very little code. To create a task you need to specify the starting variables for x/y/z/pitch. Each move only requires one variable to be updated, followed by the UpdateArm() command. The example below shows the code to draw a 10cm*10cm square in the XZ plane:
x=10; y=0; z=10; pitch=0; gripper=0; %gripper is open time=2; %time for each move to take (2 seconds) UpdateArm(x,y,z,pitch,gripper,time); %move to start point x=20; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 1 z=20; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 2 x=10;
10031755
Queron Williams
12
UpdateArm(x,y,z,pitch,gripper,time); %move along edge 3 z=10; UpdateArm(x,y,z,pitch,gripper,time); %move along edge 4 Whilst performing a task matlab produces a log file showing the target position and t angles at each time the arm is updated. This can be used for debugging or manually checking output from the IK.
x= x= x= x= x=
10.00 20.00 20.00 10.00 10.00
y= y= y= y= y=
0.00 0.00 0.00 0.00 0.00
z= z= z= z= z=
10.00 10.00 20.00 20.00 10.00
pitch= pitch= pitch= pitch= pitch=
0.00 0.00 0.00 0.00 0.00
1=0.00 1=0.00 1=0.00 1=0.00 1=0.00
2=168.87 3=-162.03 4=-96.84 2=74.61 3=-108.12 4=-56.49 2=81.48 3=-60.86 4=-110.62 2=137.50 3=-98.82 4=-128.68 2=168.87 3=-162.03 4=-96.84
By putting the angles from each line of the log file into the forward kinematics it can be seen in figure 5.1-5.4 that the arm accurately draws the square.
Figure 5.1: Matlab, Frame1 of sequence
Figure 5.2: Matlab, Frame2 of sequence
Figure 5.3: Matlab, Frame3 of sequencee
Figure 5.4: Matlab, Frame4 of sequence
This is also matched by the physical arm as shown in figure 5.5-5.8 A more complex pick and place task was also performed. This involved picking up a pen and drawing a circle. a log file for this task is included in the appendix, a video can be seen at http://youtu.be/rd4mS6ZGti4 . This task was performed well however it did not produce a perfect circle. I found that this was due to a hardware limitation of the robot, there is too much friction in the base rotation servo so it cannot keep θ1 accurate.
10031755
Queron Williams
13
Figure 5.5: Video, Frame1 of sequence
Figure 5.6: Video, Frame2 of sequence
Figure 5.7: Video, Frame3 of sequencee
Figure 5.8: Video, Frame4 of sequence
6. C ONCLUSION In conclusion I have shown how to derive the forward kinematics for the Lynxmotion robot arm using the Proximal method. If i had more time i would like to look into using the Distal method as it seams that this is a more standardised method. I have also produced a model of the robot arm and used this to test and that the forward kinematics are correct. The forward kinematics have then been used to accurately plot the robots workspace, showing what areas are within reach and also indicating required t angels to reach the target. I have also shown how to derive the inverse kinematics using the geometric method. The geometric method is simpler for smaller robot arms but can become difficult when working with larger robot arms. If I was to repeat this task on a robot arm with more degrees of freedom I would try and use the algebraic method. The Inverse kinematics were then modelled and tested in a variety of positions using the Forwards kinematics to check each set of results, including multiple solutions. Finally i have used this model to plan a task. The output of each stage of the task have been checked using the forward kinematics. The task has then been executed on the physical robot and its output is shown to match with what was expected from the forward kinematics. The most difficult problem whilst planning the task was dealing with the hardware design of the robot arm. Often things would work in the model but not on the physical arm. this was because the robot arm often did not have the range of motion needed or would collide with its self before reaching its target. Overall I feel that I have produced a stable and reliable model of the robot arm. Whilst completing this task i have gained a good understanding of both forward / inverse kinematics and why these tools can be useful for deg robotic systems.
10031755
Queron Williams
14
A. MATLAB C ODE - MAIN . M 1 2 3 4 5 6 7 8 9
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%% Queron Williams %%%%%%%%%%% %%%%%%%%%% Robotic Mechanics %%%%%%%%%% %%%%%%%%%%%% October 2013 %%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% f i r s t c l e a r everything clc clear a l l close a l l
10 11 12 13 14 15
%% s t a t i c varables and gl o ba l s global rad2deg deg2rad L1 L2 L3 L4 L5 global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b deg2rad = pi /180 ; % multiplying by t h i s w i l l convert degrees to rads rad2deg = 180/ pi ; % multiplying by t h i s w i l l convert degrees to rads
16 17 18 19 20 21 22
%% Program options PlotAxis = 0; PlotLinks = 1 ; PlotWorkspace = 0 ; InverseKinematics = 0 ; executePath = 1 ;
23 24 25 26 27 28 29 30 31 32 33 34 35 36
%% v a r i a b l e s such as l i n k length and theta ’ s %l i n k lengths L1 = 6 . 9 ; L2 = 9 . 4 ; L3 = 1 0 . 8 ; L4 = 8 . 5 ; L5 = 0 ; % thetas theta1 = 00 * deg2rad ; %−90 to 90 theta2 = 137.50 * deg2rad ; % around −10 to 90 theta3 = −98.82 * deg2rad ; theta4 = −128.68 * deg2rad ; theta5 = 0 * deg2rad ;
37 38 39 40 41 42 43 44 45
%% DH %l i n k DH1 = DH2 = DH3 = DH4 = DH5 = DHE =
table ai −1 [ 0, [ 0, [ L2 , [ L3 , [ 0, [ 0,
alphai −1 0, ( pi /2) , 0, 0, −( pi /2) , 0,
d L1 , 0, 0, 0, L4 , L5 ,
theta theta1 ] ; theta2 ] ; theta3 ] ; theta4 ] ; theta5 ] ; 0];
46 47 48 49
%% transformation matrix ’ s % c a l c u l a t e the transformation matrix f o r each l i n k BL = get_dh_matrix ( [ 0 , 0 , 0 , 0 ] ) ; %Base Link
10031755
Queron Williams
15
50 51 52 53 54 55
T01 T12 T23 T34 T45 T5E
= = = = = =
get_dh_matrix (DH1) ; get_dh_matrix (DH2) ; get_dh_matrix (DH3) ; get_dh_matrix (DH4) ; get_dh_matrix (DH5) ; get_dh_matrix (DHE) ;
56 57 58 59 60 61 62
% calculate T02 = T01 * T03 = T02 * T04 = T03 * T05 = T04 * T0E = T05 *
the t o t a l transformation matrix T12 ; T23 ; T34 ; T45 ; T5E ;
63 64 65 66 67 68 69 70 71 72 73 74 75 76
%% %% %% hold on grid on ; y l a b e l ( ’Y (cm) ’ ) , x l a b e l ( ’X(cm) ’ ) , z l a b e l ( ’Z(cm) ’ ) daspect ( [ 1 1 1 ] ) %keep aspect r a t i o the same %view ([ −1 , −1 ,1])%isometric view ( [ 0 , − 1 , 0 ] )%side %view ( [ − 1 , 0 , 0 ] )%back %a x i s auto a x i s ([ −10 25 −0 25 −5 2 5 ] ) ; %manualy s e t a x i s %a x i s ([ −30 30 −30 30 −15 4 0 ] ) ; %manualy s e t a x i s
77 78 79 80 81 82 83 84 85 86 87 88 89 90
i f PlotLinks >= 1 OOx = [ 0 ; T01 ( 1 , 4 ) ; T02 ( 1 , 4 ) ; T03 ( 1 , 4 ) ; OOy = [ 0 ; T01 ( 2 , 4 ) ; T02 ( 2 , 4 ) ; T03 ( 2 , 4 ) ; OOz = [ 0 ; T01 ( 3 , 4 ) ; T02 ( 3 , 4 ) ; T03 ( 3 , 4 ) ; plot3 (OOx,OOy,OOz, ’b ’ , ’ LineWidth ’ , PlotLinks ) t e x t (BL( 1 , 4 ) ,BL( 2 , 4 ) ,BL( 3 , 4 ) , ’ L0 ’ ) ; t e x t ( T01 ( 1 , 4 ) , T01 ( 2 , 4 ) , T01 ( 3 , 4 ) , ’ L1 ’ ) ; t e x t ( T02 ( 1 , 4 ) , T02 ( 2 , 4 ) , T02 ( 3 , 4 ) , ’ L2 ’ ) ; t e x t ( T03 ( 1 , 4 ) , T03 ( 2 , 4 ) , T03 ( 3 , 4 ) , ’ L3 ’ ) ; t e x t ( T04 ( 1 , 4 ) , T04 ( 2 , 4 ) , T04 ( 3 , 4 ) , ’ L4 ’ ) ; t e x t ( T05 ( 1 , 4 ) , T05 ( 2 , 4 ) , T05 ( 3 , 4 ) , ’ L5 ’ ) ; t e x t ( T0E ( 1 , 4 ) ,T0E ( 2 , 4 ) ,T0E ( 3 , 4 ) , ’LE ’ ) end
T04 ( 1 , 4 ) ; T05 ( 1 , 4 ) ; T0E ( 1 , 4 ) ] ; T04 ( 2 , 4 ) ; T05 ( 2 , 4 ) ; T0E ( 2 , 4 ) ] ; T04 ( 3 , 4 ) ; T05 ( 3 , 4 ) ; T0E ( 3 , 4 ) ] ; % t h i s p l o t s the p vector
91 92 93 94 95 96 97 98 99 100
i f P l o t A x i s == 1 t r p l o t (BL , ’ frame ’ , t r p l o t ( T01 , ’ frame ’ t r p l o t ( T02 , ’ frame ’ t r p l o t ( T03 , ’ frame ’ t r p l o t ( T04 , ’ frame ’ t r p l o t ( T05 , ’ frame ’ t r p l o t ( T0E , ’ frame ’ end
, , , , , ,
’0 ’ , ’1 ’ ’2 ’ ’3 ’ ’4 ’ ’5 ’ ’e ’
, , , , , ,
’ color ’ , ’ color ’ ’ color ’ ’ color ’ ’ color ’ ’ color ’ ’ color ’
, , , , , ,
[1 ,0 ,0]) ; [0.6 ,0 ,0.3]) ; [0.3 ,0 ,0.6]) ; [0 ,0 ,1]) ; [0 ,0.3 ,0.6]) ; [0 ,0.6 ,0.3]) ; [0 ,1 ,0]) ;
101
10031755
Queron Williams
16
102 103 104 105 106 107 108 109 110 111 112 113 114 115
i f PlotWorkspace == 1 X = []; Y = []; Z = []; C = [] ,[]; f o r i = −90:2:90 , f p r i n t f ( ’ at i t e r a t i o n : %d \n ’ , i ) %progress report 4 workspace plot f o r j =0:10:135 , f o r k= −135:10:0 , f o r l = −180:5:0 , theta1= i * deg2rad ; theta2= j * deg2rad ; theta3=k * deg2rad ; theta4= l * deg2rad ;
116 117 118 119 120 121 122
DH1 DH2 DH3 DH4 DH5 DHE
= = = = = =
[ [ [ [ [ [
0, 0, L2 , L3 , 0, 0,
0, ( pi /2) , 0, 0, −( pi /2) , 0,
L1 , 0, 0, 0, L4 , L5 ,
T01 T02 T03 T04 T05 T0E
= = = = = =
get_dh_matrix (DH1) ; T01 * get_dh_matrix (DH2) ; T02 * get_dh_matrix (DH3) ; T03 * get_dh_matrix (DH4) ; T04 * get_dh_matrix (DH5) ; T05 * get_dh_matrix (DHE) ;
theta1 ] ; theta2 ] ; theta3 ] ; theta4 ] ; theta5 ] ; 0];
123 124 125 126 127 128 129 130
C( end+1 ,1) = 1−( j /135) ; %s e t red value from j o i n t 2 C( end , 2 ) = ( ( k+135) /135) ;%s e t green value from j o i n t 3 C( end , 3 ) = ( l /−180) ; %s e t blue value from j o i n t 4 X( end+1) = T0E ( 1 , 4 ) ; Y ( end+1) = T0E ( 2 , 4 ) ; Z( end+1) = T0E ( 3 , 4 ) ;
131 132 133 134 135 136 137
end
138
end
139
end
140
end s c a t t e r 3 (X , Y , Z, 2 0 ,C, ’ f i l l e d ’ ) ; hold on
141 142 143 144 145
end
146 147
%hold o f f
148 149 150 151
i f InverseKinematics == 1 %phi = 91+ theta2+theta3+theta4 ; %comment in f o r same phi as input phi = 4 5; %comment in to manualy s e t wrwtg angles
152
10031755
Queron Williams
17
[ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( T0E ( 1 , 4 ) ,T0E ( 2 , 4 ) ,T0E ( 3 , 4 ) , phi ) ; f p r i n t f ( ’ input angles : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , theta1 * rad2deg , theta2 * rad2deg , theta3 * rad2deg , theta4 * rad2deg ) f p r i n t f ( ’End e f e c t o r position : \ t x= %.2 f \ t y= %.2 f \ t z= %.2 f \n ’ , T0E ( 1 , 4 ) , T0E ( 2 , 4 ) , T0E ( 3 , 4 ) ) f p r i n t f ( ’ Solution 1 : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a ) f p r i n t f ( ’ Solution 2 : \ t 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2b , IKtheta3b , IKtheta4b )
153
154
155
156
157
158 159
end
160 161 162 163 164 165 166 167 168
i f executePath == 2 %simple square path x =10; y =0; z =10; pitch =0; gripper =0; %gripper i s open time =2; %time f o r each move to take (2 seconds ) UpdateArm( x , y , z , pitch , gripper , time ) ; %move to s t a r t point
169
x =20; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 1
170 171 172
z =20; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 2
173 174 175
x =10; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 3
176 177 178
z =10; UpdateArm( x , y , z , pitch , gripper , time ) ; %move along edge 4
179 180 181 182
end
183 184 185 186 187 188 189 190
i f executePath == 1 %pick and place task path pathX = 11; pathY = 0 ; pathZ = 5 ; pathWRWTG = −90; pathGripper = 0 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
191 192 193
pathZ = 2 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
194 195 196
pathGripper = 45; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
197 198 199
pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
10031755
Queron Williams
18
200
pathX = 15; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
201 202 203
f o r i = −90:10:0 , pathWRWTG = i ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end
204 205 206 207 208
pathX = 21; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
209 210 211
pathY = 5 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
212 213 214
pathZ = 4 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
215 216 217 218
f o r i =0 : 10 :360 , pathX = 21 + ( 5 * sin ( i * deg2rad ) ) ; pathY = ( 5 * cos ( i * deg2rad ) ) ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end pathY = 0 ; pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
219 220 221 222 223 224 225 226 227
pathX = 15; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
228 229 230
f o r i =0: −10: −90 , pathWRWTG = i ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 0 . 4 ) ; end
231 232 233 234 235
pathX = 11; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
236 237 238
pathZ = 2 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
239 240 241
pathGripper = 0 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
242 243 244
pathZ = 8 ; UpdateArm( pathX , pathY , pathZ ,pathWRWTG, pathGripper , 2 ) ;
245 246 247 248
end
10031755
Queron Williams
19
B. MATLAB C ODE - SOLVE IK. M 1
2
function [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( xTarget , yTarget , zTarget , wrwtgTarget ) global rad2deg deg2rad L1 L2 L3 L4 IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b
3
IKtheta1 = atan2 ( yTarget , xTarget ) * rad2deg ; %wrwtg = 90+ theta2+theta3+theta4 ; wrwtg = wrwtgTarget ; z = zTarget −(L4 * sin ( wrwtg * deg2rad ) )−L1 ; XYhyp = s q r t ( ( xTarget ^2) +( yTarget ^2) ) −(L4 * cos ( wrwtg * deg2rad ) ) ;
4 5 6 7 8 9
IKcostheta3 = ( ( XYhyp^2) +( z ^2) −(L2^2) −(L3^2) ) / ( 2 * L2 * L3 ) ;
10 11
IKtheta3a = atan2 (+ s q r t (1 −( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ; IKtheta3b = atan2(− s q r t (1 −( IKcostheta3 ^2) ) , IKcostheta3 ) * rad2deg ;
12 13 14
IKcostheta2a = ( ( XYhyp * ( L2+(L3 * cos ( IKtheta3a * deg2rad ) ) ) ) +( z * L3 * sin ( IKtheta3a * deg2rad ) ) ) / ( ( XYhyp^2) +( z ^2) ) ; IKcostheta2b = ( ( XYhyp * ( L2+(L3 * cos ( IKtheta3b * deg2rad ) ) ) ) +( z * L3 * sin ( IKtheta3b * deg2rad ) ) ) / ( ( XYhyp^2) +( z ^2) ) ;
15
16
17
IKtheta2a = atan2 (+ s q r t (1 −( IKcostheta2a ^2) ) , IKcostheta2a ) * rad2deg ; IKtheta2b = atan2 (+ s q r t (1 −( IKcostheta2b ^2) ) , IKcostheta2b ) * rad2deg ;
18 19 20
IKtheta4a = wrwtg−( IKtheta2a+IKtheta3a ) −90; IKtheta4b = wrwtg−(IKtheta2b+IKtheta3b ) −90;
21 22 23
end
10031755
Queron Williams
20
C. MATLAB C ODE - U PDATE J OINT. M 1 2 3 4 5 6 7 8 9
function updatet ( j o i n t , t a r g e t , SpeedOrTime , SpeedOrTimeAmount ) p e r s i s t e n t f i r s t ; %check i f t h i s i s the f i r s t use p e r s i s t e n t arm ; i f isempty ( f i r s t ) delete ( i n s t r f i n d a l l ) ; %i f f i r s t use c l e a r any previous s e r i a l objects arm = s e r i a l ( ’COM13 ’ , ’ BaudRate ’ , 115200 , ’ Terminator ’ , ’CR/LF ’ ) ; fopen (arm) ; %open new s e r i a l port f i r s t = 1; end
10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26
switch j o i n t ; %depending on j o i n t , convert degrees to pulse width case 0 t a r g e t = round (map( t a r g e t , −90 ,90 ,500 ,2500) ) ; %checked case 1 t a r g e t = round (map( t a r g e t ,0 ,180 ,750 ,2250) ) ; %checked case 2 t a r g e t = round (map( t a r g e t , −180 ,0 ,2500 ,500) ) ; %checked case 3 t a r g e t = round (map( t a r g e t , −180 ,0 ,500 ,2500) ) ; %checked case 4 t a r g e t = round (map( t a r g e t , −90 ,90 ,500 ,2500) ) ; %checked case 5 t a r g e t = round (map( t a r g e t , −90 ,90 ,500 ,2500) ) ; %checked end %send pulse width to servo f p r i n t f (arm , s p r i n t f ( ’#%d P %d %c %d ’ , j o i n t , t a r g e t , SpeedOrTime , SpeedOrTimeAmount) )
27 28
end
D. MATLAB C ODE - U PDATE A RM . M 1 2 3
4 5 6 7 8 9 10 11
function UpdateArm( x , y , z , wrwtg , gripper , time ) global IKtheta1 IKtheta2a IKtheta3a IKtheta4a IKtheta2b IKtheta3b IKtheta4b ; [ IKtheta1 , IKtheta2a , IKtheta3a , IKtheta4a , IKtheta2b , IKtheta3b , IKtheta4b ] = solveIK ( x , y , z , wrwtg ) ; Updatet ( 0 , IKtheta1 , ’ t ’ , ( time * 1000) ) ; Updatet ( 1 , IKtheta2b , ’ t ’ , ( time * 1000) ) ; Updatet ( 2 , IKtheta3b , ’ t ’ , ( time * 1000) ) ; Updatet ( 3 , IKtheta4b , ’ t ’ , ( time * 1000) ) ; Updatet ( 4 , gripper , ’ t ’ ,500 ) ; Updatet ( 5 , 0 , ’ t ’ ,500 ) ; f p r i n t f ( ’ x= %.2 f \ t y= %.2 f \ t z= %.2 f \ t pitch= %.2 f \ t ’ , x , y , z , wrwtg ) ; f p r i n t f ( ’ 1= %.2 f \ t 2= %.2 f \ t 3= %.2 f \ t 4= %.2 f \n ’ , IKtheta1 , IKtheta2b , IKtheta3b , IKtheta4b ) ;
12
delay ( time ) ;
13 14
end
10031755
Queron Williams
21
E. MATLAB C ODE - GET _ DH _ MATRIX . M 1 2 3
4
5 6
function T = get_dh_matrix ( parameters ) T = [ ( cos ( parameters ( 1 , 4 ) ) ) , −( sin ( parameters ( 1 , 4 ) ) ) , 0 , ( parameters ( 1 , 1 ) ) ; ( sin ( parameters ( 1 , 4 ) ) ) * ( cos ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 4 ) ) ) * ( cos ( parameters ( 1 , 2 ) ) ) , −( sin ( parameters ( 1 , 2 ) ) ) , −( sin ( parameters ( 1 , 2 ) ) ) * ( parameters ( 1 , 3 ) ) ; ( sin ( parameters ( 1 , 4 ) ) ) * ( sin ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 4 ) ) ) * ( sin ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 2 ) ) ) , ( cos ( parameters ( 1 , 2 ) ) ) * ( parameters (1 ,3) ) ; 0 ,0 ,0 ,1]; end
F. MATLAB C ODE - MAP. M 1 2 3
function output = map( x , in_min , in_max , out_min , out_max ) output = ( x − in_min ) * ( out_max − out_min ) / ( in_max − in_min ) + out_min ; end
G. MATLAB C ODE - DELAY. M 1 2 3 4 5 6 7
function delay ( seconds ) % function pause the program % seconds = delay time in seconds tic ; while toc < seconds end end
10031755
Queron Williams
22
H. L OG FILE FOR COMPLEX TASK x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x=
11.00 11.00 11.00 11.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 21.00 21.00 21.00 21.00 21.87 22.71 23.50 24.21 24.83 25.33 25.70 25.92 26.00 25.92 25.70 25.33 24.83 24.21 23.50 22.71 21.87 21.00 20.13 19.29 18.50 17.79 17.17 16.67 16.30 16.08 16.00 16.08 16.30 16.67
10031755
y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y=
0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 5.00 5.00 5.00 4.92 4.70 4.33 3.83 3.21 2.50 1.71 0.87 0.00 -0.87 -1.71 -2.50 -3.21 -3.83 -4.33 -4.70 -4.92 -5.00 -4.92 -4.70 -4.33 -3.83 -3.21 -2.50 -1.71 -0.87 -0.00 0.87 1.71 2.50
z= 5.00 z= 2.00 z= 2.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 8.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00 z= 4.00
pitch= -90.00 1= 0.00 2= 86.56 3= -101.49 4= -165.07 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58 pitch= -90.00 1= 0.00 2= 79.07 3= -110.48 4= -148.58 pitch= -90.00 1= 0.00 2= 88.77 3= -87.70 4= -181.07 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52 pitch= -90.00 1= 0.00 2= 62.98 3= -56.47 4= -186.52 pitch= -80.00 1= 0.00 2= 73.09 3= -70.55 4= -172.53 pitch= -70.00 1= 0.00 2= 82.08 3= -83.26 4= -158.82 pitch= -60.00 1= 0.00 2= 90.06 3= -95.04 4= -145.01 pitch= -50.00 1= 0.00 2= 96.88 3= -106.05 4= -130.83 pitch= -40.00 1= 0.00 2= 102.21 3= -116.27 4= -115.94 pitch= -30.00 1= 0.00 2= 105.45 3= -125.54 4= -99.91 pitch= -20.00 1= 0.00 2= 105.66 3= -133.50 4= -82.16 pitch= -10.00 1= 0.00 2= 101.63 3= -139.51 4= -62.12 pitch= 0.00 1= 0.00 2= 92.57 3= -142.71 4= -39.86 pitch= 0.00 1= 0.00 2= 61.83 3= -103.54 4= -48.29 pitch= 0.00 1= 13.39 2= 59.07 3= -99.22 4= -49.85 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= 13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= 12.69 2= 37.67 3= -90.83 4= -36.84 pitch= 0.00 1= 11.69 2= 34.72 3= -84.56 4= -40.16 pitch= 0.00 1= 10.44 2= 31.83 3= -78.51 4= -43.32 pitch= 0.00 1= 8.99 2= 29.09 3= -72.84 4= -46.24 pitch= 0.00 1= 7.38 2= 26.59 3= -67.75 4= -48.84 pitch= 0.00 1= 5.64 2= 24.46 3= -63.43 4= -51.03 pitch= 0.00 1= 3.81 2= 22.82 3= -60.12 4= -52.70 pitch= 0.00 1= 1.92 2= 21.77 3= -58.03 4= -53.75 pitch= 0.00 1= 0.00 2= 21.41 3= -57.31 4= -54.10 pitch= 0.00 1= -1.92 2= 21.77 3= -58.03 4= -53.75 pitch= 0.00 1= -3.81 2= 22.82 3= -60.12 4= -52.70 pitch= 0.00 1= -5.64 2= 24.46 3= -63.43 4= -51.03 pitch= 0.00 1= -7.38 2= 26.59 3= -67.75 4= -48.84 pitch= 0.00 1= -8.99 2= 29.09 3= -72.84 4= -46.24 pitch= 0.00 1= -10.44 2= 31.83 3= -78.51 4= -43.32 pitch= 0.00 1= -11.69 2= 34.72 3= -84.56 4= -40.16 pitch= 0.00 1= -12.69 2= 37.67 3= -90.83 4= -36.84 pitch= 0.00 1= -13.39 2= 40.58 3= -97.16 4= -33.42 pitch= 0.00 1= -13.74 2= 43.38 3= -103.42 4= -29.96 pitch= 0.00 1= -13.69 2= 46.01 3= -109.47 4= -26.53 pitch= 0.00 1= -13.17 2= 48.37 3= -115.17 4= -23.21 pitch= 0.00 1= -12.15 2= 50.42 3= -120.36 4= -20.06 pitch= 0.00 1= -10.60 2= 52.10 3= -124.90 4= -17.20 pitch= 0.00 1= -8.53 2= 53.38 3= -128.63 4= -14.75 pitch= 0.00 1= -5.99 2= 54.26 3= -131.42 4= -12.84 pitch= 0.00 1= -3.09 2= 54.77 3= -133.14 4= -11.63 pitch= 0.00 1= -0.00 2= 54.94 3= -133.72 4= -11.21 pitch= 0.00 1= 3.09 2= 54.77 3= -133.14 4= -11.63 pitch= 0.00 1= 5.99 2= 54.26 3= -131.42 4= -12.84 pitch= 0.00 1= 8.53 2= 53.38 3= -128.63 4= -14.75
Queron Williams
23
x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= x= >>
17.17 17.79 18.50 19.29 20.13 21.00 21.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 15.00 11.00 11.00 11.00 11.00
y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y= y=
3.21 3.83 4.33 4.70 4.92 5.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00 0.00
z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z= z=
4.00 4.00 4.00 4.00 4.00 4.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 8.00 2.00 2.00 8.00
pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch= pitch=
0.00 1= 10.60 0.00 1= 12.15 0.00 1= 13.17 0.00 1= 13.69 0.00 1= 13.74 0.00 1= 13.39 0.00 1= 0.00 0.00 1= 0.00 0.00 1= 0.00 -10.00 1= 0.00 -20.00 1= 0.00 -30.00 1= 0.00 -40.00 1= 0.00 -50.00 1= 0.00 -60.00 1= 0.00 -70.00 1= 0.00 -80.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00 -90.00 1= 0.00
2= 52.10 3= -124.90 4= -17.20 2= 50.42 3= -120.36 4= -20.06 2= 48.37 3= -115.17 4= -23.21 2= 46.01 3= -109.47 4= -26.53 2= 43.38 3= -103.42 4= -29.96 2= 40.58 3= -97.16 4= -33.42 2= 61.83 3= -103.54 4= -48.29 2= 92.57 3= -142.71 4= -39.86 2= 92.57 3= -142.71 4= -39.86 2= 101.63 3= -139.51 4= -62.12 2= 105.66 3= -133.50 4= -82.16 2= 105.45 3= -125.54 4= -99.91 2= 102.21 3= -116.27 4= -115.94 2= 96.88 3= -106.05 4= -130.83 2= 90.06 3= -95.04 4= -145.01 2= 82.08 3= -83.26 4= -158.82 2= 73.09 3= -70.55 4= -172.53 2= 62.98 3= -56.47 4= -186.52 2= 88.77 3= -87.70 4= -181.07 2= 79.07 3= -110.48 4= -148.58 2= 79.07 3= -110.48 4= -148.58 2= 88.77 3= -87.70 4= -181.07
(a video can be seen at http://youtu.be/rd4mS6ZGti4)
10031755
Queron Williams
24
Related Documents 3m3m1z
Manipulator Kinematics 6n4l42
December 2019 45
Robot Manipulator 5p3c3c
January 2021 0
Setw Manipulator 1cp66
November 2019 31
Kinematics Solutions.pdf 515x26
June 2021 0
Kinematics Formulas 371g55
August 2021 0
Kinematics Bansal u4771
July 2020 0More Documents from "Queron Williams" 411e2r
Manipulator Kinematics 6n4l42
December 2019 45
Micromouse Final Report 676940
October 2021 0
Preguntas Poderosas 180 3f2n3g
April 2020 41
6l6m1j
December 2019 119
El Lenguaje Del Petroleo b721j
December 2019 96