WR Home      Topic Home      Chapter:  1  2 
<Previous Next>

Chapter 02

Basics of Animation and Forward Kinematics

Page 13

Programming


So, it can be generalized that: For the purpose of graphical animation, if the joint J is rotated, then all the links which follows after, must be rotated to same degree and same direction.

M1 and M2
In order to achieve it programmatically, a function rotate_motor_logically is developed. It is nammed rotate motor "logically" because it does not rotates the motor "physically", it rotates the co-ordinates in the matrix lx,ly only. Based on this function another function may be developed like rotate_motor_graphically, which might show a graphical animation. For now the function is limited to numeric use only.
Also recall the matrix lx, ly:
lx, ly
Lets have a look at the parameters:
function [ successFlag, nlx, nly ] = rotate_motor_logically( motor_no, angleInDegree, lx, ly, debugFlag)
Input parameters:
  1. motor_no: Specifies which motor is to be rotated, valid values: 1 and 2.
    Incase 1 is passed [lx(2), ly(2)] and [lx(3), ly(3)] are rotated
    Incase 2 is passed only [lx(3), ly(3)] is rotated
  2. angleInDegree: How much to rotate
  3. lx: Current, link x positions of the robotics arm
  4. ly: Current, link y positions of the robotics arm
  5. debugFlag: Prints all details to console, Set Verbose mode On or Off
Output parameters:
  1. successFlag: TRUE(1) if the rotation was sucessfull
  2. nlx: Modified (new) lx
  3. nly: Modified (new) ly

The heart of the function, are the two equations:
x' = ( x - xc )cosθ - (y-yc)sinθ + xc
y' = ( y - yc )sinθ + (y-yc)cosθ + yc


Input Arguments: 
	
	motor_no = 1
	angleInDegree  = 90
	lx =     [0   110   110]
	ly =     [0     0   140]
	debugFlag = 1

Function Called: [ successFlag, nlx, nly ] = rotate_motor_logically( motor_no, angleInDegree, lx, ly, debugFlag)
Output: 
	
	DEBUG: Before Rotating motor 1
	DEBUG:  lx[1] = 0.00      lx[2] = 110.00    lx[3] = 110.00 
	DEBUG:  ly[1] = 0.00      ly[2] = 0.00      ly[3] = 140.00 

	DEBUG: After Rotating motor 1 by 90.000000 degree
	DEBUG: nlx[1] = 0.00     nlx[2] = 0.00     nlx[3] = 140.00 
	DEBUG: nly[1] = 0.00     nly[2] = -110.00  nly[3] = -110.00

successFlag =     1


nlx =         0    0.0000  140.0000


nly =         0 -110.0000 -110.0000


Click here to download the code.
Click here to view the code and sample command-line run.
A compelete working example, using the function is given on the next page.

WR Home      Topic Home      Chapter:  1  2 
<Previous Next>