def RRRXXX(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
## P63_1 = -d4
P63_1 = -robo.d[4]
## P63_2 = -r3*Sa4
P63_2 = -sin(robo.alpha[4])*robo.r[3]
## P63_3 = -r3*Ca4
P63_3 = -cos(robo.alpha[4])*robo.r[3]
A0 = array(invdata.T_GENERAL[:3, :3])
P0 = array(invdata.T_GENERAL[:3, 3])
g = -dot(A0, P0)
g = Matrix([g[0][0], g[1][0], g[2][0], 1])
## fc = [-d4; -Ca5*Sa4*r3; Sa5*Sa4*r3]
fc = Matrix([P63_1, cos(robo.alpha[5])*P63_2, -sin(robo.alpha[5])*P63_2, 0])
## fs = [Sa4*r3; -Ca5*d4; Sa5*d4]
fs = Matrix([-P63_2, cos(robo.alpha[5])*P63_1, -sin(robo.alpha[5])*P63_1, 0])
## fr = [0; Sa5; Ca5]
fr = Matrix([0, sin(robo.alpha[5]), cos(robo.alpha[5]), 0])
## f0 = [-d5; -Sa5*Ca4*r3; -Ca5*Ca4*r3]
f0 = Matrix([-robo.d[5], sin(robo.alpha[5])*P63_3, cos(robo.alpha[5])*P63_3, 1])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def XXRRRX(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
g = Matrix([
robo.d[3], -sin(robo.alpha[3]) * robo.r[3],
cos(robo.alpha[3]) * robo.r[3], 1
])
A0 = array(invdata.T_GENERAL[:3, :3])
P0 = array(invdata.T_GENERAL[:3, 3])
fc = dot(A0, [-robo.d[6], -robo.r[5] * sin(robo.alpha[6]), 0])
## fc = A0*[-d6; -Sa6*r5; 0]
fc = Matrix([fc[0], fc[1], fc[2], 0])
symo.write_line(fc)
## fs = A0*[-Sa6*r5; d6; 0]
fs = dot(A0, [-robo.r[5] * sin(robo.alpha[6]), robo.d[6], 0])
fs = Matrix([fs[0], fs[1], fs[2], 0])
## fr = -A0*[0; 0; 1]
fr = -dot(A0, [0, 0, 1])
fr = Matrix([fr[0], fr[1], fr[2], 0])
## f0 = A0*[0; 0; -Ca6*r5] + P0
f0 = sum(dot(A0, [0, 0, -robo.r[5] * cos(robo.alpha[6])]), P0)
f0 = Matrix([f0[0][0], f0[1][0], f0[2][0], 1])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def XXRRRX(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
g = Matrix([robo.d[3], -sin(robo.alpha[3])*robo.r[3], cos(robo.alpha[3])*robo.r[3], 1])
A0 = array(invdata.T_GENERAL[:3, :3])
P0 = array(invdata.T_GENERAL[:3, 3])
fc = dot(A0, [-robo.d[6], -robo.r[5]*sin(robo.alpha[6]), 0])
## fc = A0*[-d6; -Sa6*r5; 0]
fc = Matrix([fc[0], fc[1], fc[2], 0])
symo.write_line(fc)
## fs = A0*[-Sa6*r5; d6; 0]
fs = dot(A0, [-robo.r[5]*sin(robo.alpha[6]), robo.d[6], 0])
fs = Matrix([fs[0], fs[1], fs[2], 0])
## fr = -A0*[0; 0; 1]
fr = -dot(A0, [0, 0, 1])
fr = Matrix([fr[0], fr[1], fr[2], 0])
## f0 = A0*[0; 0; -Ca6*r5] + P0
f0 = sum(dot(A0, [0, 0, -robo.r[5]*cos(robo.alpha[6])]), P0)
f0 = Matrix([f0[0][0], f0[1][0], f0[2][0], 1])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def XXXRRR(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
## P36_1 = d4
P36_1 = robo.d[4]
## P36_2 = -r4*Sa4
P36_2 = -sin(robo.alpha[4])*robo.r[4]
## P36_3 = r4*Ca4
P36_3 = cos(robo.alpha[4])*robo.r[4]
[px,py,pz] = invdata.T_GENERAL[:3,3]
g = Matrix([px,py,pz,1])
## fc = [d4; -Ca3*Sa4*r4; -Sa3*Sa4*r4]
fc = Matrix([P36_1, cos(robo.alpha[3])*P36_2, sin(robo.alpha[3])*P36_2, 0])
## fs = [Sa4*r4; Ca3*d4; Sa3*d4]
fs = Matrix([-P36_2, cos(robo.alpha[3])*P36_1, sin(robo.alpha[3])*P36_1, 0])
## fr = [0; -Sa3; Ca3]
fr = Matrix([0, -sin(robo.alpha[3]), cos(robo.alpha[3]), 0])
## f0 = [d3; -Sa3*Ca4*r4; Ca3*Ca4*r4]
f0 = Matrix([robo.d[3], -sin(robo.alpha[3])*P36_3, cos(robo.alpha[3])*P36_3, 1])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def XRRRXX(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
g = Matrix([
robo.d[2], -sin(robo.alpha[2]) * robo.r[2],
cos(robo.alpha[2]) * robo.r[2], 1
])
A0 = array(invdata.T_GENERAL[:3, :3])
P0 = array(invdata.T_GENERAL[:3, 3])
## fc = A0*[d5-d6; r5*(Ca5*Sa6-Ca6*Sa5); 0]
fc = dot(A0, [
robo.d[5] - robo.d[6], robo.r[5] *
(cos(robo.alpha[5]) * sin(robo.alpha[6]) -
cos(robo.alpha[6]) * sin(robo.alpha[5])), 0
])
fc = Matrix([fc[0], fc[1], fc[2], 0])
## fs = A0*[r5*(Ca5*Sa6-Ca6*Sa5); d6-d5; 0]
fs = dot(A0, [
robo.r[5] *
(cos(robo.alpha[5]) * sin(robo.alpha[6]) -
cos(robo.alpha[6]) * sin(robo.alpha[5])), robo.d[6] - robo.d[5], 0
])
fs = Matrix([fs[0], fs[1], fs[2], 0])
## fr = A0*[0; 0; -1]
fr = dot(A0, [0, 0, -1])
fr = Matrix([fr[0], fr[1], fr[2], 0])
## f0 = A0*[0; 0; r5*C(a5-a6)] + P0
f0 = sum(dot(A0, [0, 0, robo.r[5] * cos(robo.alpha[5] - robo.alpha[6])]),
P0)
f0 = Matrix([f0[0][0], f0[1][0], f0[2][0], 1])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def RRRXXX(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
## P63_1 = -d4
P63_1 = -robo.d[4]
## P63_2 = -r3*Sa4
P63_2 = -sin(robo.alpha[4]) * robo.r[3]
## P63_3 = -r3*Ca4
P63_3 = -cos(robo.alpha[4]) * robo.r[3]
A0 = array(invdata.T_GENERAL[:3, :3])
P0 = array(invdata.T_GENERAL[:3, 3])
g = -dot(A0, P0)
g = Matrix([g[0][0], g[1][0], g[2][0], 1])
## fc = [-d4; -Ca5*Sa4*r3; Sa5*Sa4*r3]
fc = Matrix(
[P63_1,
cos(robo.alpha[5]) * P63_2, -sin(robo.alpha[5]) * P63_2, 0])
## fs = [Sa4*r3; -Ca5*d4; Sa5*d4]
fs = Matrix(
[-P63_2,
cos(robo.alpha[5]) * P63_1, -sin(robo.alpha[5]) * P63_1, 0])
## fr = [0; Sa5; Ca5]
fr = Matrix([0, sin(robo.alpha[5]), cos(robo.alpha[5]), 0])
## f0 = [-d5; -Sa5*Ca4*r3; -Ca5*Ca4*r3]
f0 = Matrix([
-robo.d[5],
sin(robo.alpha[5]) * P63_3,
cos(robo.alpha[5]) * P63_3, 1
])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def XXXRRR(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
## P36_1 = d4
P36_1 = robo.d[4]
## P36_2 = -r4*Sa4
P36_2 = -sin(robo.alpha[4]) * robo.r[4]
## P36_3 = r4*Ca4
P36_3 = cos(robo.alpha[4]) * robo.r[4]
[px, py, pz] = invdata.T_GENERAL[:3, 3]
g = Matrix([px, py, pz, 1])
## fc = [d4; -Ca3*Sa4*r4; -Sa3*Sa4*r4]
fc = Matrix(
[P36_1,
cos(robo.alpha[3]) * P36_2,
sin(robo.alpha[3]) * P36_2, 0])
## fs = [Sa4*r4; Ca3*d4; Sa3*d4]
fs = Matrix(
[-P36_2,
cos(robo.alpha[3]) * P36_1,
sin(robo.alpha[3]) * P36_1, 0])
## fr = [0; -Sa3; Ca3]
fr = Matrix([0, -sin(robo.alpha[3]), cos(robo.alpha[3]), 0])
## f0 = [d3; -Sa3*Ca4*r4; Ca3*Ca4*r4]
f0 = Matrix([
robo.d[3], -sin(robo.alpha[3]) * P36_3,
cos(robo.alpha[3]) * P36_3, 1
])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
def XRRRXX(robo, symo, com_key, X_joints, pieper_joints):
"""
Function that prints the symbolic solution of this decoupled robot case using PIEPER METHOD.
Parameters:
===========
1) X_joints: Joints that are out of spherical wrist
2) pieper_joints: Joints that the spherical wrist is composed of.
"""
g = Matrix([robo.d[2], -sin(robo.alpha[2])*robo.r[2], cos(robo.alpha[2])*robo.r[2], 1])
A0 = array(invdata.T_GENERAL[:3, :3])
P0 = array(invdata.T_GENERAL[:3, 3])
## fc = A0*[d5-d6; r5*(Ca5*Sa6-Ca6*Sa5); 0]
fc = dot(
A0,
[robo.d[5]-robo.d[6],
robo.r[5]*(cos(robo.alpha[5])*sin(robo.alpha[6])-cos(robo.alpha[6])*sin(robo.alpha[5])),
0]
)
fc = Matrix([fc[0], fc[1], fc[2], 0])
## fs = A0*[r5*(Ca5*Sa6-Ca6*Sa5); d6-d5; 0]
fs = dot(
A0,
[robo.r[5]*(cos(robo.alpha[5])*sin(robo.alpha[6])-cos(robo.alpha[6])*sin(robo.alpha[5])),
robo.d[6]-robo.d[5], 0]
)
fs = Matrix([fs[0], fs[1], fs[2], 0])
## fr = A0*[0; 0; -1]
fr = dot(A0, [0, 0, -1])
fr = Matrix([fr[0], fr[1], fr[2], 0])
## f0 = A0*[0; 0; r5*C(a5-a6)] + P0
f0 = sum(dot(A0, [0, 0, robo.r[5]*cos(robo.alpha[5]-robo.alpha[6])]), P0)
f0 = Matrix([f0[0][0], f0[1][0], f0[2][0], 1])
# Position Equations First
solve_position(robo, symo, com_key, X_joints, fc, fs, fr, f0, g)
# Then Orientation Equations
solve_orientation(robo, symo, pieper_joints)
return
