Page History

Input:

xtargetPosX : LREAL;
ytargetPosY : LREAL;
ztargetPosZ : LREAL;

Output:

myInverseKinematics : ARRAY [0..7] OF LREAL;

Info

Type: Function

Responsible: Magnus, Simen

Last minute update of this improved the accuracy by taking the offsets of the rope connection point at the end-effector into the calculation. New code found at

This function calculates the rope lengths for each motor. It performs the same operation as as 24_7 Euclidean Distance, but is implemented to return the rope lengths for each motor. The initial positions for each cable is defined in this function and it returns the length for each motor in an array. The reason this is implemented is because it makes the code simpler in main programs. Mostly a utility function instead of running 24_7 Euclidean Distance for all motors in the main code.

The reason our inverse kinematic is as simple as this is because of the system. When we find the rope length for each motor, we can calculate the actual position of each motor in the end position as long as its calibrated and homed. Even though stated that this is the inverse kinematics, the full dynamics require 24_7 SpeedCalculation to be complete.

Concept taken from wikipedia.

Code Block

language	actionscript3
title	Code Overview
linenumbers	true
collapse	true

FUNCTION myInverseKinematics : 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;
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;

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

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

Space shortcuts

Page tree

Versions Compared

Old Version 1

New Version 2

Key