#!/usr/bin/env python import math as m import numpy as np def main(): # Length of each links for the planar 3R robot l1 = 5 l2 = 7 l3 = 2 # Given the following transformation from 0 to P: T0_P = np.mat([[-0.5, -0.8660254, 0, 5.34327225], [0.8660254, -0.5, 0, 10.6066229], [0, 0, 1, 0], [0, 0, 0, 1]]) # Find at least 2 solutions sets [theta_1, theta_2, theta_3] for the robot # manipulator position. # Use IK formulas learned from the lecture. # Get the x, y coordinate from T0_P: x, y = ###CODE#HERE### print('x, y coord: {},{}'.format(x,y)) # If surprised by the order here, it is recommended to review the lecture. # You are welcome to add as many intermediate steps as you judge necessary. #_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_# # Here starts the calculations for the first set of solutions: print(" Solution 1: ") # Theta 2 cos_theta_2 = ###CODE#HERE### print("Cosinus Theta 2: {}".format(cos_theta_2)) sin_theta_2 = ###CODE#HERE### print("Sinus Theta 2: {}".format(sin_theta_2)) theta_2 = ###CODE#HERE### # Theta 1 k1 = ###CODE#HERE### k2 = ###CODE#HERE### theta_1 = ###CODE#HERE### # Theta 3 phi = ###CODE#HERE### print("Phi: {}".format(m.degrees(phi))) theta_3 = ###CODE#HERE### # 1st set of solution with angles converted to degrees solution_1 = [m.degrees(theta_1), m.degrees(theta_2), m.degrees(theta_3)] print("Theta 1, 2, and 3: {}".format(solution_1)) #_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_*-*_# # Here starts the calculations for the second set of solutions: print(" Solution 2: ") # Theta 2 cos_theta_2 = ###CODE#HERE### print("Cosinus Theta 2: {}".format(cos_theta_2)) sin_theta_2 = ###CODE#HERE### print("Sinus Theta 2: {}".format(sin_theta_2)) theta_2 = ###CODE#HERE### # Theta 1 k1 = ###CODE#HERE### k2 = ###CODE#HERE### theta_1 = ###CODE#HERE### # Theta 3 phi = ###CODE#HERE### print("Phi: {}".format(m.degrees(phi))) theta_3 = ###CODE#HERE### # 2nd set of solution with angles converted to degrees solution_2 = [m.degrees(theta_1), m.degrees(theta_2), m.degrees(theta_3)] print("Theta 1, 2, and 3: {}".format(solution_2)) if __name__ == '__main__': main()

The goal is given a Transformation T0_P, find at least two solutions sets [theta_1, theta_2, theta_3] for the robot manipulator x, y position.

We are using a planar 3 revolute joints robot that is identical to the example given in the lecture.

3R Planar Manipulator Algebraic Approach Recall (2)-(4): -5 0 x1 12) T=SC0y To 0 1 0 lo 0 0 1] C =C123 S = $123 8, +82 + 3 = Atan2(5.co) = 83 = Atan2 (s.co)- 8.-02 (27) 3R Planar Manipulator Algebraic Approach Check for multiple solutions!!!!!!!!!!!!!!!!!!! @= Atan2(2.) (10) 8, = Atan2(y,x) - Atan2(kz,k) (24) z = Atan2(5.C)-, -, (27) Solve (7) for cz. Gry? S2 = 1,1- (9) The choice of sign in equation (9) results in different OP-- - -values for Sz. This choice affects all three angles. There are 2 solutions to this IK problem Sol A: [...0240341 Sol B:[@.02.8381 21,1 Do solutions exist? Check for constraints!!!!!!!!!!!!!!!! Solve (7) for C. +--+ 52 Dashboard Calendar To Do Notifications Inbox 3R Planar Manipulator Algebraic Approach Recall (2)-(4): -5 0 x1 12) T=SC0y To 0 1 0 lo 0 0 1] C =C123 S = $123 8, +82 + 3 = Atan2(5.co) = 83 = Atan2 (s.co)- 8.-02 (27) 3R Planar Manipulator Algebraic Approach Check for multiple solutions!!!!!!!!!!!!!!!!!!! @= Atan2(2.) (10) 8, = Atan2(y,x) - Atan2(kz,k) (24) z = Atan2(5.C)-, -, (27) Solve (7) for cz. Gry? S2 = 1,1- (9) The choice of sign in equation (9) results in different OP-- - -values for Sz. This choice affects all three angles. There are 2 solutions to this IK problem Sol A: [...0240341 Sol B:[@.02.8381 21,1 Do solutions exist? Check for constraints!!!!!!!!!!!!!!!! Solve (7) for C. +--+ 52 Dashboard Calendar To Do Notifications Inbox