mimbot: gesture-imitating robotic system final report

MimBot: Gesture-Imitating Robotic System FinalReport

Brielle GrahamDepartment of Electrical and Computer Engineering

Rutgers, The State University of New JerseyNew Brunswick, NJ 08901

Bao HuynhDepartment of Electrical and Computer Engineering


Randall ReyesDepartment of Electrical and Computer Engineering


March 29, 2012

Abstract

This project will develop a system to interface between a human controller anda robot arm. The system will use an inexpensive webcam to record the motion ofthe controllers hand and will translate that motion into equivalent movement in therobot arm. This project will integrate motion tracking and reconstruction conceptsof Robotics and Computer Vision and Electrical Circuits in order to create a motion-imitating system. The completion of this project will be an advance in gesture-basedtrackers, as the majority of such commercially available systems are constrained by theuse of typically expensive gloves or specialized infrared cameras.

1

1 Objective

A human-robot interface system based on visual tracking without requiring specialized track-ing equipment.

2 Procedure

2.1 Primary Components

The following is a list of the primary components to be implemented in the system.Input: access a live feed of video from the cameraRecognization and Tracking: identify and record the change in position of the controller’shandCalibration: determine the characteristic parameters of the system to enable reconstructionReconstruction: determine real-world position of the robot armModel Construction: build robot arm capable of motion in 3 dimensions and imitating apincher motion

2.2 Implementation of Components

2.2.1 Input

The system incorporates live feed from dual cameras. The system accesses the cameras se-quentially fromMATLAB, enabling near-simultaneous data-acquisition and video previewingfrom both cameras. To approximate a continuous feed, the program retrieved data for a largenumber (< 1000) frames. The frame acquisition rate was adjusted to account for the speedlimitations of the robot arm.

2.2.2 Recognization and Tracking

Early incarnations of the system relied on SURF implementation for recognization. How-ever it was soon determined that SURF was not able to provide both the accuracy andspeed needed. An alternative method using edge detection was also explored, however theMATLAB code ’Tracking red color objects using matlab’ written by A. Bhargav Anand (ofAmrita University, Bangalore, India) was ultimately chosen.

For each frame of the video, the difference method is used to isolate red in the image andproduce the x,y pixel coordinates of red points. This can be adjusted to track blue andyellow points. The controller would be required to attach a red and yellow marker to theindex finger and thumb respectively, and to attach a blue marker to the elbow. The distancebetween the red and yellow markers is used to determine the openness of pincers. The orien-tation of the red and blue determine the orientation of the arm. The horizontal movementof the red is used to determine azimuth, and the position in relation to the blue marker is

2

used to determine elevation.

In order to determine azimuth rotation, the magnitude of the change of position of the redmarker was noted and interpreted as equivalent rotation with a constant scale factor.

Stereo reconstruction was used to recreate the orientation between the red and blue anddetermine the angle of elevation.

2.2.3 Calibration

In this previous incarnation of the project, the method of Calibrated Stereo Reconstructionwas utilized to reconstruct the 3D location of a point in the world once its 2D locations havebeen calculated. Here, the CalTech Toolbox is used to calibrate the camera. Also, from theCalTech calibration toolbox, the extrinsic parameters matrices l

wR,lw T,rw R,rw T of the leftand right cameras respectively are obtained. From these matrices, the following calculationsare done:

Equation 2:

lwM =

[lwR

lwT

0 0 0 1

]and

rwM =

[rwR

rwT

0 0 0 1

]

⇒wr M = (rwM)T =

[(rwR)T −(rwR)T (rwT )

0 0 0 1

]

⇒lr M = (lwM)(wr M), from which l

rR and lrT are obtained

From the CalTech calibration toolbox, the following intrinsic parameters are computed:fx = 732.3202, fy = 708.7952, Ox = 423.1368, Oy = 171.7976. Here it is assumed that theaspect ratio is 1, in that case the focal length can be calculated by taking the arithmeticmean of fx and fy. So f u 720.

In the current system, the calibration first approximates the length of the arm. A minimumof two pictures are necessary for a rough estimation. The first picture is of the controller’shand in the ’Initial Position’, that is, the arm is vertical and index and middle fingers arevertical and parallel. The x and y pixel coordinates of the markers at this position arerecorded and z value is set as zero. Subsequent images are taken of the controller holdingthe arm at different angles parallel to the camera, with the elbow position fixed. The pixelcoordinates are recorded, and the z coordinates are all assumed to be zero. The resultant

3

motion is along a two-dimensional quarter-circle, therefore the Equation 1 defined previouslybecomes

Equation 2: r2 = (x− xe)2 + (y − ye)

2

The x,y coordinates of a marker in the initial position are given as xo, yo. From the geometryof the arm it can be inferred that the 2-dimensional position of the elbow xe, ye differs fromthe position of the marker by a translation in the y direction, specifically, a translation thelength of the arm. As MATLAB assigns the 0,0 position to the top left of the image, thistranslation is positive.

Equation 3: (xe, ye) = (xo, yo + r)

Equation 4: r2 = (x− xo)2 + (y − yo)

2

The x and y values are the coordinates for the position of the marker in any of the calibrationimages apart from the first. From this, the length of the arm can be approximated as

Equation 5: r = 12× [ (xi)

2

yi+ yi],

where xi = x− xo and yi = y − yo

2.2.4 Reconstruction

Stereo Reconstructionis used to reconstruct the 3D world points, and the result is used tocalculate elevation. Suppose the 2D camera points of the world point to be reconstructedfrom the left and right cameras respectively are pr and pl. So calculations continue as:

p̃l =pl

f

and

p̃r =pr

f

q = p̃l × (lrRp̃l)

q′=

q

|q|Using these values, the following equation may be solved for a,b and c.

Equation 6:

4

ap̃l + cq′=l

r Rbp̃r +lr T

Finally the 3D world point is computed by:

Equation 7: Pw =lw MP l

where

P l = ap̃l +c

2q′

The motors in Lego MindStorms kit, the equipment used to create the robot arm, operateusing change in angles. The MATLAB code to control movements takes Motor ID, Powerand Angle as inputs. The sign of the power tells the motor in which direction to rotate,while the magnitude dictates the speed. The Angle input controls amount of rotation. Fig-ure (INPUT FIGURE NUMBER) shows a 2-dimensional view of the geometry of the system.

The angle of elevation at any position x,y,z γe in terms of θ, the angle between the initialand current position, is given by

Equation 8: γe = 90− θ

Equation 9: θ = arctan( x−xo√x2+y2

)

The azimuth angle γa, that is the angle at which the arm intersects the z axis at any positionx,y,z, is given as

Equation 10: γa = arccos( zr)

where z is calculated using the equation of a sphere.

Equation 11: z =√

(r2 − (x− xe)2 − (ye − y)2)

5

Figure 1: Diagram of geometry of controller arm. The position (1,1.2) is the elbow. It isassumed that this point never moves. The ray joining point (1,1.2) to (1,0.2) is the assumedorientation of the arm in the initial position. In this position, z is assumed to be 0. Thediagonal ray is any given position, and has an x,y,z value. The angle of elevation γe is definedas the 90 degrees - angle between this ray and the vertical ray.

2.2.5 Model Construction

Figures 2, 3 and 4 depict three different configurations considered for the robot arm. Theobject was to create a configuration that would allow motion in three dimensions as well asa pincher movement. The robot arm was built using the Lego MindStorms robotics kit.

6

Figure 2: First Robot Arm Model. This is the initial idea of the motorized elbow. Byusing two motors, two degrees of rotation are created. The spherical coordinate system isimplemented with the radius, r, representing the length of the arm, represents the azimuth,and represents the elevation. The azimuth determines the rotation around the z-axis andelevation determines how far the arm goes up or down from the origin of the x-y plane.

Figure 3: Second Robot Arm Model. This is a revamped model of the motorized elbow. Thepoints of rotation are now closer together allowing a more realistic approach. However, dueto the requirement of two motors, the points of rotation can never be in the same position.

7

3 Experimental Results

4 Discussion

4.1 Difficulties in Implementation

The development of the robot encountered numerous difficulties. The first difficulty was thesimulation of the rotational degree of the elbow. Two degrees are required of the elbow,which are the azimuth and elevation. In order to achieve these degrees, two motors areutilized. Due to the requirement of two motors to form the elbow, the initial schematic ofthe arm was discarded and recreated. The new schematic allowed for the use of two motorsfor the elbow in exchange for the pitch and roll of the wrist.

Weight had also hindered the development of the robot. The motor utilized to create thepincer was too heavy. As the arm would attempt to follow the movement of a human arm,the weight of the motor decreased the accurate angle before the elevation motor could lockin place and created a large margin of error between the human arm and the robot arm. Inorder to fix this, the angle at which the motor decreases the actual angle by must be includedin the calculations or develop a faster lock function. Also, the arm was redesigned with themotor in question located immediately above the other two motors. This was done to lessenthe torque force this motor was providing on the arm. As development of the robot pro-gressed, more difficulties were encountered. The power output of the azimuth motor provedto be too strong and would not remain stationary. The addition of a clamp or a weight wasrequired to keep the arm stationary and allowed easier observation. The Lego NXT block isused to act as counterweight to keep the motorized elbow in balance.

Another difficulty was color detection. Beyond a certain distance, the system is unable torecognize the distinctions between colors. This limits the usable workspace.

4.2 Sources of Error

Suppose the real world point is (xw, yw, zw) and the reconstructed world point is xreconstw , yreconstw , zreconstw .

First, the Euclidean distance between the reconstructed world point and the real world pointis calculated:

Equation 10: d =√(xw − xreconst

w )2 + (yw − yreconstw )2 + (zw − zreconstw )2

Then the distance between the real world point and the world origin which is the locationof the camera is calculated as follow:

Equation 11: dw−origin =√(xw + yw + zw)

The error is defined as follows:

9

Equation 12: error = ddw−origin

× 100%

This is shown in Figure 6.

Human error is also a major limiting factor in this project. As the calibration is an approx-imation, the potential for errors is high. As a result, the angles of motion of the robot armtend to be much lower than the actual movement, even though the direction of motion iscorrect. This error will be reduced once the two-camera system is again implemented, as thecalibration will be done through CalTech Toolbox.

Errors in movement are shown in graphs 7-10. These errors are the calculated angle plot-ted against the actual angle of movement of the arm. These errors are hardware limitationsand may be difficult to mitigate with the current equipment.

10

Figure 6: Plot showing stereo reconstruction error as a function of the distance between thereal world point and the world origin. The x axis of the error plot is the distance fromthe world origin of the real world point, the y axis of the error plot is the percent error ascalculated above. So, in the error graph, the error is being treated as a function of distanceof the real world point from the origin. The error curves are plotted for 3 different cases inwhich the numbers of pictures (frames) used for camera calibration are 10, 15, 20.

4.3 Future Work and Extensions

Future work on the project will be focused on both increasing the system robustness andreplacing the current color-recognition system. A number of open-source codes availableimplement methods to detect skin tone in an image. It is hoped that this can be extended tothis project, removing the need for colored markers. The controller would be able to operatethe robot using only their hand.

Further work would also involve improving the recognition rate while reducing the error inmovement.

11

Figure 7: In this figure, the actual azimuth movement and computed azimuth movement areplotted against the number of frames. The actual azimuth is obtained by using a protractorto record the angles created by the human hand. The computed azimuth is calculated byobtaining the change of the previous red marker and the current red marker. As shown inthe figure, the computed azimuth is always less than the actual azimuth. This error is dueto using only one camera and not using stereo reconstruction. The use of one camera limitsthe system to using a numerical estimation method to approximate the distance betweenthe x value of the previous image and the new image which induces numerical error into thecalculation.

A Appendix A: Current Trends in Robotics

A.1 Error Optimization in 3D Stereo Reconstruction

The 3D stereo reconstruction method has never been an absolutely accurate method forreconstructing the 3D image of an object from the cameras’ 2D images. In fact, the mainalgorithm of stereo reconstruction leaves plenty of space for errors to infiltrate into the calcu-lations. For that reason, analyzing error in 3D stereo reconstruction has become one of themajor research topics in modern computer vision. In a recent paper called “Realistic StereoError Models and Finite Optimal Stereo Baselines” presented at the 2011 IEEE Workshopon Application of Computer Vision (WACV) by Tao Zhang, from the Vision and TechnologyLab at the University of Colorado, and Terry Boult, from VAST Lab UCCS and SecuritiesInc., the mathematical relationship between the error variances and the stereo system pa-rameters were derived. The authors took in two consideration the cases in which errors existin only one camera as well as in two cameras. Through the paper, the author refuted theprevious work on error analysis in stereo reconstruction, which optimized error in disparityspace and led to the result that, ignoring matching errors, errors decrease when the baselinegoes to infinity, as being erroneous (1).

12

Figure 8: In this figure, the percent error of the azimuth is plotted over the number offrames. Similar to the elevation percent, the azimuth percent error is calculated by takingthe absolute value of the difference between the actual and computed angles and divided bythe absolute value of the actual angle. As depicted in the graph, the percent error becomesvery large when the user attempts to make a very wide movement. The cause of this is dueto the same problem displayed in the azimuth error plot.

Let Pl and Pr be the coordinates of a point P in the left and right camera frame re-spectively; (xl, yl) and (xr, yr) be the projection of P onto the left and right camera plane.

The relation between Pl and Pr is:Pr = Rc(Pl − TC)

where Rc is the rotation matrix given by:

Rc =

cos γ 0 sin γ0 1 0

− sin γ 0 cos γ

Let φ = γ

2. In this paper, it is assumed that Rc = I3×3. TC =[B 0 0]T where B is the

baseline. In this paper, the left camera frame is used as the reference frame. The relationbetween the left camera frame Pl and the world frame Pw is given by:

Pw = Rw(Pl − Tw) (Eqn 1) (1)where Rw is the rotational matrix and Tw is the translation vector. Rw is given by:

Rw =

−1 0 00 − cos θ − sin θ0 − sin θ cos θ

(Eqn 2) (1)

where θ is the rotation angle, it is also the view angle. Rw is obtained by rotation along thex axis followed by sign changing in the x and y directions (1).

13

Figure 9: In this figure, the actual elevation movement and computed elevation movementare plotted as the number of frames increases. The actual angle represents an accurate mea-surement of the angle the user makes by using a protractor. The computed angle representsthe angle that is calculated from stereo reconstruction and the conversion from camera pointsto real world points to obtain the elevation angle, . As shown in the graph, the error betweenthe actual angle and computed angle increases as the number of frames increases. This iscaused by several errors. The first major problem is the way snapshots are taken of images.There is a delay of about half a second between the first camera and the second camera.This causes the cameras to see two different pictures instead of the same picture from twoangles. As the number of frames increases, the error between the two images increase. Thesecond problem is the maximum angle of the computed angle. When the human arm is heldinto a vertical position(90 angle), the computed angle is measured to be around 55-60.

The intersection of the rays OlPl and OrPr is the reconstruction point. However theymay not intersect in space, in that case the reconstructed point is taken to be the mid pointof the line segment perpendicular to both of these lines in space (1).Let the 3D point reconstructed from the 2D coordinates (xl, yl) and (xr, yr) be P2 in theleft camera frame. Let l = [xl,yl,f ]

T , r = TC + βRTc [xr,yr,f ]

T , and w =l × RTc [xr,yr,f ]

T .In this paper, γ = 0, then α, β and λ can be determined by the following equation:

αl + λw = r (Eqn 3) (1)P2 is given by:

P2 = αl + λ2w (Eqn 4) (1)

whereα = xrylyr−xly

2r+(xr−xl)f

2

∆B

λ = B(yr−yl)f∆

14

Figure 10: In this figure, the percentage error of the elevation angle is plotted over thenumber of frames. The percent error is calculated by taking the absolute value of thedifference between the actual angle and computed angle, and divided by the absolute valueof the actual angle. Similar to the elevation error plot, as the number of frames increases,the percent error increases due to the problems stated in the error plot.

∆ =

∣∣∣∣∣∣xl −xr (yl − yr)fyl −yr (xr − xl)ff −f xlyr − xryl

∣∣∣∣∣∣ (Eqn 5) (1)

In noise free cases, P2 is reduced to:

P2 =Bd

xl

ylf

(Eqn 6) (1)

where B is the baseline, d = xl − xr is the disparity.

A.2 Errors in One Camera

Assume the left camera coordinate (xl, yl) for a point is accurate, then the errors in the rightcamera can be calculated by zero mean Gaussian distributions with covariance matrix:

Σ =

[σ2x 00 σ2

y

](Eqn 7) (1)

If we model the mapping in (Eqn 4) as approximately affine in the vicinity of the mean ofthe distribution, then the errors of P2 in the camera frame is also a Gaussian distributionwith the following parameters:

µp2 =Bd[xl,yl,f ]

T (Eqn 8) (1)

15

Σc = JcΣJTc (Eqn 9) (1)

Jc =Bd2

xl

−xlxryly2l +f2

yl−xry2l −0.5(xr−xl)f

2

y2l +f2

f −0.5ylf(xl+xr)

y2l +f2

(Eqn 10) (1)

Jc is the Jacobian matrix computed from the mapping in (Eqn 4)Let: J2 = xry

2l + 0.5(xr − xl)f

2, J3 = 0.5ylf(xl + xr), J4 = xly2l + 0.5(xl − xr)f

2 andb = y2l + f 2.From Σc obtained from (Eqn 9), the following variances in three directions are obtained:

σ2cx =

x2l B

2

d4(σ2

x + x2ry

2l

σ2y

b2) (Eqn 11) (1)

σ2cy =

B2

d4(y2l σ

2x + J2

2σ2y

b2) (Eqn 12) (1)

σ2cz=

B2f2

d4(σ2

x+y2l (xl+xr)2

4

σ2y

b2) (Eqn 13) (1)

Finite optimal baselines can be obtained by minimizing these variances.Let Pl = [X,Y ,Z]T be a point in left camera frame. The relation between the baseline B

and xr is given by:xr = xl − B

Zf (Eqn 14) (1)

substitute (Eqn 14) into (Eqn 13), we get:

solve the equation dσ2cz

dB= 0, we get the optimal baseline to minimize the depth error.

Bcz =2Zf(xl +

b2σ2x

xly2l σ

2y) (Eqn 15) (1)

Bcx = Zf(xl +

b2σ2x

xly2l σ

2y) (Eqn 16) (1)

Bcy =2Z

f(f2+2y2l )(xly

2l +

b2σ2x

xlσ2y) (Eqn 17) (1)

A.3 If Noise - free Triangulation is used

When the noise - free ideal triangulation is used, the mapping used is expressed in (Eqn 6).The Jacobian matrix is the following:

J̃c =Bd2

xl 0yl xl − xr

f 0

(Eqn 18) (1)

The computed variances are:

σ̃2cx =

x2l B

2σ2x

d4=

x2l Z

4σ2x

B2f4 (Eqn 19) (1)

σ̃2cy =

(y2l Z2σ2

x+B2f2σ2y)Z

2

B2f4 (Eqn 20) (1)

σ̃2cz =

B2f2σ2x

d4= Z4σ2

x

B2f2 (Eqn 21) (1)

16

A.4 Error Propagation to World Frame

The error in the mapping of (Eqn 4) when propagated from the camera to the world framecan be computed by the following Gaussian distributions:

µpw = Rw(µp2 − Tw) (Eqn 22) (1)Σw = RwΣcR

Tw (Eqn 23) (1)

From (Eqn 23), the variances in the x, y, and z directions are:

σ2wx =

B2x2l

d4(σ2

x + x2ry

2l

σ2y

b2) (1)

σ2wy =

B2

d4((yl cos θ + f sin θ)2σ2

x + (J2 cos θ + J3 sin θ)2 σ

2y

b2) (1)

σ2wz =

B2

d4((yl sin θ − f cos θ)2σ2

x + (J3 cos θ − J2 sin θ)2 σ

2y

b2) (1)

The overall error σ2w = σ2

wx + σ2wy + σ2

wz is:

The optimal baseline to minimize the depth error:

Bwz =2Z(yl sin θ−f cos θ)(b2σ2

x+x2l y

2l σ

2y)

xlylfσ2y(2y

2l sin θ+f2 sin θ−yl cos θ)

(1)

The optimal baseline to minimize the height error:

Bwy =2Z(yl cos θ+f sin θ)(b2σ2

x+x2l y

2l σ

2y)

xlylfσ2y(2y

2l cos θ+f2 cos θ+ylf sin θ)

(1)

The optimal baseline to minimize the width error:

Bwx = Bcx (1)

The optimal baseline to minimize the overall error:

Bwa =Zf( b2σ2

x

xly2l σ

2y+ xl) (Eqn 24) (1)

A.5 When the intrinsic parameters of the camera is known

If all the intrinsic parameters of the cameras are known, the error propagation between theimage frame and the camera frame is given below:

µim =

[s−1x 00 s−1

y

](µ− [oxoy]

T ) (Eqn 25)

Σim =

[σ2x

s2x0

0σ2y

s2y

](Eqn 26)

Where ox, oy, sx, sy are the intrinsic parameters of the cameras (1).

17

A.6 Error in Two Cameras

When the errors are modeled in both cameras, the combined covariance matrix is:

Σ2 =

σ2x 0 0 00 σ2

y 0 00 0 σ2

x 00 0 0 σ2

y

(Eqn 27) (1)

The corresponding Jacobian matrix is:

Jc2 =Bd2

xl−xlylxr

b−xr

xlylxr

b

yl−J2b

−ylJ4b

f −J3b

−f J3b

(Eqn 28) (1)

The covariance matrix propagated into the world frame can be computed as Σw2 = RwJc2Σ2JTc2R

Tw.

From Σw2, the variances of errors in 3 directions are:

σ2wx2 =

B2

d4((x2

l + x2r)σ

2x +

2x2l y

2l x

2r

b2σ2y) (Eqn 29) (1)

σ2wy2 =

B2

d4(2(f sin θ+ yl cos θ)

2σ2x +

σ2y

b2((J2 cos θ+ J3 sin θ)

2+(J4 cos θ+ J3 sin θ)2))

(Eqn 30) (1)

σ2wz2 =

B2

d4(2(yl sin θ− f cos θ)2σ2

x+σ2y

b2((J3 cos θ− J2 sin θ)

2+(J4 sin θ− J3 cos θ)2))

(Eqn 31) (1)

The variance of the overall error σw2 is: σ2w2 = σ2

wx2 + σ2wy2 + σ2

wz2 (1)

The optimal baselines for x, y, z directions:

Bwx2 =2xlZ(b2σ2

x+x2l y

2l σ

2y)

f(b2σ2x+2xly

2l σ

2y)

(Eqn 32) (1)

Bwy2 = Bwz2 =2Zf(xl +

b2σ2x

xly2l σ

2y) (Eqn 33) (1)

Bw2 =2Z(x2

l +y2l +f2)(b2σ2x+x2

l y2l σ

2y)

xlf(b2σ2x+y2l (2x

2l +y2l +f2)σ2

y)(Eqn 34) (1)

B Morphogenic Robotics

Morphogenic robotics is an interdisciplinary field of robotics that makes use of simulatedor physical robots to study natural intelligence and to design better robotic systems basedupon the principles of natural biological evolution. This field of robotics aims to importinto the robotic system the properties of living organic creatures such as language, emotion,anticipation, social skills and other cognitive behaviors in living organisms (2).

Morphogenesis of animals can be divided into early embryonic development and laterembryonic development. Early embryonic development typically involves cleavage, gastru-lation, and axis formation, while later embryonic development is mainly responsible for thedevelopment of the nervous system, starting with the segregation of neural and glial cells

18

from the ectoderm germ layer (2).The regulatory dynamics in a unicellular cell can be described by a set of ordinary differen-

tial equations. For example, the mathematical model of gene expression with autoregulationcan be described by:

d[R]dt

= −γR[R] + αRH([P ]) (Eqn 1) (2)d[P ]dt

= −γP [P ] + αP [R] (Eqn 2) (2)where [R] and [P ] are the concentration of mRNA and protein, respectively, γR and γP arethe decay rate of the mRNA and protein, αR and αP are the synthesis rate of the mRNAand protein, and H(X) is the Hill function. If the autoregulation is a repression, also knownas negative auto regulation, the Hill function can be described by

Hr(x) =β

θn+xn (Eqn 3) (2)and if the autoregualtion is activation, the Hill function is

Ha(x) =βxn

θn+xn (Eqn 4) (2)where β is the activation coefficient, θ is the threshold, and n is the Hill coefficient.

For describing the morphogenesis of multicellular organisms, the interaction between thecells and its influence on gene expression dynamics must be taken into account. Mjolsnesshas suggested a generalized model:

dgijdt

= −γjgij + ϕ[

ng∑l=1

W jlgil + θj] +Dj∇2gij (Eqn 5) (2)

where gij denotes the concentration of the jth gene product (protein) in the ith cell. Thefirst term on the right hand side of (Eqn 5) represents the degradation of the protein at a rateof γj, the second term specifies the protein gij, and the last term describes protein diffusionat a rate of Dj. ϕ is an activation function for the protein production, which is usuallydefined as a sigmoid function ϕ(z) = 1

1+e−µz . The interaction between the genes is described

with an interaction matrix W ij, the element of which can be either active (a positive value)or repressive (a negative value). θj is a threshold for activation of gene expression. ng is thenumber of proteins(2).

One application of Morphogenesis robotics is Swarm robotics.A swarm robotic system isa multi robot system consisting of a large number of homogeneous simple robots. Swarmrobots are often used to fulfill tasks that are difficult or even impossible for a single robotwhen uncertainties or incomplete information is present or when a distributed control orasynchronous computation is needed. Swarm robotic systems with a distributed controlare flexible, robust, and adaptive for tasks that are inherently distributed in space andtime. Typical applications of swarm robotic systems include group transport, foraging, shapeformation, boundary coverage, urban search and rescue, and unknown terrain exploration(2).

Another application of Morphogenesis robotics is Cell - Robot mapping. The main ideaof Cell - Robot mapping is to implement cellular mechanisms in biological morphogenesis toself - organized control of swarm robots. The principle behind this is to establish a metaphorbetween a cell and a robot. It is assumed that the motional dynamics of each robot canbe modeled by the regulatory dynamics of a biological cell. The location and velocity ofthe robots are described by the protein concentration of a few genes whose expression isinfluenced by each other. Typically, for a robot in a 3D space, three proteins are used fordenoting the robot’s position and three for the velocity (2).

Keeping the metaphor between the cells and the robots in mind, the movement dynamics

19

of each robot can be described by a model in which the concentration of two proteins of typeG represents the x and y position of a robotic system, respectively. This entails that eachrobot is able to detect the distance to its neighboring robots (2).

20

C Appendix B: MATLAB Code

The following code is heavily based on ’Tracking red color objects using matlab’ [1]

1 clearvars2 close all3

4

5 vid1=videoinput('winvideo',2);6 %vid1=videoinput('macvideo',1);7 set(vid1, 'FramesPerTrigger', Inf);8 set(vid1, 'ReturnedColorspace', 'rgb')9 vid1.FrameGrabInterval = 20;

10

11 vid2=videoinput('winvideo',3);12 %vid1=videoinput('macvideo',1);13 set(vid2, 'FramesPerTrigger', Inf);14 set(vid2, 'ReturnedColorspace', 'rgb')15 vid2.FrameGrabInterval = 20;16

17 preview(vid1)18 preview(vid2)19

20 previousOpenness=1;21 totalElevation = 0;22 framecounter=0;23 oldxblue=0;24 oldyblue=0;25 oldzblue=0;26 oldxred=0;27 oldyred=0;28 oldzred=0;29 currentaztotal=0;30 azimuthtest=[];31

32

33 'Starting in 3 secs'34 pause(3);35

36 % %Sets the default position of the arm37 % Rotate('A',−20,34,'B',0,0);38

39 %Start tracking40 start(vid1);41 start(vid2);42 datavid1 = getsnapshot(vid1);43 datavid2 = getsnapshot(vid2);44 [xprevr,yprevr,xprevy,yprevy]=pointlocation(datavid1);45 [xprevr2,yprevr2,xprevy2,yprevy2]=pointlocation(datavid2);46 closepreview(vid1);47 closepreview(vid2);48

49 pause(1);

21

50 while framecounter<10051 framecounter=framecounter+1;52 datacurrent=getsnapshot(vid1);53 data2=getsnapshot(vid2);54 magnitude=size(datacurrent);55 xmax=magnitude(1,1);56 ymax=magnitude(1,2);57 [xcurrr,ycurrr,xcurrb,ycurrb,xcurry,ycurry]=dualpointlocation(datacurrent);58 [xcurrr2,ycurrr2,xcurrb2,ycurrb2,xcurry2,ycurry2]=dualpointlocation(data2);59

60

61

62 if xcurrr<999863 %AZIMUTH64 sidechanger=−(xcurrr−xprevr);65 sidechangersign=−sign(sidechanger);66 siderotate=roundn(((abs(sidechanger))/(0.4*xmax))*50,1);67

68 xprevr=xcurrr;69 yprevr=ycurrr;70

71 azimuthsign=sidechangersign;72 limitazimuth=abs(siderotate);73 currentaztotal=currentaztotal+(limitazimuth*azimuthsign);74 if currentaztotal<5575 if limitazimuth>576 if currentaztotal>−5577 if azimuthsign<078 Rotatesingle( 'B', 10*azimuthsign, ...

floor(limitazimuth))79 elseif azimuthsign>080 Rotatesingle( 'B', 10*azimuthsign, ...

floor(limitazimuth))81 end82 azimuthtest=[azimuthtest;vid1.FramesAcquired];83 else84 'Going beyond minimum azimuth'85 currentaztotal=currentaztotal−(limitazimuth*azimuthsign);86 end87 end88 else89 'Going beyond maximum azimuth'90 currentaztotal=currentaztotal−(limitazimuth*azimuthsign);91 end92

93 %ELEVATION94 if (xcurrr2<9998 && xcurrb<9998 && xcurrb2<9998)95 [xred3D, yred3D, zred3D] = StereoReconst(xcurrr, ycurrr, ...

xcurrr2, ycurrr2);96 [xblue3D, yblue3D, zblue3D] = StereoReconst(xcurrb, ycurrb, ...

xcurrb2, ycurrb2);97

98 ∆x = xblue3D − oldxblue;99 ∆y = yblue3D − oldyblue;

22

100 ∆z = zblue3D − oldzblue;101 oldxred3Dt = oldxred + ∆x;102 oldyred3Dt = oldyred + ∆y;103 oldzred3Dt = oldzred + ∆z;104

105 %Set elbow as origin by subtracting elbow point from wrist ...point

106 oldx3D = (oldxred3Dt − xblue3D);107 oldy3D = oldyred3Dt − yblue3D;108 oldz3D = oldzred3Dt − zblue3D;109 oldArmLength = sqrt((oldxred3Dt−xblue3D)ˆ2 + ...

(oldyred3Dt−yblue3D)ˆ2 + (oldzred3Dt−zblue3D)ˆ2);110

111 x3D = (xred3D − xblue3D);112 y3D = yred3D − yblue3D;113 z3D = (zred3D − zblue3D);114 armLength = sqrt((xred3D−xblue3D)ˆ2 + (yred3D−yblue3D)ˆ2 + ...

(zred3D−zblue3D)ˆ2);115

116 %Computes the angles and ∆ angles for Elevation and Azimuth ...using

117 %Spherical coordinates. Elevation angle is obtained from ...arccos(z/R)

118 %and Azimuth angle is obtained from arctan(y/x). Delta ...angles are

119 %obtained by subtracting the previous angles to the new angles.120 previousAngleElevation = asin(oldx3D/oldArmLength) * (180/pi);121 newAngleElevation = asin(x3D/armLength) * (180/pi)122 ∆Elevation = newAngleElevation − previousAngleElevation123

124 %Save previous elbow and wrist points125 oldxred = xred3D;126 oldyred = yred3D;127 oldzred = zred3D;128 oldxblue = xblue3D;129 oldyblue = yblue3D;130 oldzblue = zblue3D;131

132

133 if (ge(abs(∆Elevation),5))134 if (gt(newAngleElevation,−10) && lt(newAngleElevation,40))135 signElevation = sign(∆Elevation)136 elevation = abs(∆Elevation)137 elseif (gt(newAngleElevation,40))138 sprintf('Going over 40 degrees')139 ∆Elevation = 40−totalElevation;140 signElevation = sign(∆Elevation);141 elevation = abs(∆Elevation);142 elseif (lt(newAngleElevation,−10))143 sprintf('Going below −10 degrees')144 ∆Elevation = −10−totalElevation;145 signElevation = sign(∆Elevation);146 elevation = abs(∆Elevation);147 else

23

148 signElevation=1;149 elevation = 0;150 end151 totalElevation = ∆Elevation+totalElevation;152 Rotatesingle('A',−signElevation*10,round(elevation))153 end154 end155 %PINCER156 if xcurry<9998157 xprevy=xcurry;158 yprevy=ycurry;159 openness=sqrt((xcurry−xcurrr)ˆ2+(ycurry−ycurrr)ˆ2);160 if openness>100161 openOrClosed=1;162 else163 openOrClosed=0;164 end165

166 if previousOpenness<1 && openOrClosed>0167 pinchersign=1;168 pinchermag=50 ;169

170 elseif previousOpenness>0 && openOrClosed<1171 pinchersign=−1;172 pinchermag=50;173

174 else175 pinchersign=0;176 pinchermag=0;177 end178

179 previousOpenness=openOrClosed;180 Rotatesingle('C', 10*pinchersign, pinchermag);181

182 end183 end184 pause(1.5)185 end186

187 stop(vid1);188 stop(vid2);189

190 % Flush all the image data stored in the memory buffer.191 flushdata(vid1);192 flushdata(vid2);

1 function [xred, yred, xyellow,yyellow]=pointlocation(data1)2 data1y=imcomplement(data1);3 diff imr1 = imsubtract(data1(:,:,1), rgb2gray(data1));4 diff imb1 = imsubtract(data1(:,:,3), rgb2gray(data1y));5

6 %Use a median filter to filter out noise

24

7 diff imr1 = medfilt2(diff imr1, [3 3]);8 diff imb1 = medfilt2(diff imb1, [3 3]);9

10

11 % Convert the resulting grayscale image into a binary image.12 diff imr1 = im2bw(diff imr1,0.15);13 diff imb1 = im2bw(diff imb1,0.15);14

15

16 % Remove all those pixels less than 300px17 diff imr1 = bwareaopen(diff imr1,300);18 diff imb1 = bwareaopen(diff imb1,300);19

20

21 % Label all the connected components in the image.22 bwr1 = bwlabel(diff imr1, 8);23 bwb1 = bwlabel(diff imb1, 8);24

25

26 % Here we do the image blob analysis.27 % We get a set of properties for each labeled region.28 statsr1 = regionprops(bwr1, 'BoundingBox', 'Centroid');29 statsb1 = regionprops(bwb1, 'BoundingBox', 'Centroid');30 lenr1=length(statsr1);31 lenb1=length(statsb1);32

33

34 %If cameras see the marks.35

36 if((lenb1>0)&& (lenr1>0))37 %Make sure we return the correct result38 for countb1=1:lenb139 checkobjsize1=statsb1(countb1,1).BoundingBox;40 if checkobjsize1(3)<60 && checkobjsize1(4)<6041 statsb1(1,1) = statsb1(countb1,1);42 end43 end44 for countr1=1:lenr145 checkobjsizer1=statsr1(countr1,1).BoundingBox;46 if checkobjsizer1(3)<60 && checkobjsizer1(4)<6047 statsr1(1,1) = statsr1(countr1,1);48 end49 end50

51

52 %Gets the x and y coordinates for each marker(4 x and y53 %coordinates total).54 rcalib1 = [statsr1(1).Centroid(1,1), statsr1(1).Centroid(1,2)];55 xred=rcalib1(1,1);56 yred=rcalib1(1,2);57 bcalib1 = [statsb1(1).Centroid(1,1), statsb1(1).Centroid(1,2)];58 xyellow=bcalib1(1,1);59 yyellow=bcalib1(1,2);60

25

61

62

63

64 else %If camera(s) doesn't see a marker(s)65 if lenr1<166 sprintf('Error: Cannot see the red marker.')67 xred=9999;68 yred=9999;69 xyellow=9999;70 yyellow=9999;71 end72 if lenb1<173 sprintf('Error: Cannot see the yellow marker.')74 xred=9999;75 yred=9999;76 xyellow=9999;77 yyellow=9999;78 end79

80 end81

82 imshow(data1);83

84 hold on85

86 %This is a loop to bound the red objects in a rectangular box.87 for objectr1 = 1:length(statsr1)88 bbr1 = statsr1(objectr1).BoundingBox;89 bcr1 = statsr1(objectr1).Centroid;90 rectangle('Position',bbr1,'EdgeColor','r','LineWidth',2)91 plot(bcr1(1),bcr1(2), '−m+')92 ar1=text(bcr1(1)+15,bcr1(2), strcat('X: ', ...

num2str(round(bcr1(1))), ' Y: ', num2str(round(bcr1(2)))));93 set(ar1, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', ...

12, 'Color', 'yellow');94 end95

96 for objectb1 = 1:length(statsb1)97 bbb1 = statsb1(objectb1).BoundingBox;98 bcb1 = statsb1(objectb1).Centroid;99 rectangle('Position',bbb1,'EdgeColor','b','LineWidth',2)

100 plot(bcb1(1),bcb1(2), '−m+')101 ab1=text(bcb1(1)+15,bcb1(2), strcat('X: ', ...

num2str(round(bcb1(1))), ' Y: ', num2str(round(bcb1(2)))));102 set(ab1, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', ...


105 hold off106 end

26

1 function [xred, yred, xblue, yblue, xyellow, ...yyellow]=dualpointlocation(data1)

2 data1y=imcomplement(data1);3

4 % Now to track red objects in real time5 % we have to subtract the red component6 % from the grayscale image to extract the red components in the image.7 diff imr1 = imsubtract(data1(:,:,1), rgb2gray(data1));8 diff imb1 = imsubtract(data1(:,:,3), rgb2gray(data1));9 diff imy1 = imsubtract(data1y(:,:,3), rgb2gray(data1y));

10

11 %Use a median filter to filter out noise12 diff imr1 = medfilt2(diff imr1, [3 3]);13 diff imb1 = medfilt2(diff imb1, [3 3]);14 diff imy1=medfilt2(diff imy1,[3 3]);15

16 % Convert the resulting grayscale image into a binary image.17 diff imr1 = im2bw(diff imr1,0.15);18 diff imb1 = im2bw(diff imb1,0.15);19 diff imy1 = im2bw(diff imy1,0.25);20

21 % Remove all those pixels less than 300px22 diff imr1 = bwareaopen(diff imr1,300);23 diff imb1 = bwareaopen(diff imb1,300);24 diff imy1 = bwareaopen(diff imy1,50);25

26 % Label all the connected components in the image.27 bwr1 = bwlabel(diff imr1, 8);28 bwb1 = bwlabel(diff imb1, 8);29 bwy1 = bwlabel(diff imy1, 8);30

31 % Here we do the image blob analysis.32 % We get a set of properties for each labeled region.33 statsr1 = regionprops(bwr1, 'BoundingBox', 'Centroid');34 statsb1 = regionprops(bwb1, 'BoundingBox', 'Centroid');35 statsy1 = regionprops(bwy1,'BoundingBox','Centroid');36 lenr1=length(statsr1);37 lenb1=length(statsb1);38 leny1=length(statsy1);39

40

41 %If cameras see the marks.42

43 if(lenr1>0) %&& ((lenb1>0) | | leny1>0))44 for countr1=1:lenr145 checkobjsizer1=statsr1(countr1,1).BoundingBox;46 if checkobjsizer1(3)<60 && checkobjsizer1(4)<6047 statsr1(1,1) = statsr1(countr1,1);48 end49

50 rcalib1 = [statsr1(1).Centroid(1,1), statsr1(1).Centroid(1,2)];51 xred=rcalib1(1,1);52 yred=rcalib1(1,2);53 end

27

54

55 else56 sprintf('Error: Cannot see the red marker.')57 xred=9999;58 yred=9999;59 end60

61 %Make sure we return the correct result62 if lenb1>063 for countb1=1:lenb164 checkobjsize1=statsb1(countb1,1).BoundingBox;65 if checkobjsize1(3)<60 && checkobjsize1(4)<6066 statsb1(1,1) = statsb1(countb1,1);67 end68 end69

70 bcalib1 = [statsb1(1).Centroid(1,1), statsb1(1).Centroid(1,2)];71 xblue=bcalib1(1,1);72 yblue=bcalib1(1,2);73 else74 sprintf('Error: Cannot see the blue marker.')75 xblue=9999;76 yblue=9999;77 end78

79 if leny1>080 for county1=1:leny181 checkobjsize1=statsy1(county1,1).BoundingBox;82 if checkobjsize1(3)<60 && checkobjsize1(4)<6083 statsy1(1,1) = statsy1(county1,1);84 end85 end86

87 ycalib1 = [statsy1(1).Centroid(1,1), statsy1(1).Centroid(1,2)];88 xyellow=ycalib1(1,1);89 yyellow=ycalib1(1,2);90 else91

92 sprintf('Error: Cannot see the yellow marker.')93 xyellow=9999;94 yyellow=9999;95

96 end97 %If camera(s) doesn't see a marker(s)98

99

100 imshow(data1);101

102 hold on103

104 %This is a loop to bound the red objects in a rectangular box.105 for objectr1 = 1:length(statsr1)106 bbr1 = statsr1(objectr1).BoundingBox;107 bcr1 = statsr1(objectr1).Centroid;

28

108 rectangle('Position',bbr1,'EdgeColor','r','LineWidth',2)109 plot(bcr1(1),bcr1(2), '−m+')110 ar1=text(bcr1(1)+15,bcr1(2), strcat('X: ', ...

num2str(round(bcr1(1))), ' Y: ', num2str(round(bcr1(2)))));111 set(ar1, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', ...


114 for objectb1 = 1:length(statsb1)115 bbb1 = statsb1(objectb1).BoundingBox;116 bcb1 = statsb1(objectb1).Centroid;117 rectangle('Position',bbb1,'EdgeColor','b','LineWidth',2)118 plot(bcb1(1),bcb1(2), '−m+')119 ab1=text(bcb1(1)+15,bcb1(2), strcat('X: ', ...

num2str(round(bcb1(1))), ' Y: ', num2str(round(bcb1(2)))));120 set(ab1, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', ...


123 for objecty1 = 1:length(statsy1)124 bby1 = statsy1(objecty1).BoundingBox;125 bcy1 = statsy1(objecty1).Centroid;126 rectangle('Position',bby1,'EdgeColor','y','LineWidth',2)127 plot(bcy1(1),bcy1(2), '−m+')128 ay1=text(bcy1(1)+15,bcy1(2), strcat('X: ', ...

num2str(round(bcy1(1))), ' Y: ', num2str(round(bcy1(2)))));129 set(ay1, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', ...


132 hold off133 end

1 function [ output args ] = Rotatesingle( joint1, power1, limit1)2 %Joint A = Theta3 % −Power = Arm goes Up4 % +Power = Arm goes Down5 %6 %Joint B = Phi7 % +Power = CCW rotation8 % −Power = CW rotation9 %

10 %Joint C = Pincer11 % +Power = Open pincer12 % −Power = Close pincer13

14 motor1 = NXTMotor( joint1, 'Power', power1, 'TachoLimit', limit1, ...'ActionAtTachoLimit', 'Holdbrake');

15 motor1.SendToNXT();16 %motor1.WaitFor();17

18

29

19 end

30

D Appendix C: Personal Contributions

D.1 Brielle Graham

My first major contribution was point location and tracking. I was able to take an imageof a scene from a live feed, locate the red, blue and yellow points and determine the pixelcoordinates of the points. I also adusted the design of the arm, moving the motor to reducetorque. I programmed the current system used to determine and rotate to azimuth. I alsoprogrammed the single-camera tracking and movement systems used in the interim demo. Ifound the new limits of rotation and was also able to adjust the code for robot rotation toallow smoother movements. I helped with code debugging and fine-tuning.

D.2 Bao Huynh

In this project, I was originally responsible Stereo Reconstruction. I also introduced the ideaof a gesture imitating arm - hand robotic system using stereo reconstruction to reconstructthe 3D locations of the joints and elbow of the human hand from the 2D camera coordinates,then use spherical coordinates system to calculate the rotational angle of the arm.As the project processed towards the end, I contributed mathematical ideas and helped withcode debugging as well as in designing the poster board. In code debugging, I helped myteam figuring out that we were missing a linear frame transformation in our calculation. Ialso helped in doing research in current trends in robotics at the end.

D.3 Randall Reyes

� Initial Robot Designs

� Poster Design

� Code integration

� Helped with Stereo Reconstruction

31

References

[1] A. Bhargav Anand, Tracking red color objects using matlab. Amrita School of Engi-neering, Bangalore, India, 17 Nov 2010. http://www.mathworks.com/matlabcentral/fileexchange/authors/92623

[2] Terry Boult, Tao Zhang, Realistic Stereo Error Models and Finite Optimal Stereo Base-lines. 2011 IEEE Workshop on Application of Computer Vision (WACV), pp 426 - 433.Kona, HI January 2011

[3] Yaochu Jin, Yan Meng Morphogenetic Robotics: An Emerging New Field in Developmen-tal Robotics, IEEE Transactions on Systems, Man, and Cybernetics, Part C: Applicationsand Reviews. Vol. 41, no. 2, pp 145 - 160. March 2011

32

mimbot: gesture-imitating robotic system final report

Documents