def get_and_store_rot_matrix(fname):
with open(fname) as rot_pickle:
ROT_EE = pickle.load(rot_pickle)
return ROT_EE[0]
ROT_X = Matrix([
[1, 0, 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]
])
ROT_Y = Matrix([
[cos(p), 0, sin(p)],
[0, 1, 0],
[-sin(p), 0, cos(p)]
])
ROT_Z = Matrix([
[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[0, 0, 1]
])
ROT_EE = ROT_Z * ROT_Y * ROT_X
Rot_Error = ROT_Z.subs(y, radians(180)) * ROT_Y.subs(p, radians(-90))
ROT_EE = ROT_EE * Rot_Error
with open(fname, 'w') as rot_pickle:
pickle.dump(ROT_EE, rot_pickle)
return ROT_EE
def rotate_point(x, y, degrees):
radians = math.radians(degrees)
cos_theta = math.cos(radians)
sin_theta = math.sin(radians)
return x * cos_theta - y * sin_theta, \
x * sin_theta + y * cos_theta
def distribution(self, theta, pkg=np):
"""Plasma distribution theta in degrees
Args:
theta (float or np.array or sp.Symbol): the angle(s) in degrees.
pkg (module, optional): Module to use in the funciton. If sp, as
sympy object will be returned. If np, a np.array or a float
will be returned. Defaults to np.
Returns:
(float, float) or (sympy.Add, sympy.Mul) or
(numpy.array, numpy.array): The R and Z coordinates of the
point with angle theta
"""
if pkg == np:
theta = np.radians(theta)
else:
theta = mpmath.radians(theta)
R = self.major_radius + self.minor_radius * pkg.cos(
theta + self.triangularity * pkg.sin(theta)
)
Z = (
self.elongation * self.minor_radius * pkg.sin(theta)
+ self.vertical_displacement
)
return R, Z
def rot_matrix():
r, p, y = symbols('r p y')
rot_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
rot_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
rot_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
rot_ee = rot_z * rot_y * rot_x
rot_z_adj = rot_z.subs(y, radians(180))
rot_y_adj = rot_y.subs(p, radians(-90))
rot_error = rot_z_adj * rot_y_adj
rot_ee = rot_ee * rot_error
return rot_ee
def read_dial(config, idx, img):
offset, clockwise = config
offset_r = offset * (np.pi / 180)
height, width = img.shape[:2]
center = [width / 2, height / 2]
radius = int(width / 2)
circle = sp.Circle(sp.Point(center), radius)
offset_ray = sp.Ray(sp.Point(center), angle=mp.radians(offset))
offset_img = img.copy()
origin_point = [center[0], 0]
offset_point = [
math.cos(offset_r) * (origin_point[0] - center[0]) -
math.sin(offset_r) * (origin_point[1] - center[1]) + center[0],
math.sin(offset_r) * (origin_point[0] - center[0]) +
math.cos(offset_r) * (origin_point[1] - center[1]) + center[1]
]
cv2.line(offset_img, (int(center[0]), int(center[1])),
(int(offset_point[0]), int(offset_point[1])), (0, 255, 0), 2)
write_debug(offset_img, f"dial-{idx}")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
blurred = cv2.GaussianBlur(gray, (5, 5), 0)
write_debug(blurred, f"blurred-{idx}")
edges = cv2.Canny(blurred, 50, 200)
write_debug(edges, f"edges-{idx}")
edge = find_hand_edge(edges)
hand_edge_img = img.copy()
cv2.line(hand_edge_img, (edge[0], edge[1]), (edge[2], edge[3]),
(0, 255, 0), 2)
write_debug(hand_edge_img, f"hand-edge-{idx}")
hand_ray = generate_hand_ray(center, edge)
circle_intersection = hand_ray.intersection(circle)[0]
cv2.line(img, (int(center[0]), int(center[1])),
(int(circle_intersection.x), int(circle_intersection.y)),
(0, 0, 255), 2)
write_debug(img, f"intersection-{idx}")
angle_r = math.atan2(circle_intersection.y - center[1],
circle_intersection.x - center[0]) - math.atan2(
origin_point[1] - center[1],
origin_point[0] - center[0])
angle = angle_r * 180 / np.pi
if angle < 0:
angle = 360 + angle
angle_p = angle / 360
if not clockwise:
angle_p = 1 - angle_p
return int(10 * angle_p)
def get_rotation_matrix():
r, p, y = symbols('r p y')
R_x = Matrix([[1, 0, 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]])
R_y = Matrix([[cos(p), 0, sin(p)],
[0, 1, 0],
[-sin(p), 0, cos(p)]])
R_z = Matrix([[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[0, 0, 1]])
R_E = R_z * R_y * R_x
# Compensate for rotation discrepancy between DH parameters and Gazebo
R_error_correction = R_z.subs(y, radians(180)) * R_y.subs(p, radians(-90))
R_E = R_E * R_error_correction
return R_E
def convertToRadians(expr):
newExpr = None
if trigUnit == TrigUnit.Degrees:
newExpr = mpmath.radians(expr)
elif trigUnit == TrigUnit.Gradients:
newExpr = expr * mpmath.pi / 200
else:
newExpr = expr
return newExpr
def calc_edge_compass_angle(edge):
north_ray = sympy.Ray2D((0, 0), (0, 1))
# 反向算进入edge，直观。
edge_ray = sympy.Ray(*edge.getShape()[:2][::-1])
angle = north_ray.closing_angle(edge_ray)
angle = (angle + 2 * sympy.pi) % (2 * sympy.pi)
angle_degrees = float(degrees(angle))
angle_radians = float(radians(degrees(angle)))
edge._angle_degrees = round(angle_degrees, 4)
edge._angle_radians = round(angle_radians, 4)
return angle_degrees, angle_radians
def __init__(self, a, c, b, thetaB, g, tiltS, rot_c, LF):
self.a = a
self.c = c
self.b = b
self.thetaB = thetaB
self.g = Matrix([g[0], g[1], g[2]]) # g vector h, k, l coordinates
self.tiltS = tiltS # tilt of sample around [100]
self.rot_c = rot_c # rotation of crystal from [100] being along the tilt axis
self.description = 'calculate screw beta for a given configuration'
# define required relationships between reference frames
# define required relationships between reference frames
self.LF = LF
# sample is tilted by tiltS wrt to the lab frame
self.SF = sampleFrame(radians(tiltS), self.LF)
# crystal is rotated by rot_c wrt to sample frame
self.CF = crystalFrame(radians(rot_c), 0., self.SF)
# displacement frame is rotated by rot_b wrt crystal frame
self.DF = self.CF # displacementFrame is the same as crystalFrame for a screw dislocation
# the incident beam frame is rotated 180 around x wrt to the lab frame
self.RincF = incidentBeamFrame(self.LF)
def __init__(self, a, c, b, rot_b, nu, thetaB, g, tiltS, rot_c, LF):
self.a = a
self.c = c
self.b = b # b = a [100] in crystal frame
self.rot_b = rot_b # rotation of burger vector around dislocation line
self.nu = nu
self.thetaB = thetaB
self.g = Matrix([g[0], g[1], g[2]]) # g vector h, k, l coordinates
self.tiltS = tiltS # tilt of sample around [100]
self.rot_c = rot_c # rotation of crystal from [100] being along the tilt axis
self.description = 'calculate edge beta for a given configuration'
# define required relationships between reference frames
self.LF = LF
# sample is tilted by tiltS wrt to the lab frame
self.SF = sampleFrame(radians(tiltS), self.LF)
# crystal is rotated by rot_c wrt to sample frame
self.CF = crystalFrame(radians(rot_c), 0., self.SF)
# displacement frame is rotated by rot_b wrt crystal frame
self.DF = displacementFrame(radians(rot_b), self.CF)
# the incident beam frame is rotated 180 around x wrt to the lab frame
self.RincF = incidentBeamFrame(self.LF)
#use the inverse of pi as a constant for computational optimisation
self.ipi = 1. / pi
def rotate_tcp(self, angle, angle_in_degrees=False, axis='i', save=True):
if not (axis == 'i' or axis == 'j' or axis == 'k'):
raise Exception(
"not the right axis (i,j or k) selected: {}".format(axis))
if angle_in_degrees:
angle = radians(angle)
if axis == 'i':
self.Tcp = self.Tcp.orient_new_axis('Tcp', angle, self.Tcp.i)
elif axis == 'j':
self.Tcp = self.Tcp.orient_new_axis('Tcp', angle, self.Tcp.j)
elif axis == 'k':
self.Tcp = self.Tcp.orient_new_axis('Tcp', angle, self.Tcp.k)
if save:
self.save_tcp()
def m_rot(axis, variable, use_degrees=True):
"""
Defines a rotation matrix in Denavit-Hartenberg notation.
:param axis: one of Cartesian coordinate system axes (x, y, z)
:param variable: variable defining rotation
:param use_degrees: uses degrees if True, uses rads otherwise
:type axis: char
:type variable: numbers.Number or sympy.Symbol
:type use_degrees: bool
:return: evaluated matrix
:rtype: object inheriting from sym.matrices.MatrixBase
"""
# check if axis provided is correct
ax = axis.lower()
assert ax in ['x', 'y', 'z'], "Invalid axis provided."
# checks if variable is a Symbol or a number
assert len([t for t in [numbers.Number, sym.Symbol, sym.Mul] if
isinstance(variable, t)]) == 1, "'variable' should be a sympy.Symbol, a number or an expression."
# convert to degrees if set to true
if not isinstance(variable, sym.Symbol) and use_degrees:
var = mp.radians(variable)
else:
var = variable
return sym.N(sym.Matrix([
[
{'x': 1}.get(ax, sym.cos(var)),
{'z': -sym.sin(var)}.get(ax, 0),
{'y': sym.sin(var)}.get(ax, 0),
0],
[
{'z': sym.sin(var)}.get(ax, 0),
{'y': 1}.get(ax, sym.cos(var)),
{'x': -sym.sin(var)}.get(ax, 0),
0],
[
{'y': -sym.sin(var)}.get(ax, 0),
{'x': sym.sin(var)}.get(ax, 0),
{'z': 1}.get(ax, sym.cos(var)),
0],
[0, 0, 0, 1, ]
]), 5, chop=True)
def rotate_point_around_anchor(self, x, y, anchor_x, anchor_y, angle):
anglefrac = angle.as_integer_ratio()
radian_qty = radians(anglefrac[0] / anglefrac[1])
old_x = x - anchor_x
old_y = y - anchor_y
coord = matrix([float(old_x), float(old_y)])
transform = matrix([[cos(radian_qty), -sin(radian_qty)], [sin(radian_qty), cos(radian_qty)]])
result = transform * coord
new_x = Decimal(str(result[0]))
new_y = Decimal(str(result[1]))
new_x += anchor_x
new_y += anchor_y
return new_x, new_y
def move(headX, faceWidth):
#Rotate robot
moveTime = abs(6000 - headX) / ANGULAR_VELOCITY
if headX < 6000:
moveRight(moveTime)
elif headX > 6000:
moveLeft(moveTime)
#Move robot
dist = mpmath.cot(mpmath.radians(62.2 * (w / 640) / 2))
moveTime = abs(dist - faceDistTarget) * VELOCITY
if dist < faceDistTarget:
moveBack(moveTime)
elif dist > faceDistTarget:
moveForward(moveTime)
client.sendData("Are you Sarah Connor?")
def p_expression_nd(self, p):
'''expression : SIN LPAREN expression RPAREN
| COS LPAREN expression RPAREN
| TAN LPAREN expression RPAREN
| ABS LPAREN expression RPAREN
| SQRT LPAREN expression RPAREN
| RAD LPAREN expression RPAREN
| ID LPAREN funcargs RPAREN'''
# p[1] = symbols('f g h', cls=Function)
p[0] = str(p[1]) + str(p[2]) + str(p[3]) + str(p[4])
if p[1] == 'sin':
p[0] = sin(p[3])
elif p[1] == 'cos':
p[0] = cos(p[3])
elif p[1] == 'tan':
p[0] = tan(p[3])
elif p[1] == 'abs':
p[0] = Abs(p[3])
elif p[1] == 'sqrt':
p[0] = sqrt(p[3])
elif p[1] == 'rad':
p[0] = mpmath.radians(p[3])
else:
p[0] = Function(p[1])(*p[3])
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self,EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self,EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self,position,orientation):
self.position = position
self.orientation = orientation
comb = Combine(position,orientation)
class Pose:
def __init__(self,comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
# Twist Angles
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
# Link Lengths
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
# Link Offsets
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
# Joint Angles
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
dh_table = {
alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi / 2., a1: 0.35, d2: 0, q2: q2 - (pi / 2.),
alpha2: 0., a2: 1.25, d3: 0, q3: q3,
alpha3: -pi / 2., a3: -0.054, d4: 1.5, q4: q4,
alpha4: pi / 2., a4: 0, d5: 0, q5: q5,
alpha5: -pi / 2., a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7:0.303, q7: 0,
}
def tf_matrix(alpha, a, d, theta):
TF = Matrix([
[ cos(theta), -sin(theta), 0, a],
[sin(theta) * cos(alpha), cos(theta) * cos(alpha), -sin(alpha), -sin(alpha) * d],
[sin(theta) * sin(alpha), cos(theta) * sin(alpha), cos(alpha), cos(alpha) * d],
[ 0, 0, 0, 1]
])
return TF
t0_1 = tf_matrix(alpha0, a0, d1, q1).subs(dh_table)
t1_2 = tf_matrix(alpha1, a1, d2, q2).subs(dh_table)
t2_3 = tf_matrix(alpha2, a2, d3, q3).subs(dh_table)
t3_4 = tf_matrix(alpha3, a3, d4, q4).subs(dh_table)
t4_5 = tf_matrix(alpha4, a4, d5, q5).subs(dh_table)
t5_6 = tf_matrix(alpha5, a5, d6, q6).subs(dh_table)
t6_ee = tf_matrix(alpha6, a6, d7, q7).subs(dh_table)
t0_ee = t0_1 * t1_2 * t2_3 * t3_4 * t4_5 * t5_6 * t6_ee
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x,
req.poses[x].orientation.y,
req.poses[x].orientation.z,
req.poses[x].orientation.w]
)
r, p, y = symbols('r p y')
ROT_x = Matrix([
[1, 0, 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]
])
ROT_y = Matrix([
[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[-sin(p), 0, cos(p)]
])
ROT_z = Matrix([
[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[ 0, 0, 1]
])
rot_ee = ROT_z * ROT_y * ROT_x
Rot_Corr = ROT_z.subs(y, radians(180))*ROT_y.subs(p, radians(-90))
rot_ee = rot_ee * Rot_Corr
rot_ee = rot_ee.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([
[px],
[py],
[pz]
])
WC = EE - (0.303) * rot_ee[:,2]
theta1 = atan2(WC[1], WC[0])
l1_horiz = dh_table[a1]
l1_z = dh_table[d1]
wc_x = WC[0]
wc_y = WC[1]
wc_horiz = sqrt(wc_x * wc_x + wc_y * wc_y) - l1_horiz
wc_z = WC[2] - l1_z
l2_len = dh_table[a2] # 1.25
l3_x = dh_table[d4] # 1.5
l3_y = dh_table[a3] #-0.054
l3_len = sqrt(l3_x * l3_x + l3_y * l3_y)
l3_theta_offset = atan2(l3_y, l3_x)
A = l3_len
B = sqrt(wc_horiz * wc_horiz + wc_z * wc_z)
C = l2_len
a = acos((B * B + C * C - A * A) / (2 * B * C))
b = acos((A * A + C * C - B * B) / (2 * A * C))
c = acos((A * A + B * B - C * C) / (2 * A * B))
d = atan2(wc_z, wc_horiz)
theta2 = (pi / 2) - (a + d)
theta3 = (pi / 2) - (b + l3_theta_offset)
r0_3 = t0_1[0:3, 0:3] * t1_2[0:3, 0:3] * t2_3[0:3, 0:3]
r0_3 = r0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
r3_6 = r0_3.inv("LU") * rot_ee
theta4 = atan2(r3_6[2, 2], -r3_6[0, 2])
theta5 = atan2(sqrt(r3_6[0,2]*r3_6[0,2] + r3_6[2,2]*r3_6[2,2]), r3_6[1,2])
theta6 = atan2(-r3_6[1,1], r3_6[1,0])
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = t0_ee.evalf(subs={q1: theta1, q2: theta2, q3: theta3, q4: theta4, q5: theta5, q6: theta6})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1], WC[2]] # <--- Load your calculated WC values in this array
your_ee = [FK[0,3], FK[1,3], FK[2,3]] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print ("\nTotal run time to calculate joint angles from pose is %04.4f seconds" % (time()-start_time))
# Find WC error
if not(sum(your_wc)==3):
wc_x_e = abs(your_wc[0]-test_case[1][0])
wc_y_e = abs(your_wc[1]-test_case[1][1])
wc_z_e = abs(your_wc[2]-test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print ("\nWrist error for x position is: %04.8f" % wc_x_e)
print ("Wrist error for y position is: %04.8f" % wc_y_e)
print ("Wrist error for z position is: %04.8f" % wc_z_e)
print ("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1-test_case[2][0])
t_2_e = abs(theta2-test_case[2][1])
t_3_e = abs(theta3-test_case[2][2])
t_4_e = abs(theta4-test_case[2][3])
t_5_e = abs(theta5-test_case[2][4])
t_6_e = abs(theta6-test_case[2][5])
print ("\nTheta 1 error is: %04.8f" % t_1_e)
print ("Theta 2 error is: %04.8f" % t_2_e)
print ("Theta 3 error is: %04.8f" % t_3_e)
print ("Theta 4 error is: %04.8f" % t_4_e)
print ("Theta 5 error is: %04.8f" % t_5_e)
print ("Theta 6 error is: %04.8f" % t_6_e)
print ("\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print (" ")
# Find FK EE error
if not(sum(your_ee)==3):
ee_x_e = abs(your_ee[0]-test_case[0][0][0])
ee_y_e = abs(your_ee[1]-test_case[0][0][1])
ee_z_e = abs(your_ee[2]-test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print ("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print ("End effector error for y position is: %04.8f" % ee_y_e)
print ("End effector error for z position is: %04.8f" % ee_z_e)
print ("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self,EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self,EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self,position,orientation):
self.position = position
self.orientation = orientation
comb = Combine(position,orientation)
class Pose:
def __init__(self,comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
## Insert IK code here! starting at: Define DH parameter symbols
# Define DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7') # twist angles
# Joint angle symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Para mas informacion debo buscar la seccion KR210 Forward Kinematics
# Modified DH params
DH_Table = { alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2., a1: 0.35, d2: 0, q2: -pi/2. + q2,
alpha2: 0, a2: 1,25, d3: 0, q3: q3,
alpha3: -pi/2., a3: -0.054, d4: 1.5, q4: q4,
alpha4: pi/2., a4: 0, d5: 0, q5: q5,
alpha5: -pi/2., a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
# Define Modified DH Transformation matrix
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[ cos(q), -sin(q), 0, a],
[sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[sin(q)*sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]])
return TF
# Create individual transformation matrices
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_E = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_E
# Extract end-effector position and orientation from request
# px, py, pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w)])
# Find EE rotation matrix
# Define RPY rotation matrices
# http://planning.cs.uiuc.edu/node102.html : Muy importante explica las matrices siguientes.
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]]) # ROLL
ROT_y = Matrix([[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[-sin(p), 0, cos(p)]]) # PITCH
ROT_z = Matrix([[ cos(y), -sin(y),0],
[ sin(y), cos(y),0],
[ 0, 0,1]]) # YAW
ROT_EE = ROT_z * ROT_y * ROT_x
# More information can be found in KR210 Forward Kinematics section
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
# To align our DH parameters with that of the you RDF file
# once we have a rotation matrix to the end-effector.
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px],
[py],
[pz]])
WC = EE - (0.303) * ROT_EE[:,2]
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta_i
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# Create Modified DH parameters
DH_table = {
alpha0: 0,
a0: 0,
d1: 0.75,
alpha1: radians(-90),
a1: 0.35,
d2: 0,
q2: q2 - radians(90),
alpha2: 0,
a2: 1.25,
d3: 0,
alpha3: radians(-90),
a3: -0.054,
d4: 1.50,
alpha4: radians(90),
a4: 0,
d5: 0,
alpha5: radians(-90),
a5: 0,
d6: 0,
alpha6: 0,
a6: 0,
d7: 0.303
}
# Define Modified DH Transformation matrix
def DH_TF_Matrix(alpha, a, d, q):
T = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return T
# Create individual transformation matrices
T0_1 = DH_TF_Matrix(alpha0, a0, d1, q1).subs(DH_table)
T1_2 = DH_TF_Matrix(alpha1, a1, d2, q2).subs(DH_table)
T2_3 = DH_TF_Matrix(alpha2, a2, d3, q3).subs(DH_table)
T3_4 = DH_TF_Matrix(alpha3, a3, d4, q4).subs(DH_table)
T4_5 = DH_TF_Matrix(alpha4, a4, d5, q5).subs(DH_table)
T5_6 = DH_TF_Matrix(alpha5, a5, d6, q6).subs(DH_table)
T6_EE = DH_TF_Matrix(alpha6, a6, d7, q7).subs(DH_table)
T0_2 = (T0_1 * T1_2)
T0_3 = (T0_2 * T2_3)
T0_4 = (T0_3 * T3_4)
T0_5 = (T0_4 * T4_5)
T0_6 = (T0_5 * T5_6)
T0_EE = (T0_6 * T6_EE)
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Correction Needed to Account of Orientation Difference Between Definition of
# Gripper Link in URDF versus DH Convention
def rot_x(q):
R_x = Matrix([[1, 0, 0], [0, cos(q), -sin(q)], [0, sin(q), cos(q)]])
return R_x
def rot_y(q):
R_y = Matrix([[cos(q), 0, sin(q)], [0, 1, 0], [-sin(q), 0, cos(q)]])
return R_y
def rot_z(q):
R_z = Matrix([[cos(q), -sin(q), 0], [sin(q), cos(q), 0], [0, 0, 1]])
return R_z
r, p, y = symbols('r p y')
R_corr = rot_z(y).subs(y, radians(180)) * rot_y(p).subs(p, radians(-90))
R_EE = rot_z(y) * rot_y(p) * rot_x(r)
R_EE = R_EE * R_corr
R_EE = R_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - DH_table[d7] * R_EE[:, 2]
# Calculate joint angles using Geometric IK method
theta1 = atan2(WC[1], WC[0])
# SSS triangle for theta2 and theta3
side_a = sqrt(DH_table[d4] * DH_table[d4] + DH_table[a3] * DH_table[a3])
r = sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - DH_table[a1]
s = WC[2] - DH_table[d1]
side_b = sqrt(r * r + s * s)
side_c = DH_table[a2]
angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos((side_c * side_c + side_a * side_a - side_b * side_b) /
(2 * side_c * side_a))
theta2 = radians(90) - angle_a - atan2(s, r)
theta3 = radians(90) - angle_b - atan2(DH_table[a3], DH_table[d4])
# Inverse Orientation
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.transpose() * R_EE
# Euler angles from rotation matrix
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2])
# Choosing between multiple possible solutions:
if sin(theta5) < 0:
theta4 = atan2(-R3_6[2, 2], R3_6[0, 2])
theta6 = atan2(R3_6[1, 1], -R3_6[1, 0])
else:
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
"""
This is the set up code, Don't modify.
"""
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
# DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') # twist angle
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # Joint angle
# Create Modified DH parameters (KR210 Forward kinematics section)
DH_Table = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: rad(-90),
a1: 0.35,
d2: 0,
q2: q2 - rad(90),
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: rad(-90),
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: rad(90),
a4: 0,
d5: 0,
q5: q5,
alpha5: rad(-90),
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Create Individual Transformation matrices for each joint transition
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Find EE rotation matrix
# Define RPY rotation matrices
# http://planning.cs.uiuc.edu/node102.html
r, p, y = symbols('r p y') # Gyroscope symbols - Roll, Pitch and Yaw
ROT_x = rot_x(r)
ROT_y = rot_y(p)
ROT_z = rot_z(y)
ROT_EE = ROT_z * ROT_y * ROT_x
# KR210 Forward Kinematics section for future details
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - 0.303 * ROT_EE[:, 2]
# Calculate joint angles using Geometric IK method.
# More information can be found in the Inverse Kinematics with Kuka KR210
WC1_0_dist = sqrt(WC[0] * WC[0] + WC[1] * WC[1])
theta1 = atan2(WC[1], WC[0])
# SSS triangle for theta2 and theta3
side_a = 1.501
side_b = sqrt(pow((WC1_0_dist - 0.35), 2) + pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos(((side_b * side_b) + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos(((side_a * side_a) + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
angle_c = acos(((side_a * side_a) + side_b * side_b - side_c * side_c) /
(2 * side_a * side_b))
theta2 = pi / 2 - angle_a - atan2(WC[2] - 0.75, WC1_0_dist - 0.35)
theta3 = pi / 2 - (angle_b + 0.036)
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * ROT_EE
# Euler angles from rotation matrix
# More information can be found in the Euler Angles from a Rotation Matrix section
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
# Forward Kinematics
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
# End of forward kinematics
########################################################################################
# For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # Load your calculated WC values in this array
your_ee = FK[:3,
3] # Load your calculated end effector value from your forward kinematics
# Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(POS):
## Set up code
## Do not modify!
# x = 0
########################################################################################
##
## Insert IK code here!
theta1 = 0
theta2 = 0
theta3 = 0
theta4 = 0
theta5 = 0
theta6 = 0
### Your FK code here
# Create symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6, = symbols(
'alpha0:7')
#
#
# Create Modified DH parameters
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: -pi / 2. + q2,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
#
#
# Create individual transformation matrices
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return TF
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(s)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Define RPY rotation matices
px, py, pz, roll, pitch, yaw = symbols('px, py, pz, r, p, y')
ROT_x = Matrix([[1, 0, 0], [0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]]) # roll
ROT_y = Matrix([[cos(pitch), 0, sin(pitch)], [0, 1, 0],
[-sin(pitch), 0, cos(pitch)]]) # pitch
ROT_z = Matrix([[cos(yaw), -sin(yaw), 0], [sin(yaw), cos(yaw), 0],
[0, 0, 1]]) # yaw
ROT_EE = ROT_z * ROT_y * ROT_x
ROT_corr = ROT_EE * ROT_z.subs(yaw, radians(180)) * ROT_y.subs(
pitch, radians(-90))
EE = Matrix([[px], [py], [pz]])
WC = EE - 0.303 * ROT_EE[:, 2] # d7 = 0.303
theta1 = atan2(WC[1], WC[0])
side_a = 1.501 # d4.subs(s)
side_b = simplify(
sqrt((sqrt(WC[0]**2 + WC[1]**2) - 0.35)**2 + (WC[2] - 0.75)**2))
side_c = 1.25 # a2.subs(s)
#angle_a = simplify(acos((side_b**2 + side_c**2 - side_a**2)/(2 * side_b * side_c)))
#angle_b = simplify(acos((side_a**2 + side_c**2 - side_b**2)/(2 * side_a * side_c)))
#angle_c = simplify(acos((side_a**2 + side_b**2 - side_c**2)/(2 * side_a * side_b)))
#theta2 = simplify(pi/2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0]**2 + WC[1]**2) - 0.35))
#theta3 = simplify(pi/2 - (angle_b + 0.036))
T0_4 = T0_1 * T1_2 * T2_3 * T3_4
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
# R0_3_inv = simplify(R0_3.inv("LU"))
# R3_6 = simplify(R0_3.inv("LU") * ROT_EE)
# Extract end-effector position and orientation from request
# px, py, pz = end-effector position
# roll, pitch, yaw = end-effectro orientation
for x in range(len(POS)):
start_time = time()
pos = {
px: POS[x][0],
py: POS[x][1],
pz: POS[x][2],
roll: POS[x][3],
pitch: POS[x][4],
yaw: POS[x][5]
}
print "No. ", x, ", POS =", px, py, pz, roll, pitch, yaw
#EE = Matrix([[px], [py], [pz]])
#WC = EE - 0.303 * ROT_EE[:,2] # d7 = 0.303
EE = EE.subs(pos)
WC = WC.subs(pos)
#
# Calculate joint angles using Geometric IK method
# for JT1, JT2, JT3
side_b = side_b.subs(
pos
) # (sqrt((sqrt(WC[0]**2 + WC[1]**2) - 0.35)**2 + (WC[2] - 0.75)**2))
angle_a = (acos(
(side_b**2 + side_c**2 - side_a**2) / (2 * side_b * side_c)))
angle_b = (acos(
(side_a**2 + side_c**2 - side_b**2) / (2 * side_a * side_c)))
angle_c = (acos(
(side_a**2 + side_b**2 - side_c**2) / (2 * side_a * side_b)))
theta1 = theta1.subs(pos)
theta2 = (pi / 2 - angle_a - atan2(WC[2] - 0.75,
sqrt(WC[0]**2 + WC[1]**2) - 0.35))
theta3 = (pi / 2 - (angle_b + 0.036))
# for JT4, JT5, JT6
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R0_3_inv = simplify(R0_3.inv("LU"))
R3_6 = R0_3_inv * ROT_EE.subs(pos)
r13 = R3_6[0, 2]
r33 = R3_6[2, 2]
r23 = R3_6[1, 2]
r21 = R3_6[1, 0]
r22 = R3_6[1, 1]
r12 = R3_6[0, 1]
r32 = R3_6[2, 1]
theta5 = (atan2(sqrt(r13**2 + r33**2), r23)).evalf()
if (sin(theta5) < 0):
# print("BELOW!!!")
theta4 = (atan2(-r33, r13)).evalf()
theta6 = (atan2(r22, -r21)).evalf()
elif (theta5 == 0):
# print("EQUAL!!!")
theta4 = 0
theta6 = (atan2(-r12, -r32)).evalf()
else:
# print("ELSE!!!!!")
theta4 = (atan2(r33, -r13)).evalf()
theta6 = (atan2(-r22, r21)).evalf()
while (theta4 > pi):
theta4 = theta4 - 2 * pi
while (theta4 < -pi):
theta4 = 2 * pi + theta4
while (theta5 > pi):
theta5 = theta5 - 2 * pi
while (theta5 < -pi):
theta5 = 2 * pi + theta5
while (theta6 > pi):
theta6 = theta6 - 2 * pi
while (theta6 < -pi):
theta6 = 2 * pi + theta6
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
FK3 = T0_4.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
# your_wc = [1,1,1] # <--- Load your calculated WC values in this array
# your_ee = [1,1,1] # <--- Load your calculated end effector value from your forward kinematics
rqst_wc = [WC[0], WC[1], WC[2]]
your_wc = [FK3[0, 3], FK3[1, 3], FK3[2, 3]]
# print "WC =", WC
# print "FK3 =", FK3[0,3], FK3[1,3], FK3[2,3]
# print "theta =", theta4, ", ", theta5, ", ", theta6
#print 'WC=',WC
#print 'T03=',FK3[0,3], FK3[1,3], FK3[2,3]
your_ee = [FK[0, 3], FK[1, 3], FK[2, 3]]
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# # Find WC error
# if not(sum(your_wc)==3):
# wc_x_e = abs(your_wc[0]-rqst_wc[0])
# wc_y_e = abs(your_wc[1]-rqst_wc[1])
# wc_z_e = abs(your_wc[2]-rqst_wc[2])
# wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
# print ("\nWrist error for x position is: %04.8f" % wc_x_e)
# print ("Wrist error for y position is: %04.8f" % wc_y_e)
# print ("Wrist error for z position is: %04.8f" % wc_z_e)
# print ("Overall wrist offset is: %04.8f units" % wc_offset)
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - POS[x][0])
ee_y_e = abs(your_ee[1] - POS[x][1])
ee_z_e = abs(your_ee[2] - POS[x][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" %
ee_offset)
def rotation_base_ee(roll, pitch, yaw):
rot_end_effector = rotation_z(yaw) * rotation_y(pitch) * rotation_x(roll)
rot_correction = rotation_z(radians(180)) * rotation_y(radians(-90))
return rot_end_effector * rot_correction
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self,EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self,EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self,position,orientation):
self.position = position
self.orientation = orientation
comb = Combine(position,orientation)
class Pose:
def __init__(self,comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
#print('assigning DH parameters')
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7') # twist angle
# joint angle symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# print 'DH parameters assigned'
# print 'Created DH Table'
# Modified DH params
DH_Table = {alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2., a1: 0.35, d2: 0, q2: -pi/2. + q2,
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: -pi/2., a3: -0.054, d4: 1.5, q4: q4,
alpha4: pi/2, a4: 0, d5: 0, q5: q5,
alpha5: -pi/2., a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
# print 'DH Table Created'
# define modified DH transformation matrix
# print 'Creating TF Matrix'
def TF_Matrix(alpha, a, d, q):
TF = Matrix([
[cos(q), -sin(q), 0, a],
[sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[sin(q)* sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[0, 0, 0, 1]
])
return TF
# print 'applying transforms'
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# print 'transforms applied'
# Extract end-effector position and orientation from request
# px, py, pz = end-effector position
# roll, pitch, yaw = end-effector orientation
# print 'applying end effector position and orientation'
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
print('pose_x',req.poses[x].position.x)
print('pose_y',req.poses[x].position.y)
print('pose_z',req.poses[x].position.z)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
print('orient_x',req.poses[x].orientation.x)
print('orient_y',req.poses[x].orientation.y)
print('orient_z',req.poses[x].orientation.z)
print('orient_w',req.poses[x].orientation.w)
# print 'end effector position and orientation applied'
# Find EE rotation matrix
# Define RPY roation matrices
# http://planning.cs.uiuc.edu/node102.html
# print 'applying EE rotation matrix'
r, p , y = symbols('r p y')
ROT_x = Matrix([
[1, 0 , 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]]) # ROLL
ROT_y = Matrix([
[cos(p), 0 , sin(p)],
[0, 1, 0],
[-sin(p), 0, cos(p)]]) # PITCH (CHANGED!)
ROT_z = Matrix([
[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[0, 0, 1]]) # YAW
ROT_EE = ROT_z * ROT_y * ROT_x
print('ROT_EE1',ROT_EE)
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
print('ROT_Err',Rot_Error)
ROT_EE = ROT_EE * Rot_Error
print('ROT_EE2',ROT_EE)
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
print('ROT_EE3',ROT_EE)
EE = Matrix([[px], [py], [pz]])
print('EE_matrix:',EE)
# print 'EE rotation matrix applied'
WC = EE - (0.303) * ROT_EE[:,2]
print('WC',WC)
theta1 = atan2(WC[1], WC[0])
# print 'theta 1 calculated'
# SSS triangle for theta2 and theta3
side_a = 1.501
side_b = sqrt(pow(sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35, 2) + pow((WC[2] - 0.75), 2)) #CHANGED!!!
side_c = 1.25
angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))
angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))
angle_c = acos((side_a * side_a + side_b * side_b - side_c * side_c ) / (2 * side_a * side_b))
theta2 = pi/2. - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35) #(CHANGED!!!)
theta3 = pi/2. - (angle_b + 0.036) # 0.036 accounts for sag in link4 of -0.054m
# print 'theta 2 and 3 calculated'
print('T0_1:',T0_1[0:3,0:3])
print('T1_2:',T1_2[0:3,0:3])
print('T2_3:',T2_3[0:3,0:3])
R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
print('R0_3_pre:',R0_3)
R0_3 = R0_3.evalf(subs={q1: theta1, q2:theta2, q3: theta3})
print('R0_3:',R0_3)
print('transpose:',R0_3.transpose())
print('ROT_EE:', ROT_EE)
R3_6 = R0_3.transpose() * ROT_EE
print('R3_6:',R3_6)
# Euler angles from rotation matrix
print('theta4_ang',R3_6[2,2], -R3_6[0,2])
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta5 = atan2(sqrt(R3_6[0,2]*R3_6[0,2] + R3_6[2,2]*R3_6[2,2]), R3_6[1,2])
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
# print 'theta 4, 5 and 6 calculated'
print ('theta1:',theta1)
print ('theta2:',theta2)
print ('theta3:',theta3)
print ('theta4:',theta4)
print ('theta5:',theta5)
print ('theta6:',theta6)
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_EE.evalf(subs={q1: theta1, q2:theta2, q3:theta3, q4:theta4, q5:theta5, q6:theta6})
print 'forward kinematics applied'
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1], WC[2]] # <--- Load your calculated WC values in this array
your_ee = [FK[0,3], FK[1,3], FK[2,3]] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
print 'your_wc and your_ee applied'
## Error analysis
print ("\nTotal run time to calculate joint angles from pose is %04.4f seconds" % (time()-start_time))
# Find WC error
if not(sum(your_wc)==3):
wc_x_e = abs(your_wc[0]-test_case[1][0])
wc_y_e = abs(your_wc[1]-test_case[1][1])
wc_z_e = abs(your_wc[2]-test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print ("\nWrist error for x position is: %04.8f" % wc_x_e)
print ("Wrist error for y position is: %04.8f" % wc_y_e)
print ("Wrist error for z position is: %04.8f" % wc_z_e)
print ("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1-test_case[2][0])
t_2_e = abs(theta2-test_case[2][1])
t_3_e = abs(theta3-test_case[2][2])
t_4_e = abs(theta4-test_case[2][3])
t_5_e = abs(theta5-test_case[2][4])
t_6_e = abs(theta6-test_case[2][5])
print ("\nTheta 1 error is: %04.8f" % t_1_e)
print ("Theta 2 error is: %04.8f" % t_2_e)
print ("Theta 3 error is: %04.8f" % t_3_e)
print ("Theta 4 error is: %04.8f" % t_4_e)
print ("Theta 5 error is: %04.8f" % t_5_e)
print ("Theta 6 error is: %04.8f" % t_6_e)
print ("\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print (" ")
# Find FK EE error
if not(sum(your_ee)==3):
ee_x_e = abs(your_ee[0]-test_case[0][0][0])
ee_y_e = abs(your_ee[1]-test_case[0][0][1])
ee_z_e = abs(your_ee[2]-test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print ("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print ("End effector error for y position is: %04.8f" % ee_y_e)
print ("End effector error for z position is: %04.8f" % ee_z_e)
print ("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
# Create symbols
# Twist angle: alpha(i-1) = angle between Zi-1 to Zi measured about Xi-1
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# Link length: a(i-1) = distance from Zi-1 to Zi measured along Xi-1
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link length
# Offset length: d(i) = distance from Xi-1 to Xi measured along Zi
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link offset
# Joint angle: q(i) = angle between Xi-1 to Xi measured about Zi
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Create Modified DH parameters
dh = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2.,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.5,
q4: q4,
alpha4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Define Modified DH Transformation matrix
def tf_matrix(alpha, a, d, q):
return Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
# Create individual transformation matrices
T0_1 = tf_matrix(alpha0, a0, d1, q1).subs(dh)
T1_2 = tf_matrix(alpha1, a1, d2, q2).subs(dh)
T2_3 = tf_matrix(alpha2, a2, d3, q3).subs(dh)
T3_4 = tf_matrix(alpha3, a3, d4, q4).subs(dh)
T4_5 = tf_matrix(alpha4, a4, d5, q5).subs(dh)
T5_6 = tf_matrix(alpha5, a5, d6, q6).subs(dh)
T6_EE = tf_matrix(alpha6, a6, d7, q7).subs(dh)
# Extract rotation matrices from the transformation matrices
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
### Get end-effector(EE) position & orientation
i = 0
req_pos = req.poses[i].position
req_ori = req.poses[i].orientation
px = req_pos.x
py = req_pos.y
pz = req_pos.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req_ori.x, req_ori.y, req_ori.z, req_ori.w])
### Defince end-effector(EE) rotation matrix
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
ROT_EE = ROT_z * ROT_y * ROT_x
### Correction for the difference between URDF and DH convention
ROT_Corr = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * ROT_Corr
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate joint angles using Geometric IK method
# EE and WC positions
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * ROT_EE[:, 2]
# Calculte joint angles using the IK method
theta1 = atan2(WC[1], WC[0])
# the triangle for theta2, theta3 and WC
side_a = 1.501
side_b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) +
pow((WC[2] - 0.75), 2))
side_c = 1.25
# cos law
angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
theta2 = pi / 2 - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
theta3 = pi / 2 - (angle_b + 0.036
) # 0.036 accounts for sag in link4 of -0.054m
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * ROT_EE
# Euler angles from rotation matrix
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
global dh_model
dh_model.init_variables()
# DH Parameters
dh_model.init_dh_parameters()
# Create invididual transformation matrices
dh_model.init_transform_matrices()
# Exctract end-effector position and orientation from request
# px, py, pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px, py, pz = get_position(req.poses[x])
(roll, pitch, yaw) = get_euler(req.poses[x])
ROT_EE = dh_model.get_rot_end_effector()
# More information can be found in KR210 Forward Kinematics section
Rot_Error = dh_model.get_rot_error(radians(180), radians(-90))
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * ROT_EE[:, 2]
# Calculate joint angles using Geometric IK method
# More information can be found in the Inverse Kinematics with Kuka KR210
theta1 = atan2(WC[1], WC[0])
# SSS triangle for theta 2 and theta3
side_a = 1.501
side_b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) +
pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
angle_c = acos((side_a * side_a + side_b * side_b - side_c * side_c) /
(2 * side_a * side_b))
theta2 = pi / 2 - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
theta3 = pi / 2 - (angle_b + 0.036
) # 0.036 accounts for sag in link4 of -0.054m
R0_3 = dh_model.T0_1[0:3, 0:3] * dh_model.T1_2[0:3,
0:3] * dh_model.T2_3[0:3,
0:3]
R0_3 = R0_3.evalf(subs={
dh_model.q1: theta1,
dh_model.q2: theta2,
dh_model.q3: theta3
})
R3_6 = R0_3.transpose() * ROT_EE
# Euler angles from rotation matrix
# More information can be found in the Euler Angles from a Rotation Matrix section
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
a = sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2])
theta5 = atan2(a, R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
## Ending at: Populate response for the IK request
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = dh_model.T0_EE.evalf(
subs={
dh_model.q1: theta1,
dh_model.q2: theta2,
dh_model.q3: theta3,
dh_model.q4: theta4,
dh_model.q5: theta5,
dh_model.q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
# Define DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') #link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') #link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') # twist angle
# Joint Angle Symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Modified DH parameters
DH_Table = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: rad(-90),
a1: 0.35,
d2: 0,
q2: -rad(90) + q2,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: rad(-90),
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: rad(90),
a4: 0,
d5: 0,
q5: q5,
alpha5: rad(-90),
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Create individual transformation matrices
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
T0_EE = simplify(T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE)
print("\nTime taken to create transformation matrices: %04.4f seconds" %
(time() - start_time))
# ----- IK Code -------
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
# Find EE rotation matrix
# Define RPY rotation matrices
# (Further reading got from walkthrough)
# http://planning.cs.uiuc.edu/node102.html
r, p, y = symbols('r p y')
ROT_x = rot_x(r) #Roll
ROT_y = rot_y(p) #Pitch
ROT_z = rot_z(y) #Yaw
# ROT_EE = ROT_x * ROT_y * ROT_z
ROT_EE = ROT_z * ROT_y * ROT_x
#Calculate Rotation Error
ROT_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * ROT_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * ROT_EE[:, 2]
# Calculate Joint Angles using Geometric IK Method
theta1 = atan2(WC[1], WC[0])
# SSS triangle for theta2 and theta3
side_a = 1.501 # Found by using "measure" tool in RViz.
side_b = sqrt(
pow((sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35), 2) +
pow((WC[2] - 0.75), 2))
side_c = 1.25 # Length of joint 1 to 2
angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
angle_c = acos((side_a * side_a + side_b * side_b - side_c * side_c) /
(2 * side_a * side_b))
theta2 = pi / 2 - angle_a - atan2(
WC[2] - 0.75,
sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
theta3 = pi / 2 - (angle_b + 0.036
) # 0.036 account for sag in link4 of -0.054m
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * ROT_EE
# Euler Angles from rotation matrix
theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
# select best solution based on theta5
if (theta5 > pi):
theta4 = atan2(-R3_6[2, 2], R3_6[0, 2])
theta6 = atan2(R3_6[1, 1], -R3_6[1, 0])
else:
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Define DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
r1, r2, r3 = symbols(
'r1:4') #Variables the roll, pitch and yaw will be put into to
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# Joint angle symbols
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') #The thetas
# Modified DH params
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
alpha1: -pi / 2,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2,
alpha2: 0,
a2: 1.25,
d3: 0,
alpha3: -pi / 2,
a3: -0.054,
d4: 1.50,
alpha4: -pi / 2,
a4: 0,
d5: 0,
alpha5: -pi / 2,
a5: 0,
d6: 0,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# Define Modified DH Transformation matrix
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return TF
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(s)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
#Find EE rotation
#RPY rotation matrices
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r),
cos(r)]]) #ROLL
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0,
cos(p)]]) #Pitch
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0,
1]]) #Yaw
ROT_EE = ROT_z * ROT_y * ROT_x
#Rotation Error correction
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (.303) * ROT_EE[:, 2]
#Calculate joint angles using Geometrix IK method
#theta1 = atan2(WC[1],WC[0])
#Calculate IK
theta1 = atan2(py, px)
j2z = 0.75
j2x = 0.35 * cos(theta1)
j2y = 0.35 * sin(theta1)
j2pz = j2z - pz
blength = sqrt(
np.square(px - j2x) + np.square(py - p2y) + np.square(pz - j2z))
j3p = sqrt(np.square(2.53 - 0.35) + np.square(1.946 - 2.0))
angle1 = asin(j2pz / blength)
f1 = acos((np.square(1.25) + np.square(blength) - np.square(j3p)) /
(2 * 1.25 * blength))
theta2 = np.pi / 2 + g1 - f1
j3z = j2z + (1.25 * cos(theta2))
s = j3z - 0.054 - pz
theta3 = asin(s / j3p) - theta2
# #Calculate theta2 and theta3 while theta 4,5,6 = 0
# side_a = 1.25
# side_b = 1.80452
# side_l = sqrt((px - .35)*(px - .35) + (pz - .75)*(pz - .75))
#
# #Once the triangle is built we cans uses the law of cosins to find the angles
# #SSS triangle
# angle_b = acos((side_a * side_a + side_l * side_l - side_b * side_b) / (2 * side_a * side_l))
# angle_l = acos((side_a * side_a + side_b * side_b - side_l * side_l) / (2 * side_a * side_b))
#
# theta2 = pi/2 - angle_b - atan2(pz - 0.75, sqrt(px * px + py * py) - 0.35)
#
# theta3 = pi/2 - (angle_l +0.036)
# theta4 = 0
# theta5 = 0
# theta6 = 0
## #This accounts for the case when atan is greater then 1
## if theta1 > 2.2:
## theta1 = theta1 - pi
# # SSS triangle for theta2 and theta3
# side_a = 1.501
# side_b = sqrt(pow((sqrt(WC[0] * WC[0] + WC[1]*WC[1]) - 0.35),2) + pow((WC[2]-0.75),2))
# side_c = 1.250
# angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))
# angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))
# angle_c = acos((side_a * side_a + side_b * side_b - side_c * side_c) / (2 * side_a * side_b))
# theta2 = pi/2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
# theta3 = pi/2 - (angle_b +0.036)
# R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
# R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
# R3_6 = R0_3.inv("LU") * ROT_EE
# ### Identify useful terms from rotation matrix
# r31 = R3_6[2,0]
# r11 = R3_6[0,0]
# r21 = R3_6[1,0]
# r32 = R3_6[2,1]
# r33 = R3_6[2,2]
# # Euler angles from rotation matrix
## first, second, third = tf.transformations.euler_from_matrix(np.array(R3_6).astype(np.float64), "ryzy")
##
## theta4 = first
## theta5 = second
## theta6 = third
## theta4 = atan2(r21,r11)
## theta5 = atan2(sqrt(r11 * r11 +r21*r21),r31)
## theta6 = atan2(r32,r33)
# theta4 = atan2(R3_6[2,2],-R3_6[0,2])
# theta5 = atan2(sqrt(R3_6[0,2]*R3_6[0,2]+R3_6[2,2]*R3_6[2,2]),R3_6[1,2])
# theta6 = atan2(-R3_6[1,1],R3_6[1,0])
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print(
"\nWrist error for x position is: {0:4.8f} True Value: {1: 4.8f} Your value: {2:4.8f}"
.format(np.float64(wc_x_e), np.float64(test_case[1][0]),
np.float64(your_wc[0])))
print(
"Wrist error for y position is: {0:4.8f} True Value: {1: 4.8f} Your value: {2:4.8f}"
.format(np.float64(wc_y_e), np.float64(test_case[1][1]),
np.float64(your_wc[1])))
print(
"Wrist error for z position is: {0:4.8f} True Value: {1: 4.8f} Your value: {2:4.8f}"
.format(np.float64(wc_z_e), np.float64(test_case[1][2]),
np.float64(your_wc[2])))
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
#More information about troubleshooting.
print(
"\nTheta 1 error is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(t_1_e), np.float64(test_case[2][0]),
np.float64(theta1)))
print(
"Theta 2 error is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(t_2_e), np.float64(test_case[2][1]),
np.float64(theta2)))
print(
"Theta 3 error is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(t_3_e), np.float64(test_case[2][2]),
np.float64(theta3)))
print(
"Theta 4 error is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(t_4_e), np.float64(test_case[2][3]),
np.float64(theta4)))
print(
"Theta 5 error is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(t_5_e), np.float64(test_case[2][4]),
np.float64(theta5)))
print(
"Theta 6 error is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(t_6_e), np.float64(test_case[2][5]),
np.float64(theta6)))
## print ("\nTheta 1 error is: %04.8f Truth value: %14.8f Your value: %24.8f" % t_1_e, test_case[2][0], theta1 )
# print ("Theta 2 error is: %04.8f" % t_2_e)
# print ("Theta 3 error is: %04.8f" % t_3_e)
# print ("Theta 4 error is: %04.8f" % t_4_e)
# print ("Theta 5 error is: %04.8f" % t_5_e)
# print ("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print(
"\nEnd effector error for x position is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(ee_x_e), np.float64(test_case[0][0][0]),
np.float64(your_ee[0])))
print(
"End effector error for y position is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(ee_y_e), np.float64(test_case[0][0][1]),
np.float64(your_ee[1])))
print(
"End effector error for z position is: {0:4.8f} Truth value: {1:4.8f} Your value: {2:4.8f}"
.format(np.float64(ee_z_e), np.float64(test_case[0][0][2]),
np.float64(your_ee[2])))
# print ("\nEnd effector error for x position is: {0:4.8f}".format(np.float64(ee_x_e)))
# print ("End effector error for y position is: %04.8f" % ee_y_e)
# print ("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
#DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alp0, alp1, alp2, alp3, alp4, alp5, alp6 = symbols('alp0:7')
#joint angles
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
DH_tab = {
alp0: 0.,
a0: 0.0,
d1: 0.75,
q1: q1,
alp1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: -pi / 2. + q2,
alp2: 0.,
a2: 1.25,
d3: 0,
q3: q3,
alp3: -pi / 2.,
a3: -0.054,
d4: 1.5,
q4: q4,
alp4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alp5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alp6: 0.,
a6: 0,
d7: 0.303,
q7: q7,
}
#DH Transformation matrix
def TF_Matrix(alp, a, d, q):
TF = Matrix(
[[cos(q), -sin(q), 0, a],
[sin(q) * cos(alp),
cos(q) * cos(alp), -sin(alp), -sin(alp) * d],
[sin(q) * sin(alp),
cos(q) * sin(alp),
cos(alp),
cos(alp) * d], [0, 0, 0, 1]])
return TF
#Individual transformation matrices
T0_1 = TF_Matrix(alp0, a0, d1, q1).subs(DH_tab)
T1_2 = TF_Matrix(alp1, a1, d2, q2).subs(DH_tab)
T2_3 = TF_Matrix(alp2, a2, d3, q3).subs(DH_tab)
T3_4 = TF_Matrix(alp3, a3, d4, q4).subs(DH_tab)
T4_5 = TF_Matrix(alp4, a4, d5, q5).subs(DH_tab)
T5_6 = TF_Matrix(alp5, a5, d6, q6).subs(DH_tab)
T6_E = TF_Matrix(alp6, a6, d7, q7).subs(DH_tab)
T0_EE = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_E
#Get End-effector position
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
#Get roll-pitch-yaw
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
#Get Rotation Matrices from roll-pitch-yaw
r, p, y = symbols('r p y')
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
ROT_EE = ROT_z * ROT_y * ROT_x
#Include Rotation error Correction
Rot_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * Rot_Error
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
# Calculate Wrist center: Using following equation
# _ _ _ _ _ _
# 0 0 0 | 0 | | Px | 0 | 0 |
# r = r - d. R | 0 | = | Py | - d. R | 0 |
# WC/0 EE/0 6 |_ 1 _| |_ Pz _| 6 |_ 1 _|
#
WC = EE - (0.303) * ROT_EE[:, 2]
## Insert IK code here!
theta1 = atan2(WC[1], WC[0])
#SSS triangle for theta2 and theta3
side_a = 1.501
side_b = sqrt(
pow((sqrt(WC[0]**2 + WC[1]**2) - 0.35), 2) + pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos((side_b**2 + side_c**2 - side_a**2) / (2 * side_b * side_c))
angle_b = acos((side_a**2 + side_c**2 - side_b**2) / (2 * side_a * side_c))
angle_c = acos((side_a**2 + side_b**2 - side_c**2) / (2 * side_a * side_b))
theta2 = pi / 2. - angle_a - atan2(WC[2] - 0.75,
sqrt(WC[0]**2 + WC[1]**2) - 0.35)
theta3 = pi / 2. - (angle_b + 0.036
) # 0.036 because of sag in link4 i.e. -0.054m
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
#R3_6 = R0_3.inv("LU") * ROT_EE #Transpose is same as inverse of rotation matrix
R3_6 = R0_3.T * ROT_EE
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(R3_6[0, 2]**2 + R3_6[2, 2]**2), R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**2These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**2")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
## Insert IK code here starting at: Define DH parameter symbols
## YOUR CODE HERE!
## Create symbols for joint variables
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # theta 1
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
##KUKA KR210
s = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: q2 - pi / 2.,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.5,
q4: q4,
alpha4: pi / 2.,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
##Homogeneous Transforms
"""
T0_1 = Matrix([[ cos(q1), -sin(q1), 0, a0],
[ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],
[ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0), cos(alpha0), cos(alpha0)*d1],
[ 0, 0, 0, 1]])
T0_1 = T0_1.subs(s)
T1_2 = Matrix([[ cos(q2), -sin(q2), 0, a1],
[ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],
[ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1), cos(alpha1), cos(alpha1)*d2],
[ 0, 0, 0, 1]])
T1_2 = T1_2.subs(s)
T2_3 = Matrix([[ cos(q3), -sin(q3), 0, a2],
[ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],
[ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2), cos(alpha2), cos(alpha2)*d3],
[ 0, 0, 0, 1]])
T2_3 = T2_3.subs(s)
T3_4 = Matrix([[ cos(q4), -sin(q4), 0, a3],
[ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],
[ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3), cos(alpha3), cos(alpha3)*d4],
[ 0, 0, 0, 1]])
T3_4 = T3_4.subs(s)
T4_5 = Matrix([[ cos(q5), -sin(q5), 0, a4],
[ sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5],
[ sin(q5)*sin(alpha4), cos(q5)*sin(alpha4), cos(alpha4), cos(alpha4)*d5],
[ 0, 0, 0, 1]])
T4_5 = T4_5.subs(s)
T5_6 = Matrix([[ cos(q6), -sin(q6), 0, a5],
[ sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6],
[ sin(q6)*sin(alpha5), cos(q6)*sin(alpha5), cos(alpha5), cos(alpha5)*d6],
[ 0, 0, 0, 1]])
T5_6 = T5_6.subs(s)
T6_G = Matrix([[ cos(q7), -sin(q7), 0, a6],
[ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7],
[ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6), cos(alpha6), cos(alpha6)*d7],
[ 0, 0, 0, 1]])
T6_G = T6_G.subs(s)
"""
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return TF
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s)
T6_G = TF_Matrix(alpha6, a6, d7, q7).subs(s)
print(simplify(T3_4 * T4_5 * T5_6))
## Composition of Homogenious Transforms
#T0_2 = simplify(T0_1 * T1_2) # base_link to link_2
#T0_3 = simplify(T0_2 * T2_3) # base_link to link_3
#T0_4 = simplify(T0_3 * T3_4) # base_link to link_4
#T0_5 = simplify(T0_4 * T4_5) # base_link to link_5
#T0_6 = simplify(T0_5 * T5_6) # base_link to link_6
#T0_G = simplify(T0_6 * T6_G) # base_link to link_G
T0_G = T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_G
## Correction needed to account for orientation difference between definition of
# gripper link on URDF vs DH Convention
## Numerically evaluate transforms
#print("T0_1 = ",T0_1.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
#print("T0_2 = ",T0_2.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
#print("T0_3 = ",T0_3.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
#print("T0_4 = ",T0_4.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
#print("T0_5 = ",T0_5.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
#print("T0_6 = ",T0_6.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
#print("T0_G = ",T0_G.evalf(subs={q1: 0, q2: 0, q3: 0, q4: 0, q5: 0, q6: 0}))
## Total Homogeneous Transform between base_link and gripper_link with
# Orientation correction applied
#T_total = simplify(T0_G * R_corr)
## Calculate wrist center
EE_pos = Matrix([
req.poses[x].position.x, req.poses[x].position.y,
req.poses[x].position.z
])
#quaternion = (req.poses[x].orientation.x,
# req.poses[x].orientation.y,
# req.poses[x].orientation.z,
# req.poses[x].orientation.w)
#EE_orientation = tf.transformations.euler_from_quaternion(quaternion)
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
r, p, y = symbols('r p y')
X_rot = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
Y_rot = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
Z_rot = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
rrpy = Z_rot * Y_rot * X_rot
#print("ROTEEEASDAS: ",otEE)
#rotEE = Z_rot.subs(y, yaw) * Y_rot.subs(p, pitch) * X_rot.subs(r, roll)
#print("ROTEEEASDAS: ",rotEE)
rrpy = rrpy.subs({'r': roll, 'p': pitch, 'y': yaw})
rotError = Z_rot.subs(y, radians(180)) * Y_rot.subs(p, radians(-90))
#EE_X_rot = X_rot.subs({roll: EE_orientation[0]})
#EE_Y_rot = Y_rot.subs({pitch: EE_orientation[1]})
#EE_Z_rot = Z_rot.subs({yaw: EE_orientation[2]})
#R_z = Z_rot.subs({yaw: np.pi})
#R_y = Y_rot.subs({pitch: -np.pi/2})
#R_corr = simplify(R_z * R_y)
#temp = rotEE
rotEE = rrpy * rotError
#rotEE = rotEE.subs({'r': roll, 'p': pitch, 'y': yaw})
print("rotEE", rotEE)
#T_total = T0_G*R_corr
#WC = EE_pos - .303 * rotEE[:,2]
w_x = EE_pos[0] - .303 * rotEE[0, 2]
w_y = EE_pos[1] - .303 * rotEE[1, 2]
w_z = EE_pos[2] - .303 * rotEE[2, 2]
theta1 = atan(w_y / w_x)
J2_WC_x = abs(sqrt(w_x**2 + w_y**2) - .35)
J2_WC_y = abs(w_z - .75)
A = 1.501
B = sqrt(J2_WC_x**2 + J2_WC_y**2)
C = 1.25
a = acos((-A**2 + B**2 + C**2) / (2 * B * C))
theta2 = atan(J2_WC_x / J2_WC_y) - a
b = acos((A**2 + C**2 - B**2) / (2 * A * C))
theta3 = np.pi / 2 - b - .036
print("theta1,2,3: ", theta1, theta2, theta3)
R0_3 = T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.inv("LU") * rotEE
print("asdasfa", R0_3 * R0_3.inv("LU"))
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2])
theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2])
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0])
print("theta4,5,6: ", theta4, theta5, theta6)
## Ending at: Populate response for the IK request
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee =
## (OPTIONAL) YOUR CODE HERE!
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [w_x, w_y,
w_z] # <--- Load your calculated WC values in this array
ex = [-0.65, 0.45, -0.36, 0.95, 0.79, 0.49]
ee = T0_G.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
your_ee = [
ee[0, 3], ee[1, 3], ee[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple posisiotns. It is best to add your forward kinmeatics to \
\nlook at the confirm wether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self,EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self,EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self,position,orientation):
self.position = position
self.orientation = orientation
comb = Combine(position,orientation)
class Pose:
def __init__(self,comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
print 'Creating symbols'
# Create symbols
# Joint angles
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# Link len's
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
# Link offsets
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
# Joint twists
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')
print 'Creating DH parameters'
# Create Modified DH parameters
dhParams = {alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2., a1: 0.35, d2: 0, q2: -pi/2. + q2,
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: -pi/2., a3: -0.054, d4: 1.5, q4: q4,
alpha4: pi/2, a4: 0, d5: 0, q5: q5,
alpha5: -pi/2., a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
print 'Defining DH transformation matrix'
# Define Modified DH Transformation matrix
def dh_transform(alpha, a, d, q):
dhtMatrix = Matrix([ [ cos(q), -sin(q), 0, a],
[ sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[ sin(q)*sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]
])
return dhtMatrix
print 'Creating individual transform matrices'
# Create individual transformation matrices
T0_1 = dh_transform(alpha0, a0, d1, q1).subs(dhParams)
T1_2 = dh_transform(alpha1, a1, d2, q2).subs(dhParams)
T2_3 = dh_transform(alpha2, a2, d3, q3).subs(dhParams)
T3_4 = dh_transform(alpha3, a3, d4, q4).subs(dhParams)
T4_5 = dh_transform(alpha4, a4, d5, q5).subs(dhParams)
T5_6 = dh_transform(alpha5, a5, d6, q6).subs(dhParams)
T6_7 = dh_transform(alpha6, a6, d7, q7).subs(dhParams)
T0_7 = (T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_7)
# Extract rotation matrices from the transformation matrices
print 'Creating roll, pitch, and yaw rotation matrices'
# Roll, Pitch, Yaw Symbols
r, p , y = symbols('r p y')
# Roll
rotationX = Matrix([
[1, 0 , 0],
[0, cos(r), -sin(r)],
[0, sin(r), cos(r)]
])
# Pitch
rotationY = Matrix([
[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[ -sin(p), 0, cos(p)]
])
# Yaw
rotationZ = Matrix([
[cos(y), -sin(y), 0],
[sin(y), cos(y), 0],
[ 0, 0, 1]
])
ROT_EE = (rotationX * rotationY * rotationZ)
rotationErr = rotationZ.subs(y, radians(180)) * rotationY.subs(p, radians(-90))
ROT_EE = (ROT_EE * rotationErr)
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * ROT_EE[:,2]
print 'Calculating theta1'
theta1 = atan2(WC[1], WC[0])
side_a = 1.501
side_b = sqrt(pow(sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35, 2)+ pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos((side_b * side_b + side_c * side_c - side_a * side_a) / (2 * side_b * side_c))
angle_b = acos((side_a * side_a + side_c * side_c - side_b * side_b) / (2 * side_a * side_c))
angle_c = acos((side_a * side_a + side_b * side_b - side_c * side_c ) / (2 * side_a * side_b))
print 'Calculating theta2'
theta2 = pi/2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35)
print 'Calculating theta3'
theta3 = pi/2 - (angle_b + 0.036)
R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
R0_3 = R0_3.evalf(subs={q1: theta1, q2:theta2, q3: theta3})
R3_6 = R0_3.transpose() * rotationErr
print 'Calculating theta4'
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
print 'Calculating theta5'
theta5 = atan2(sqrt(R3_6[0,2]*R3_6[0,2] + R3_6[2,2]*R3_6[2,2]), R3_6[1,2])
print 'Calculating theta6'
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
print (theta1)
print (theta2)
print (theta3)
print (theta4)
print (theta5)
print (theta6)
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
FK = T0_7.evalf(subs={q1: theta1, q2:theta2, q3:theta3, q4:theta4, q5:theta5, q6:theta6})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [1,1,1] # <--- Load your calculated WC values in this array
your_ee = [1,1,1] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print ("\nTotal run time to calculate joint angles from pose is %04.4f seconds" % (time()-start_time))
# Find WC error
if not(sum(your_wc)==3):
wc_x_e = abs(your_wc[0]-test_case[1][0])
wc_y_e = abs(your_wc[1]-test_case[1][1])
wc_z_e = abs(your_wc[2]-test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print ("\nWrist error for x position is: %04.8f" % wc_x_e)
print ("Wrist error for y position is: %04.8f" % wc_y_e)
print ("Wrist error for z position is: %04.8f" % wc_z_e)
print ("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1-test_case[2][0])
t_2_e = abs(theta2-test_case[2][1])
t_3_e = abs(theta3-test_case[2][2])
t_4_e = abs(theta4-test_case[2][3])
t_5_e = abs(theta5-test_case[2][4])
t_6_e = abs(theta6-test_case[2][5])
print ("\nTheta 1 error is: %04.8f" % t_1_e)
print ("Theta 2 error is: %04.8f" % t_2_e)
print ("Theta 3 error is: %04.8f" % t_3_e)
print ("Theta 4 error is: %04.8f" % t_4_e)
print ("Theta 5 error is: %04.8f" % t_5_e)
print ("Theta 6 error is: %04.8f" % t_6_e)
print ("\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print (" ")
# Find FK EE error
if not(sum(your_ee)==3):
ee_x_e = abs(your_ee[0]-test_case[0][0][0])
ee_y_e = abs(your_ee[1]-test_case[0][0][1])
ee_z_e = abs(your_ee[2]-test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print ("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print ("End effector error for y position is: %04.8f" % ee_y_e)
print ("End effector error for z position is: %04.8f" % ee_z_e)
print ("Overall end effector offset is: %04.8f units \n" % ee_offset)
def handle_calculate_IK(req):
rospy.loginfo("Received %s eef-poses from the plan" % len(req.poses))
if len(req.poses) < 1:
print "No valid poses received"
return -1
else:
## Create individual transformation matrices
T0_1 = TF_Mat(alpha0, a0, d1, q1).subs(DH)
T1_2 = TF_Mat(alpha1, a1, d2, q2).subs(DH)
T2_3 = TF_Mat(alpha2, a2, d3, q3).subs(DH)
T3_4 = TF_Mat(alpha3, a3, d4, q4).subs(DH)
T4_5 = TF_Mat(alpha4, a4, d5, q5).subs(DH)
T5_6 = TF_Mat(alpha5, a5, d6, q6).subs(DH)
T6_7 = TF_Mat(alpha6, a6, d7, q7).subs(DH)
## Composition of Homogeneous Transforms
## Transform from Base link_0 to End Effector (Gripper)
T0_7 = (T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_7)
# Correction Needed to Account for Orientation Difference Between
# Definition of Gripper Link_G
# Matrix is pre-calculated to improve performance.
R_corr = Matrix([[0, 0, 1.0, 0], [0, -1.0, 0, 0], [1.0, 0, 0, 0],
[0, 0, 0, 1.0]])
# Total Homogeneous Transform Between (Base) Link_0 and (End Effector) Link_7
# With orientation correction applied
T0_7_corr = (T0_7 * R_corr)
# Find EE rotation matrix RPY (Roll, Pitch, Yaw)
r, p, y = symbols('r p y')
# Roll
ROT_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
# Pitch
ROT_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
# Yaw
ROT_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
ROT_EE = ROT_z * ROT_y * ROT_x
# Compensate for rotation discrepancy between DH parameters and Gazebo
# Correction Needed to Account for Orientation Difference Between
# Definition of Gripper Link_G in URDF versus DH Convention
ROT_corr = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * ROT_corr
# Initialize service response
joint_trajectory_list = []
for x in xrange(0, len(req.poses)):
# IK code starts here
joint_trajectory_point = JointTrajectoryPoint()
# Extract end-effector position and orientation from request
# px,py,pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
# store EE position in a matrix
EE = Matrix([[px], [py], [pz]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
ROT_EE = ROT_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
# Calculate Wrest Center
WC = EE - DH[d7] * ROT_EE[:, 2]
theta1, theta2, theta3 = Calculate_theta_1_2_3(WC)
theta4, theta5, theta6 = Calculate_theta_4_5_6(
T0_1, T1_2, T2_3, ROT_EE, theta1, theta2, theta3, q1, q2, q3)
# Populate response for the IK request
# In the next line replace theta1,theta2...,theta6 by your joint angle variables
joint_trajectory_point.positions = [
theta1, theta2, theta3, theta4, theta5, theta6
]
joint_trajectory_list.append(joint_trajectory_point)
rospy.loginfo("length of Joint Trajectory List: %s" %
len(joint_trajectory_list))
return CalculateIKResponse(joint_trajectory_list)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self,EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self,EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self,position,orientation):
self.position = position
self.orientation = orientation
comb = Combine(position,orientation)
class Pose:
def __init__(self,comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
##
## Insert IK code here!
q1,q2,q3,q4,q5,q6,q7 = symbols('q1:8')
d1,d2,d3,d4,d5,d6,d7 = symbols('d1:8')
a0,a1,a2,a3,a4,a5,a6 = symbols('a0:7')
alpha0,alpha1,alpha2,alpha3,alpha4,alpha5,alpha6 = symbols('alpha0:7')
s = {alpha0: 0, a0: 0, d1: 0.75, q1: q1,
alpha1: -pi/2, a1: 0.35, d2: 0, q2: q2-pi/2,
alpha2: 0, a2: 1.25, d3: 0, q3: q3,
alpha3: -pi/2, a3: -0.054, d4: 1.5, q4: q4,
alpha4: pi/2, a4: 0, d5: 0, q5: q5,
alpha5: -pi/2, a5: 0, d6: 0, q6: q6,
alpha6: 0, a6: 0, d7: 0.303, q7: 0}
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[ cos(q), -sin(q), 0, a],
[sin(q)*cos(alpha), cos(q)*cos(alpha), -sin(alpha), -sin(alpha)*d],
[sin(q)*sin(alpha), cos(q)*sin(alpha), cos(alpha), cos(alpha)*d],
[ 0, 0, 0, 1]])
return TF
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(s)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(s)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(s)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(s)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(s)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(s)
T6_G = TF_Matrix(alpha6, a6, d7, q7).subs(s)
R_z = Matrix([[ cos(np.pi), -sin(np.pi), 0, 0],
[ sin(np.pi), cos(np.pi), 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
R_y = Matrix([[ cos(-np.pi/2), 0, sin(-np.pi/2), 0],
[ 0, 1, 0, 0],
[-sin(-np.pi/2), 0, cos(-np.pi/2), 0],
[ 0, 0, 0, 1]])
T0_G = simplify(T0_1*T1_2*T2_3*T3_4*T4_5*T5_6*T6_G*R_z*R_y)
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
[req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w])
r, p, y = symbols('r p y')
ROT_x = Matrix([[ 1, 0, 0],
[ 0, cos(r),-sin(r)],
[ 0, sin(r), cos(r)]])
ROT_y = Matrix([[ cos(p), 0, sin(p)],
[ 0, 1, 0],
[-sin(p), 0, cos(p)]])
ROT_z = Matrix([[ cos(y),-sin(y), 0],
[ sin(y), cos(y), 0],
[ 0, 0, 1]])
ROT_EE = ROT_z*ROT_y*ROT_x
ROT_Error = ROT_z.subs(y, radians(180)) * ROT_y.subs(p, radians(-90))
ROT_EE = ROT_EE * ROT_Error
ROT_EE = ROT_EE.subs({'r':roll, 'p':pitch, 'y':yaw})
EE = Matrix([[px],
[py],
[pz]])
WC = EE - (0.303) * ROT_EE[:,2]
theta1 = atan2(WC[1],WC[0])
side_a = 1.501
side_b = sqrt(pow((sqrt(WC[0]*WC[0]+WC[1]*WC[1])-0.35),2) + pow((WC[2] - 0.75), 2))
side_c = 1.25
angle_a = acos((side_b*side_b + side_c*side_c - side_a*side_a)/(2*side_b*side_c))
angle_b = acos((side_a*side_a + side_c*side_c - side_b*side_b)/(2*side_a*side_c))
angle_c = acos((side_a*side_a + side_b*side_b - side_c*side_c)/(2*side_a*side_b))
theta2 = pi/2 - angle_a - atan2(WC[2] - 0.75, sqrt(WC[0]*WC[0] + WC[1]*WC[1]) -0.35)
theta3 = pi/2 -(angle_b + 0.036)
R0_3 = T0_1[0:3,0:3] * T1_2[0:3,0:3] * T2_3[0:3,0:3]
R0_3 = R0_3.evalf(subs={q1:theta1, q2:theta2, q3:theta3})
R3_6 = R0_3.inv(method="LU") * ROT_EE
theta4 = atan2(R3_6[2,2], -R3_6[0,2])
theta5 = atan2(sqrt(R3_6[0,2]*R3_6[0,2] + R3_6[2,2]*R3_6[2,2]),R3_6[1,2])
theta6 = atan2(-R3_6[1,1], R3_6[1,0])
##
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
## (OPTIONAL) YOUR CODE HERE!
FK = T0_G.evalf(subs={q1:theta1,q2:theta2,q3:theta3,q4:theta4,q5:theta5,q6:theta6})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0],WC[1],WC[2]] # <--- Load your calculated WC values in this array
your_ee = [FK[0,3],FK[1,3],FK[2,3]] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print ("\nTotal run time to calculate joint angles from pose is %04.4f seconds" % (time()-start_time))
# Find WC error
if not(sum(your_wc)==3):
wc_x_e = abs(your_wc[0]-test_case[1][0])
wc_y_e = abs(your_wc[1]-test_case[1][1])
wc_z_e = abs(your_wc[2]-test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print ("\nWrist error for x position is: %04.8f" % wc_x_e)
print ("Wrist error for y position is: %04.8f" % wc_y_e)
print ("Wrist error for z position is: %04.8f" % wc_z_e)
print ("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1-test_case[2][0])
t_2_e = abs(theta2-test_case[2][1])
t_3_e = abs(theta3-test_case[2][2])
t_4_e = abs(theta4-test_case[2][3])
t_5_e = abs(theta5-test_case[2][4])
t_6_e = abs(theta6-test_case[2][5])
print ("\nTheta 1 error is: %04.8f" % t_1_e)
print ("Theta 2 error is: %04.8f" % t_2_e)
print ("Theta 3 error is: %04.8f" % t_3_e)
print ("Theta 4 error is: %04.8f" % t_4_e)
print ("Theta 5 error is: %04.8f" % t_5_e)
print ("Theta 6 error is: %04.8f" % t_6_e)
print ("\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print (" ")
# Find FK EE error
if not(sum(your_ee)==3):
ee_x_e = abs(your_ee[0]-test_case[0][0][0])
ee_y_e = abs(your_ee[1]-test_case[0][0][1])
ee_z_e = abs(your_ee[2]-test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print ("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print ("End effector error for y position is: %04.8f" % ee_y_e)
print ("End effector error for z position is: %04.8f" % ee_z_e)
print ("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
## Set up code
## Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
req = Pose(comb)
start_time = time()
########################################################################################
#### Insert IK code here! starting at: Define DH parameter symbols
#if not os.path.exists("T0_EE.p"):
# Define DH param symbols
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8') # link offset
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7') # link length
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7') # twist angle
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8') # joint angle symbols
r, p, y = symbols('r p y')
# Modified DH params
DH_Table = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2.,
a1: 0.35,
d2: 0,
q2: -pi / 2. + q2,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2.,
a3: -0.054,
d4: 1.5,
q4: q4,
alpha4: pi / 2,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2.,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# define modified DH transformation matrix
def TF_Matrix(alpha, a, d, q):
TF = Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha) * d
], [0, 0, 0, 1]])
return TF
T0_1 = TF_Matrix(alpha0, a0, d1, q1).subs(DH_Table)
T1_2 = TF_Matrix(alpha1, a1, d2, q2).subs(DH_Table)
T2_3 = TF_Matrix(alpha2, a2, d3, q3).subs(DH_Table)
T3_4 = TF_Matrix(alpha3, a3, d4, q4).subs(DH_Table)
T4_5 = TF_Matrix(alpha4, a4, d5, q5).subs(DH_Table)
T5_6 = TF_Matrix(alpha5, a5, d6, q6).subs(DH_Table)
T6_EE = TF_Matrix(alpha6, a6, d7, q7).subs(DH_Table)
if not os.path.exists("T0_EE.p"):
T0_EE = simplify(T0_1 * T1_2 * T2_3 * T3_4 * T4_5 * T5_6 * T6_EE)
pickle.dump(T0_EE, open("T0_EE.p", "wb"))
R0_3 = simplify(T0_1[0:3, 0:3] * T1_2[0:3, 0:3] * T2_3[0:3, 0:3])
pickle.dump(R0_3, open("R0_3.p", "wb"))
# Find EE rotation matrix
# Define RPY roation matrices
R_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r),
cos(r)]]) # roll
R_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0,
cos(p)]]) # pitch
R_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0,
1]]) # yaw
R_EE = simplify(R_z * R_y * R_x)
# Error Correction Matrix for DH and URDF difference
R_Error = R_z.subs(y, radians(180)) * R_y.subs(p, radians(-90))
R_EE = simplify(R_EE * R_Error)
pickle.dump(R_EE, open("R_EE.p", "wb"))
else:
T0_EE = pickle.load(open("T0_EE.p", "rb"))
R0_3 = pickle.load(open("R0_3.p", "rb"))
R_EE = pickle.load(open("R_EE.p", "rb"))
# Extract end-effector position and orientation from request
# px, py, pz = end-effector position
# roll, pitch, yaw = end-effector orientation
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
R_EE = R_EE.subs({'r': roll, 'p': pitch, 'y': yaw})
EE = Matrix([[px], [py], [pz]])
WC = EE - (0.303) * R_EE[:, 2]
theta1 = atan2(WC[1], WC[0]).evalf()
# SSS triangle for theta2 and theta3
A = 1.501
Perp = WC[2] - 0.75
Base = sqrt(WC[0] * WC[0] + WC[1] * WC[1]) - 0.35
B = sqrt(Perp * Perp + Base * Base)
phi = atan2(Perp, Base)
C = 1.25
a = acos((B * B + C * C - A * A) / (2 * B * C))
b = acos((A * A + C * C - B * B) / (2 * A * C))
c = acos((A * A + B * B - C * C) / (2 * A * A))
theta2 = (pi / 2. - a - phi).evalf()
gamma = atan2(0.054, 1.5) # Tilt angle from joint3 to joint5
theta3 = (
pi / 2. -
(b + gamma)).evalf() # 0.036 accounts for sag in link4 of -0.054m
R0_3 = R0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
R3_6 = R0_3.transpose() * R_EE
# Euler angles from rotation matrix
theta5 = atan2(sqrt(R3_6[0, 2] * R3_6[0, 2] + R3_6[2, 2] * R3_6[2, 2]),
R3_6[1, 2]).evalf()
if (sin(theta5) > 0):
theta4 = atan2(R3_6[2, 2], -R3_6[0, 2]).evalf()
theta6 = atan2(-R3_6[1, 1], R3_6[1, 0]).evalf()
elif (theta5 == 0):
theta4 = 0
theta6 = atan2(-R3_6[0, 1], -R3_6[2, 1]).evalf()
else:
theta4 = atan2(-R3_6[2, 2], R3_6[0, 2]).evalf()
theta6 = atan2(R3_6[1, 1], -R3_6[1, 0]).evalf()
while (theta4 > pi):
theta4 = theta4 - 2 * pi
while (theta4 < -pi):
theta4 = theta4 + 2 * pi
while (theta5 > pi):
theta5 = theta5 - 2 * pi
while (theta5 < -pi):
theta5 = theta5 + 2 * pi
while (theta6 > pi):
theta6 = theta6 - 2 * pi
while (theta6 < -pi):
theta6 = theta6 + 2 * pi
########################################################################################
########################################################################################
## For additional debugging add your forward kinematics here. Use your previously calculated thetas
## as the input and output the position of your end effector as your_ee = [x,y,z]
FK = T0_EE.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
## End your code input for forward kinematics here!
########################################################################################
## For error analysis please set the following variables of your WC location and EE location in the format of [x,y,z]
your_wc = [WC[0], WC[1],
WC[2]] # <--- Load your calculated WC values in this array
your_ee = [
FK[0, 3], FK[1, 3], FK[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
########################################################################################
## Error analysis
print(
"Total run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
'''
print ("\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have muliple positions. It is best to add your forward kinmeatics to \
\nconfirm whether your code is working or not**")
print (" ")
'''
# FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
def test_code(test_case):
# Set up code
# Do not modify!
x = 0
class Position:
def __init__(self, EE_pos):
self.x = EE_pos[0]
self.y = EE_pos[1]
self.z = EE_pos[2]
class Orientation:
def __init__(self, EE_ori):
self.x = EE_ori[0]
self.y = EE_ori[1]
self.z = EE_ori[2]
self.w = EE_ori[3]
position = Position(test_case[0][0])
orientation = Orientation(test_case[0][1])
class Combine:
def __init__(self, position, orientation):
self.position = position
self.orientation = orientation
comb = Combine(position, orientation)
class Pose:
def __init__(self, comb):
self.poses = [comb]
def build_transform(a, d, q, alpha):
return Matrix([[cos(q), -sin(q), 0, a],
[
sin(q) * cos(alpha),
cos(q) * cos(alpha), -sin(alpha), -sin(alpha) * d
],
[
sin(q) * sin(alpha),
cos(q) * sin(alpha),
cos(alpha),
cos(alpha * d)
], [0, 0, 0, 1]])
# link lengths
a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')
# link offsets
d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')
# joint angles
q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')
# twist angles
alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols(
'alpha0:7')
# DH parameter table
dh_table = {
alpha0: 0,
a0: 0,
d1: 0.75,
q1: q1,
alpha1: -pi / 2,
a1: 0.35,
d2: 0,
q2: -pi / 2 + q2,
alpha2: 0,
a2: 1.25,
d3: 0,
q3: q3,
alpha3: -pi / 2,
a3: -0.054,
d4: 1.50,
q4: q4,
alpha4: pi / 2,
a4: 0,
d5: 0,
q5: q5,
alpha5: -pi / 2,
a5: 0,
d6: 0,
q6: q6,
alpha6: 0,
a6: 0,
d7: 0.303,
q7: 0
}
# transformation matrices between frames
t0_1 = build_transform(a0, d1, q1, alpha0).subs(dh_table)
t1_2 = build_transform(a1, d2, q2, alpha1).subs(dh_table)
t2_3 = build_transform(a2, d3, q3, alpha2).subs(dh_table)
t3_4 = build_transform(a3, d4, q4, alpha3).subs(dh_table)
t4_5 = build_transform(a4, d5, q5, alpha4).subs(dh_table)
t5_6 = build_transform(a5, d6, q6, alpha5).subs(dh_table)
t6_7 = build_transform(a6, d7, q7, alpha6).subs(dh_table)
# Base to end-effector transform
t0_7 = t0_1 * t1_2 * t2_3 * t3_4 * t4_5 * t5_6 * t6_7
# roll, pitch, yaw respectively
r, p, y = symbols('r p y')
# build out the rotation matrices for x, y, and z respectively
rot_x = Matrix([[1, 0, 0], [0, cos(r), -sin(r)], [0, sin(r), cos(r)]])
rot_y = Matrix([[cos(p), 0, sin(p)], [0, 1, 0], [-sin(p), 0, cos(p)]])
rot_z = Matrix([[cos(y), -sin(y), 0], [sin(y), cos(y), 0], [0, 0, 1]])
# multiply them together to yield the cumulative rotation matrix
rot_7 = rot_z * rot_y * rot_x
# apply the correction to the rotation matrix. this adjusts the values into our coordinate space
rot_correction = rot_z.subs(y, radians(180)) * rot_y.subs(p, radians(-90))
rot_7 = rot_7 * rot_correction
req = Pose(comb)
start_time = time()
# read in the positions
px = req.poses[x].position.x
py = req.poses[x].position.y
pz = req.poses[x].position.z
# read in the roll, pitch, yaw as euler angles from the received quaternion
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
req.poses[x].orientation.x, req.poses[x].orientation.y,
req.poses[x].orientation.z, req.poses[x].orientation.w
])
rot_ee = rot_7.subs({'r': roll, 'p': pitch, 'y': yaw})
end_effector = Matrix([[px], [py], [pz]])
wrist_center = end_effector - (0.303) * rot_ee[:, 2]
theta1 = atan2(wrist_center[1], wrist_center[0])
# SSS triangles for thetas 2 and 3
side_a = 1.50
side_b = sqrt(
pow((sqrt(wrist_center[0] * wrist_center[0] +
wrist_center[1] * wrist_center[1]) - 0.35), 2) +
pow((wrist_center[2] - 0.75), 2))
side_c = 1.25
# solve the angles in the triangle
A = acos((side_b * side_b + side_c * side_c - side_a * side_a) /
(2 * side_b * side_c))
B = acos((side_a * side_a + side_c * side_c - side_b * side_b) /
(2 * side_a * side_c))
D = atan2(
wrist_center[2] - 0.75,
sqrt(wrist_center[0] * wrist_center[0] +
wrist_center[1] * wrist_center[1]) - 0.35)
# we know the angles above plus our angles less any offsets should add to 90 degrees
theta2 = pi / 2 - (A + D)
theta3 = pi / 2 - (B + 0.036) # due to the 0.054 sag in link 4
# build the rotation matrix from 0 to 3 and sub in our thetas for the first 3 joints
rot0_3 = t0_1[0:3, 0:3] * t1_2[0:3, 0:3] * t2_3[0:3, 0:3]
rot0_3 = rot0_3.evalf(subs={q1: theta1, q2: theta2, q3: theta3})
# because the matrix is orthonormal we can use transpose instead of inv for the rotation matrix from 3 to 6
rot3_6 = rot0_3.transpose() * rot_ee
# solving the euler angles for the rotation matrix from 3 to 6
theta4 = atan2(rot3_6[2, 2], -rot3_6[0, 2])
theta5 = atan2(
sqrt(rot3_6[0, 2] * rot3_6[0, 2] + rot3_6[2, 2] * rot3_6[2, 2]),
rot3_6[1, 2])
theta6 = atan2(-rot3_6[1, 1], rot3_6[1, 0])
fk = t0_7.evalf(subs={
q1: theta1,
q2: theta2,
q3: theta3,
q4: theta4,
q5: theta5,
q6: theta6
})
your_wc = [wrist_center[0], wrist_center[1], wrist_center[2]
] # <--- Load your calculated WC values in this array
your_ee = [
fk[0, 3], fk[1, 3], fk[2, 3]
] # <--- Load your calculated end effector value from your forward kinematics
# Error analysis
print(
"\nTotal run time to calculate joint angles from pose is %04.4f seconds"
% (time() - start_time))
# Find WC error
if not (sum(your_wc) == 3):
wc_x_e = abs(your_wc[0] - test_case[1][0])
wc_y_e = abs(your_wc[1] - test_case[1][1])
wc_z_e = abs(your_wc[2] - test_case[1][2])
wc_offset = sqrt(wc_x_e**2 + wc_y_e**2 + wc_z_e**2)
print("\nWrist error for x position is: %04.8f" % wc_x_e)
print("Wrist error for y position is: %04.8f" % wc_y_e)
print("Wrist error for z position is: %04.8f" % wc_z_e)
print("Overall wrist offset is: %04.8f units" % wc_offset)
# Find theta errors
t_1_e = abs(theta1 - test_case[2][0])
t_2_e = abs(theta2 - test_case[2][1])
t_3_e = abs(theta3 - test_case[2][2])
t_4_e = abs(theta4 - test_case[2][3])
t_5_e = abs(theta5 - test_case[2][4])
t_6_e = abs(theta6 - test_case[2][5])
print("\nTheta 1 error is: %04.8f" % t_1_e)
print("Theta 2 error is: %04.8f" % t_2_e)
print("Theta 3 error is: %04.8f" % t_3_e)
print("Theta 4 error is: %04.8f" % t_4_e)
print("Theta 5 error is: %04.8f" % t_5_e)
print("Theta 6 error is: %04.8f" % t_6_e)
print(
"\n**These theta errors may not be a correct representation of your code, due to the fact \
\nthat the arm can have multiple positions. It is best to add your forward kinematics to \
\nconfirm whether your code is working or not**")
print(" ")
# Find FK EE error
if not (sum(your_ee) == 3):
ee_x_e = abs(your_ee[0] - test_case[0][0][0])
ee_y_e = abs(your_ee[1] - test_case[0][0][1])
ee_z_e = abs(your_ee[2] - test_case[0][0][2])
ee_offset = sqrt(ee_x_e**2 + ee_y_e**2 + ee_z_e**2)
print("\nEnd effector error for x position is: %04.8f" % ee_x_e)
print("End effector error for y position is: %04.8f" % ee_y_e)
print("End effector error for z position is: %04.8f" % ee_z_e)
print("Overall end effector offset is: %04.8f units \n" % ee_offset)
