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