24_7 Inverse Kinematics with Offsets

Input:

targetPosX : LREAL;
targetPosY : LREAL;
targetPosZ : LREAL;

Output:

myInverseKinematicsOffsets: ARRAY [0..7] OF LREAL

Type: Function

Responsible: Magnus, Simen

New improved version of 24_7 Inverse Kinematics (Function)!

Adds the offsets from the end-effector rope connectors to the inverse kinematic to get a more accurate results. These were measured and added for each motor.

Code Overview

FUNCTION myInverseKinematicsOffsets : ARRAY [0..7] OF LREAL (* Calculates inverse kinematics for all 8 motors, returns 8 rope lengths *)
VAR_INPUT
	targetPosX : LREAL;
	targetPosY : LREAL;
	targetPosZ : LREAL;
END_VAR
VAR
	i : INT := 0;
	motor_positions : ARRAY [0..7] OF ARRAY [0..2] OF INT;
	deltaX, deltaY, deltaZ : LREAL;
	offsets : ARRAY [0..7] OF ARRAY [0..2] OF LREAL;
	
END_VAR

motor_positions[0][0] := 0;
motor_positions[0][1] := 0;
motor_positions[0][2] := 0;

motor_positions[1][0] := 138;
motor_positions[1][1] := 0;
motor_positions[1][2] := 0;

motor_positions[2][0] := 138;
motor_positions[2][1] := 102;
motor_positions[2][2] := 0;

motor_positions[3][0] := 0;
motor_positions[3][1] := 102;
motor_positions[3][2] := 0;

motor_positions[4][0] := 0;
motor_positions[4][1] := 0;
motor_positions[4][2] := 95;

motor_positions[5][0] := 138;
motor_positions[5][1] := 0;
motor_positions[5][2] := 95;

motor_positions[6][0] := 138;
motor_positions[6][1] := 102;
motor_positions[6][2] := 95;

motor_positions[7][0] := 0;
motor_positions[7][1] := 102;
motor_positions[7][2] := 95;

offsets[0][0] := -2.1213;
offsets[0][1] := -2.1213;
offsets[0][2] := -5.0;

offsets[1][0] := 2.1213;
offsets[1][1] := -2.1213;
offsets[1][2] := -5.0;

offsets[2][0] := 2.1213;
offsets[2][1] := 2.1213;
offsets[2][2] := -5.0;

offsets[3][0] := -2.1213;
offsets[3][1] := 2.1213;
offsets[3][2] := -5.0;

offsets[4][0] := 0.0;
offsets[4][1] := -3.0;
offsets[4][2] := 5.0;

offsets[5][0] := 3.0;
offsets[5][1] := 0.0;
offsets[5][2] := 5.0;

offsets[6][0] := 0.0;
offsets[6][1] := 3.0;
offsets[6][2] := 5.0;

offsets[7][0] := -3.0;
offsets[7][1] := 0.0;
offsets[7][2] := 5.0;





i := 0;
FOR i := 0 TO 7 DO
	deltaX := ABS(motor_positions[i][0] - targetPosX + offsets[i][0]);
	deltaY := ABS(motor_positions[i][1] - targetPosY + offsets[i][1]);
	deltaZ := ABS(motor_positions[i][2] - targetPosZ + offsets[i][2]);

	
	myInverseKinematicsOffsets[i] := SQRT(deltaX*deltaX + deltaY*deltaY + deltaZ*deltaZ); // Funker ikke med POW eller **
	
END_FOR

Space shortcuts

Page tree