(rudimentary) Inverse Kinematics

Share your advanced PureBasic knowledge/code with the community.
Post Reply
PureLust
Enthusiast
Enthusiast
Posts: 477
Joined: Mon Apr 16, 2007 3:57 am
Location: Germany, NRW

(rudimentary) Inverse Kinematics

Post 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
Last edited by PureLust on Sun Apr 03, 2016 10:21 am, edited 3 times in total.
[Dynamic-Dialogs] - create complex GUIs the easy way
[DeFlicker] - easily deflicker your resizeable Windows
[WinFX] - Window Effects (incl. 'click-through' Window)
Top
User avatar
[blendman]
Enthusiast
Enthusiast
Posts: 297
Joined: Thu Apr 07, 2011 1:14 pm
Location: 3 arks
Contact:
Contact [blendman]

Re: (rudimentary) Inverse Kinematics

Post by [blendman] »

ouhhhhhhhhhh my god, it's amazing !! :D

Thank you so much, it's wonderfull :shock:
Top
davido
Addict
Addict
Posts: 1890
Joined: Fri Nov 09, 2012 11:04 pm
Location: Uttoxeter, UK

Re: (rudimentary) Inverse Kinematics

Post by davido »

@PureLust,
Very, very nice. 8)
DE AA EB
Top
User avatar
minimy
Enthusiast
Enthusiast
Posts: 630
Joined: Mon Jul 08, 2013 8:43 pm
Location: off world

Re: (rudimentary) Inverse Kinematics

Post by minimy »

Very nice IK example! Robotic arm is comming!! :wink:
If translation=Error: reply="Sorry, Im Spanish": Endif
Top
PureLust
Enthusiast
Enthusiast
Posts: 477
Joined: Mon Apr 16, 2007 3:57 am
Location: Germany, NRW

Re: (rudimentary) Inverse Kinematics

Post 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:
[Dynamic-Dialogs] - create complex GUIs the easy way
[DeFlicker] - easily deflicker your resizeable Windows
[WinFX] - Window Effects (incl. 'click-through' Window)
Top
User avatar
RSBasic
Moderator
Moderator
Posts: 1228
Joined: Thu Dec 31, 2009 11:05 pm
Location: Gernsbach (Germany)
Contact:
Contact RSBasic

Re: (rudimentary) Inverse Kinematics

Post by RSBasic »

Very nice. :)
Image
Image
Top
User avatar
Andre
PureBasic Team
PureBasic Team
Posts: 2139
Joined: Fri Apr 25, 2003 6:14 pm
Location: Germany (Saxony, Deutscheinsiedel)
Contact:
Contact Andre

Re: (rudimentary) Inverse Kinematics

Post by Andre »

Very nice, thank you :D
Bye,
...André
(PureBasicTeam::Docs & Support - PureArea.net | Order:: PureBasic | PureVisionXP)
Top
Post Reply

Return to “Tricks 'n' Tips”