Question

In: Computer Science

Program a function to compute the forward kinematic matrix based on the robot’s DH parameters. You...

Program a function to compute the forward kinematic matrix based on the robot’s DH parameters. You should have the following function:

A=FK(DH)

DH contains all the DH parameters

Solutions

Expert Solution

​The objective of forward kinematic analysis is to determine the cumulative
effect of the entire set of joint variables.


DH Parameters

Link lengths
a = [0, 650, 0, 0, 0, 0]

Link offsets
d = [0, 190, 0, 600, 0, 125]

Link twist angle
alpha = [-pi/2, 0, pi/2, -pi/2, pi/2, 0]

So basically I finding the T transformation matrix for each link from the the base frame {B} to wrist frame {W}. This is my code;

Function to compute Forward Kinematics:

def forwardK(q):

#T06 is the location of Wrist frame, {W}, relative to Base frame, {B}
T01 = genT(q[0],0,d[0],0)
T12 = genT(q[1],a[0],d[1],alpha[0])
T23 = genT(q[2],a[1],d[2],alpha[1])
T34 = genT(q[3],a[2],d[3],alpha[2])
T45 = genT(q[4],a[3],d[4],alpha[3])
T56 = genT(q[5],a[4],d[5],alpha[4])

#Tool frame {T}
#T67 = genT(0,0,d[5],0)

T03 = matmul(T01,T12,T23)
T36 = matmul(T34,T45,T56)
T06 = matmul(T03,T36)
#T07 = matmul(T06,T67)

x = T[0][3]
y = T[1][3]
z = T[2][3]

print("X: ",x)
print("Y: ",y)
print("Z: ",z,"\n")

print("T: ",T,"\n")

return T06

The function to compute T Matrix

def genT(theta, a, d, alpha):
T = array([[cos(theta), (-sin(theta)), 0, a],
[sin(theta)*cos(alpha), (cos(theta)*cos(alpha)), -sin(alpha), (- d*sin(alpha))],
[sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
[0, 0, 0, 1]])

return T

from the T Matrix relating the {B} frame to the {W} frame position vector of the {w} [x y z] is extracted. R Matrix (orientation) of the {W} relative to the {B} is obtained by the following piece of code;

T = forwardK([30,-110,-30,0,0,0])
x = T[0][3]
y = T[1][3]
z = T[2][3]
R = T[0:3,0:3]

Where T is the transformation matrix relating {W} to {B}


Related Solutions

Write a MATLAB function function = myMatrixInveesion(....) to calculate matrix inversion by implementing LU decomposition, forward...
Write a MATLAB function function = myMatrixInveesion(....) to calculate matrix inversion by implementing LU decomposition, forward and backward substitution procedures. Do NOT use the built-in "lu" or "inv" commands in your code. You will need to employ Nested Loops. Thank you! function_____ = myMatrixInversion(_____)
Write a program that contains the following Write a function copies with two int parameters, named...
Write a program that contains the following Write a function copies with two int parameters, named n and x. It should dynamically allocate a new array of size n.  The function should then copy the value in x to every position in the array. The function should return a pointer to the new array. In the main function, call the copies function from part 1. with size 5 and value -1.  Output the resulting array to the screen, and deallocate the array....
Help with a c Program to compute the car insurance premium for a person based on...
Help with a c Program to compute the car insurance premium for a person based on their age and the number of tickets they have received. The following table explains how to perform the ticket computation: If Person's Age Is Then Insurance Cost Is Less than 21 $1500 + $250 x number of tickets From 21 through 24 $1200 + $250 x number of tickets 25 or older $1000 + $200 x number of tickets Print the person’s age, number...
Write a Java program to compute the income after tax of an employee based on the...
Write a Java program to compute the income after tax of an employee based on the following rule of tax rate. Assuming the salary is $22000, display the tax and the income after tax. 12% if salary ≥ 25,000 5% if salary < 10,000 Otherwise 8% will be applied Write a Java program to compute and display the sum of the numbers that can be both divisible by 6 and 8 from 1 to 500. Suppose there is a list...
Write a program to compute the root of the function f(x) = x3 + 2 x2...
Write a program to compute the root of the function f(x) = x3 + 2 x2 + 10 x - 20 by Newton method ( x0 =2 ). Stop computation when the successive values differ by not more than 0.5 * 10-5 . Evaluate f(x) and f '(x) using nested multiplication. The output should contain: (1) A table showing at each step the value of the root , the value of the function,and the error based upon successive approximation values...
Matlab You will write a function to calculate the determinant of a matrix. It should work...
Matlab You will write a function to calculate the determinant of a matrix. It should work for any size matrix. Remember that the determinant can be calculated by multiplying the diagonal elements of an upper right triangular matrix. Your function will take a matrix passed to it and put it in upper right triangular form. You will work down the diagonal beginning at row 1 column 1, then row 2 column 2, etc. Note that the row and column numbers...
What is an implementation matrix in a program planing plan? Can you give an example of...
What is an implementation matrix in a program planing plan? Can you give an example of a implementation matrix for obesity, etc?
Hedging currency risk with a forward. Assume you are a US-based speculator and you want to...
Hedging currency risk with a forward. Assume you are a US-based speculator and you want to bet on the UK 10 year zero nominal bond. Assume you buy the UK 10-year bond, but also need to hedge out your currency exposure (i.e. you do not want exposure to changes in the USD vs. GBP fx rate). You decide to hedge your fx exposure by selling GBP 10 years forward. You now how two legs to your trade: buy UK 10-year...
Which ways can you change the values of parameters inside the function in C programming?
Which ways can you change the values of parameters inside the function in C programming?
Explain what forward genetics and reverse genetics are? Assume you have known the function of the...
Explain what forward genetics and reverse genetics are? Assume you have known the function of the gene A via genetic analysis, design some experiments using both forward and reverse genetics approaches to identify the gene X that may work in the same pathway as the gene A.
ADVERTISEMENT
ADVERTISEMENT
ADVERTISEMENT