PureBasic Forums - English

http://www.purebasic.com
https://www.purebasic.fr/english/

(rudimentary) Inverse Kinematics

https://www.purebasic.fr/english/viewtopic.php?t=65378

Page 1 of 1

(rudimentary) Inverse Kinematics

Posted: Fri Apr 01, 2016 7:35 pm
by PureLust
Since I startet with Computeranimations in the early 90s, I was fascinated by inverse Kinematics.
I did never understood it - and still do not understand the math behind it.

Nevertheless I've done some research on it lately and gave it a try.

The following code is based on an inverse kinematic for a robot-arm with a humerus, ulna and gripper.
With the right MouseButton you can rotate the Gripper at his wrist, and
with the left MouseButton you can set the targetposition of its fingertip.

Anything else (angle and position of the humerus and the ulna) will be calculated by inverse Kinematics.
If the target-position could not be reached by the Arm, the screen will tint red.

Image

It's not perfect (as it only works correct to the right side of the base for unknown reason) and the code is a mess, ... but ... it's a beginning. ;)

Maybe you will have some fun with it or can use it for your own projects.

[EDIT2:] added some info-sources, slight improvement of Vectordraw-rendering to get a nicer looking robot-arm.
[EDIT3:] added some TrackBars to change the Bone-Length during Runtime.

Code: Select all

;	Sources for Information about inverse Kinematics:
;
; => https://software.intel.com/en-us/articles/character-animation-skeletons-and-inverse-kinematics
; => http://fr.mathworks.com/help/fuzzy/examples/modeling-inverse-kinematics-in-a-robotic-arm.html?s_tid=gn_loc_drop
; => https://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino/comment-page-1#comments
; => http://www.picaxeforum.co.uk/showthread.php?13723-Maths-Module-for-more-accurate-calculations
; => http://research.ijcaonline.org/volume58/number18/pxc3883858.pdf
; => http://thegreat-samira.blogspot.de/2012/09/robotic-arm-inverse-kinematics-on.html

EnableExplicit

#WinWidth = 640
#WinHeight = 600

Global.f BASE_HGT, HUMERUS, ULNA, GRIPPER, hum_sq, uln_sq;
Global xBase, yBase
Global xBase2, yBase2


Global.d bas_angle_r, bas_angle_d
Global.d	grip_angle_r, grip_off_z, grip_off_y, rdist
Global.d	s_w, s_w_sqrt, a1, a2
Global.d	shl_angle_r, shl_angle_d, shl_angle_dn
Global.d	elb_angle_r, elb_angle_d, elb_angle_dn;
Global.d wri_angle_d, wri_angle_dn, wrist_z, wrist_y	; /* wrist angle */

; Bone-Length
BASE_HGT	=	67.31
HUMERUS	=	180  
ULNA		=	160  
GRIPPER	=	55.00

hum_sq = HUMERUS * HUMERUS
uln_sq = ULNA * ULNA		  

Procedure.d DegreeToRadian(angle.d)
	ProcedureReturn #PI * angle / 180.0;
EndProcedure

Procedure.d RadianToDegree(angle.d)
	ProcedureReturn angle * (180.0 / #PI);
EndProcedure

Procedure MoveHand(xPos.d, yPos.d, grip_angle_d.d = 0)
	
	Protected.d x, y = xPos, z = -yPos
	Static PositionOK
	
	grip_angle_r = DegreeToRadian(grip_angle_d);    //grip angle in radians for use in calculations
	
	bas_angle_r = ATan2( x, y );
	
	bas_angle_d=RadianToDegree(bas_angle_r);
	
	rdist = Sqr(( x * x ) + ( y * y ));
	
	;   /* rdist is y coordinate For the arm */
	y = rdist;
	
	;   /* Grip offsets calculated based on grip angle */
	grip_off_y = ( Sin( grip_angle_r )) * GRIPPER;
	grip_off_z = ( Cos( grip_angle_r )) * GRIPPER;
	
	;   /* Wrist position */
	wrist_z = ( z - grip_off_z ); - BASE_HGT;
	wrist_y = y - grip_off_y	 ;
	
	;   /* Shoulder To wrist distance ( AKA sw ) */
	s_w = ( wrist_z * wrist_z ) + ( wrist_y * wrist_y );
	s_w_sqrt = Sqr( s_w )										;
	
	;   /* s_w angle To ground */
	;   //float a1 = ATan2( wrist_y, wrist_z );
	a1 = ATan2( wrist_z, wrist_y );
	
	;   /* s_w angle To humerus */
	a2 = ACos((( hum_sq - uln_sq ) + s_w ) / ( 2 * HUMERUS * s_w_sqrt ));
	
	;   /* shoulder angle */
	shl_angle_r = a1 + a2;
	shl_angle_d = RadianToDegree(shl_angle_r);
	shl_angle_dn = -( 180.0 - shl_angle_d )  ;
	
	;   /* elbow angle */
	elb_angle_r = ACos(( hum_sq + uln_sq - s_w ) / ( 2 * HUMERUS * ULNA ));
	elb_angle_d = RadianToDegree( elb_angle_r )									 ;
	elb_angle_dn = -( 180.0 - elb_angle_d )										 ;
	
	;   /* wrist angle */
	wri_angle_d = ( grip_angle_d - elb_angle_dn ) - shl_angle_dn;
	wri_angle_dn = -( 180.0 - wri_angle_d )							;
	
	If shl_angle_r <> 0
		
		PositionOK = #True
		
		#DrawingType = 2			; 1 = 2D Line-Drawing  ,  2 = Vector-Drawing
		
		Select #DrawingType
				
			Case	1	;  Linien-Zeichnung via 2D-Drawing
				
				If StartDrawing(CanvasOutput(0))
					
					xBase = 340 : yBase = GadgetHeight(0)/2
					
					Box(0,0,#WinWidth, #WinHeight,$ffffff)		; Clear canvas
					
					xBase2 = 0 : yBase2 = GadgetHeight(0)/2
					Circle(xPos+xBase2,yBase2+yPos,2,$ff0000)
					
					LineXY(xBase2, yBase2, xBase2 + HUMERUS * Sin((shl_angle_r)), yBase2 - HUMERUS * Cos((shl_angle_r)),$ff0000)
					xBase2 + HUMERUS * Sin((shl_angle_r))
					yBase2 - HUMERUS * Cos((shl_angle_r))
					
					LineXY(xBase2, yBase2, xBase2 + ULNA * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn)), yBase2 - ULNA * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn)),$ff0000)
					xBase2 + ULNA * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn))
					yBase2 - ULNA * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn))
					
					LineXY(xBase2, yBase2, xBase2 + GRIPPER * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn + wri_angle_dn)), yBase2 - GRIPPER * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn + wri_angle_dn)),$ff)
					StopDrawing()
				EndIf
				
			Case 2		; Verctordrawing
				
				If StartVectorDrawing(CanvasVectorOutput(0))
					xBase = 0 : yBase = GadgetHeight(0)/2
					
					; Clear canvas
					AddPathBox(0,0, GadgetWidth(0),GadgetHeight(0))
					VectorSourceColor($ffffffff)
					FillPath()
					
					; Set Color to #red and draw Base-Circle
					VectorSourceColor(RGBA(255, 0, 0, 255))
					AddPathCircle(xBase, yBase, 20)
					FillPath()
					
					; draw humerus and elbow-circle
					MovePathCursor (xBase + 22 * Sin((shl_angle_r)), yBase - 22 * Cos((shl_angle_r)))
					AddPathLine   (xBase + (HUMERUS-19) * Sin((shl_angle_r)), yBase - (HUMERUS-19) * Cos((shl_angle_r)))
					xBase = xBase + HUMERUS * Sin((shl_angle_r))
					yBase = yBase - HUMERUS * Cos((shl_angle_r))
					AddPathCircle(xBase, yBase, 10)
					StrokePath(14)
					
					; draw ulna and wrist-circle
					MovePathCursor (xBase + 19 * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn)), yBase - 19 * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn)))
					AddPathLine   (xBase + (ULNA-14) * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn)), yBase - (ULNA-14) * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn)))
					xBase = xBase + ULNA * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn))
					yBase = yBase - ULNA * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn))
					AddPathCircle(xBase, yBase, 8)
					StrokePath(8)
					
					; draw gripper (hand) and blue fingertip
					MovePathCursor (xBase + 14 * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn + wri_angle_dn)), yBase - 14 * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn + wri_angle_dn)))
					xBase = xBase + GRIPPER * Sin(DegreeToRadian(shl_angle_d + elb_angle_dn + wri_angle_dn))
					yBase = yBase - GRIPPER * Cos(DegreeToRadian(shl_angle_d + elb_angle_dn + wri_angle_dn))
					AddPathLine	(xBase,yBase)
					StrokePath(6)
					VectorSourceColor(RGBA(0, 0, 255, 255))
					AddPathCircle(xBase,yBase, 7)
					FillPath()
					
					StrokePath(6)
					StopVectorDrawing()
				EndIf
		EndSelect
		
	Else
		If PositionOK
			If StartDrawing(CanvasOutput(0))
				DrawingMode(#PB_2DDrawing_AlphaBlend)
				Box(0,0, GadgetWidth(0),GadgetHeight(0), $200000ff)
				StopDrawing()
			EndIf
			PositionOK = #False
			Debug "Position impossible to reach"
		EndIf
	EndIf
	
EndProcedure

Procedure	Resize_Gadgets()
	ResizeGadget(0,0,0,WindowWidth(0),WindowHeight(0)-80)
	ResizeGadget(1,70,WindowHeight(0)-75,WindowWidth(0)-75,20)
	ResizeGadget(2,70,WindowHeight(0)-50,WindowWidth(0)-75,20)
	ResizeGadget(3,70,WindowHeight(0)-25,WindowWidth(0)-75,20)
	ResizeGadget(4, 8,WindowHeight(0)-70,60,20)
	ResizeGadget(5, 8,WindowHeight(0)-45,60,20)
	ResizeGadget(6, 8,WindowHeight(0)-20,60,20)
EndProcedure

Define	RADIUS.f = 80.0
Define.f	zaxis,yaxis, xaxis, X=260, Y=-100
Define	angle, Quit = #False , FollowMouse = #False
Define	GripperAngle = 110, RotateGripper = #False, UpdateArm

#WinFlags = #PB_Window_SystemMenu | #PB_Window_SizeGadget | #PB_Window_ScreenCentered | #PB_Window_MaximizeGadget
If OpenWindow(0,0,0,#WinWidth,#WinHeight,"Inverse Kinematic  (left MB to move, right MB to rotate grip)", #WinFlags)
	CanvasGadget(0,0,0,10,10)
	TrackBarGadget(1,0,0,10,10,25,300)
	TrackBarGadget(2,0,0,10,10,45,500)
	TrackBarGadget(3,0,0,10,10,55,600)
	TextGadget(4,0,0,10,10,"Gripper")
	TextGadget(5,0,0,10,10,"Ulna")
	TextGadget(6,0,0,10,10,"Humerus")
	SetGadgetState(1, GRIPPER)
	SetGadgetState(2, ULNA)
	SetGadgetState(3, HUMERUS)
	Resize_Gadgets()
	AddWindowTimer(0,0,10)	 
	BindEvent(#PB_Event_SizeWindow, @Resize_Gadgets())
	MoveHand(x, y, GripperAngle)
	
	Repeat
		Define Event = WaitWindowEvent(10)
		UpdateArm = #False
		
		Select Event
			Case #PB_Event_Gadget
				Select EventGadget()
						Case 0
					Select EventType()
						Case #PB_EventType_LeftButtonDown
							FollowMouse = #True
						Case #PB_EventType_LeftButtonUp
							FollowMouse = #False
						Case #PB_EventType_RightButtonDown
							RotateGripper = #True
						Case #PB_EventType_RightButtonUp
							RotateGripper = #False
						Case #PB_EventType_MouseMove
							If FollowMouse
								xBase2 = 00 : yBase2 = GadgetHeight(0)/2
								X = GetGadgetAttribute(0, #PB_Canvas_MouseX) - xBase2
								Y = GetGadgetAttribute(0, #PB_Canvas_MouseY) - yBase2
							EndIf
					EndSelect
					UpdateArm = #True
				Case 1 To 3
					GRIPPER	= GetGadgetState(1)
					ULNA		= GetGadgetState(2)
					HUMERUS	= GetGadgetState(3)
					hum_sq	= HUMERUS * HUMERUS;
					uln_sq	= ULNA * ULNA		  ;
					UpdateArm = #True
			EndSelect
			
		Case #PB_Event_Timer
			If RotateGripper
				GripperAngle + 5 : If GripperAngle >= 360 : GripperAngle = 0 : EndIf
					UpdateArm = #True
				EndIf
		EndSelect
		
		If UpdateArm 
			MoveHand(x, y, GripperAngle)
		EndIf
		
		If Event = #PB_Event_CloseWindow : Quit = #True : EndIf
		
	Until Quit
EndIf

Re: (rudimentary) Inverse Kinematics

Posted: Fri Apr 01, 2016 8:46 pm
by [blendman]
ouhhhhhhhhhh my god, it's amazing !! :D

Thank you so much, it's wonderfull :shock:

Re: (rudimentary) Inverse Kinematics

Posted: Fri Apr 01, 2016 9:22 pm
by davido
@PureLust,
Very, very nice. 8)

Re: (rudimentary) Inverse Kinematics

Posted: Sat Apr 02, 2016 4:49 pm
by minimy
Very nice IK example! Robotic arm is comming!! :wink:

Re: (rudimentary) Inverse Kinematics

Posted: Sun Apr 03, 2016 10:17 am
by PureLust
Thank you for all the nice comments. :D
minimy wrote:... Robotic arm is comming!!
Yeah ... so far I was only able to control my selfprinted Robotarm with 'normal' kinetics (one angle after the other) and the endposition was more a guess than a precise position.
If I could implement this into my Arduino-code, I would be able to set the exact position where I want the Gripper go to ... yesss. :D

Little changes to the code in my first posting:
I've added some TrackBars. So you could easily change the length of the Bones (humerus, ulna and gripper) now and you can see in the code which values has to be changed to do this.

Hope you have some fun with it. :mrgreen:

Re: (rudimentary) Inverse Kinematics

Posted: Sun Apr 03, 2016 6:40 pm
by RSBasic
Very nice. :)

Re: (rudimentary) Inverse Kinematics

Posted: Sun Apr 03, 2016 9:45 pm
by Andre
Very nice, thank you :D
All times are UTC+01:00
Page 1 of 1

Powered by phpBB® Forum Software © phpBB Limited