"write a program for forward kinematics and inverse kinematics of robot" Code Answer's

write a program for forward kinematics and inverse kinematics of robot

By Clean Cod on Oct 21, 2020

import math

ch = input("Enter '0' for  Forward Kinematics and '1' for Inverse Kinematics ")
if ch == "0":
    print('''Forward Kinematics
          through this program you'll be able to solve a FK problem
          which means given the values of joint angle and joint parameters,the position of the end effector
          can be defined it's like finding the position in a universal frame i.e called the coordinates of end-effector''')
    l1 = float(input("enter the length of link 1 "))
    l2 = float(input("enter the length of link 2 "))
    t1 = float(input("enter the value of theta 1 in degrees  "))
    t2 = float(input("enter the value of theta 2 in degrees  "))
    x = (l1 * math.cos(math.radians(t1))) + (l2 * math.cos(math.radians(t1 + t2)))
    y = (l1 * math.sin(math.radians(t1))) + (l2 * math.sin(math.radians(t1 + t2)))
    print("the x coordinate is ", x)
    print("the y coordinate is ", y)

### inverse kinematic begins from here
elif ch == "1":
    print('''Inverse Kinematics
          In this method we find the value of joint angles for a given joint parameters.
          According me this is what a most robots in sci-fi movies do they take inputs
          such as coordinate of final position of end-effector(or imagine the robot from movie big hero 6
                                                               if it has to move its hand it will use this method *not as simple as it sounds!) *_
          ''')
    l1 = float(input("enter the length of link 1 "))
    l2 = float(input("enter the length of link 2 "))
    w = float(input('enter the x-coordinates of end-effector'))
    v = float(input('enter the y-coordinates of end-effector'))
    a2 = math.degrees(math.cos(((w * w) + (v * v) - (l1 * l1) - (l2 * l2)) / (2 * l1 * l2)))
    a1 = math.degrees((math.atan(v / w)) - (
        math.atan((l2 * math.sin(math.radians(a2))) / ((l1) + (l2 * (math.cos(math.radians(a2))))))))
    print("theta 1 is ", a1)
    print("theta 2 is ", a2)
else:
    print("Enter a valid option")

Add Comment