Tillämpad Matematik i programmering

Explore

Trigg

I Robotik
I Robotik

There are 2 mostly used kinematics in robotic field, they are : forward kinematics and inverse kinematics. Forward kinematics is frequently used to calculate the position of end effector when we know the degree value of each joint, meanwhile inverse kinematics is used to compute the degree of each joint when we know the position of the end effector.

To calculate forward kinematic, we can use simple trigonometry or denavit hartenberg parameter or screw theory . In this example we are going to use simple trigonometry to calculate 2d forward kinematics for 1 DOF and 3d forward kinematics for 3 DOF robotic arm.

Calculating 2D Forward Kinematics for 1 DOF robot arm

⁠

image.jpeg failed to upload

⁠

For example here we have 1 dof robotic arm. link length is 10 cm. θ is 45°. The position of end effector on our cartesian coordinate (x, y) can be calculated easily using simple trigonometry.

⁠

image.gif failed to upload

⁠

image.jpeg failed to upload

⁠

cos θ = x / l

x = cos θ * l = cos 45° * 10 cm

sin θ = y / l

y = sin θ * l = sin 45° * 10 cm

Here is python 2d 1 dof forward kinematic solver:

[code language="python"]

#!/usr/bin/env python3

'''

robotic forward kinematics solver

coded by Antonius Ringlayer

'''

import math

def solve_1dof_fk(_link, _degree):

try:

x = 0

y = 0

x = math.cos(math.radians(_degree)) * _link

y = math.sin(math.radians(_degree)) * _link

return x, y

except:

raise

x, y = solve_1dof_fk(10, 45)

print ("x=" + str(x) + " y=" + str(y))

[/code]

Result:

$ ./2d_1dof_forward_kinematic_solver.py

x=7.0710678118654755 y=7.071067811865475

Position of end effector = p(7.07 cm, 7.07 cm)

Calculating 3D Forward Kinematics for 3 DOF robot arm

Calculating the position of the end effector on 3 dimensional space using trigonometry is not so hard.

For example here we have 3 dof robot arm :

Side view of robot arm :

⁠

image.jpeg failed to upload

⁠

Where : d2 is the height of second dof towards the floor, z is another dimension that we add to our cartesian geometry (the height of end effector from the floor), l1 = length of link 1, l2 = length of link 2, θ2 is d2 joint value, θ3 is d3 joint value.

Top View of Robot Arm :

⁠

toper.jpg629×697 59.3 KB⁠

⁠

Cartesian coordinate represented from the top view of our robotic arm. θ1 is d1 joint value.

We are going to calculate the position of end effector (E) at 3 dimensional spaces (x, y , z).

Known variables : °

l1 = l2 = 10 cm

d2 = 7 cm

θ1 = 60°

θ2 = 30°

θ3 = 140°

We mark some more degrees and lengths :

⁠

side25.jpg680×491 75.9 KB⁠

⁠

Steps:

since z = d2 + d3 - d6, first step is finding d3

step 1. finding d3

sin θ2 = d3 / l1

sin 30° = d3 / 10

d3 = sin 30° * 10 = 4.9 cm

Next step is finding d2 and d6 length. In order to get d2 and d6 length, we need to get more informations.

step 2. find θ4

180° = θ2 + 90° + θ4

θ4 = 180° - (30° + 90°) = 60°

step 3. find θ5

θ3 = θ4 + θ5

140° = 60° + θ5 , hence θ5 = 80°

Based on all informations that we obtained, we can redraw our picture as follow:

⁠

side32.jpg680×491 80.8 KB⁠

⁠

Step 6. finding d6 and z

Since we have θ5, finding d6 is simply child play.

cos θ5 = d6 / l2

cos 80° = d6 / 10

d6 = cos 80° * 10 = 1.7 cm

So we have below information:

z = 7 cm + 4.9 cm - 1.7 cm = 13,6 cm

Next we need to find x and y.

⁠

image.jpeg failed to upload

⁠

We figure out that x and y can be obtained if we got the hypotenuse length (d1).

We figure out from side view that d1 = d4 + d5

step 7. finding d4

cos θ2 = d4 / l1

d4 = cos 30° * 10 = 8.66 cm

step 8. finding d5 and d1

since we have θ5 = 80°

sin 80° = d5 / 10, so d5 = sin 80° * 10 = 9.85 cm.

d1 = d4 + d5 = 8.66 + 9.85 = 18.51 cm

Back again to our top view, we figure out that we have collected enough information to find x and y.

⁠

d1.jpg574×636 63.8 KB⁠

⁠

cos θ1 = x / d1

cos 60° = x / 18.51

x = cos 60° * 18.51 = 9.25 cm

sin 60° = y / 18.51 , y = sin 60° * 18.51 = 16.03 cm

Finally we find that p(x,y,z) = p(9.25 , 16.03, 13,6)

Want to print your doc?
This is not the way.

Try clicking the ⋯ next to your doc name or using a keyboard shortcut (

CtrlP

) instead.

I RobotikI Robotik

I Robotik
I Robotik