Modeling and Simulation of Astrojax

Daniel Dichter & Kate Maschan

December 16, 2013

Abstract

In this project we expand upon the classic double pendulum problemby modeling the popular toy Astrojax. We create a MATLABmodel of thethree-dimensional double pendulum whose upper mass can be driven bythe user of the toy. The strings connecting the three masses are modeledas tensile-only springs and we include viscous damping to make our modelmore realistic. We run a set of experiments in a motion capture studio tovalidate our simulation. After comparing the experimental data to resultsfrom our simulation, we found that we created a relatively accurate modelfor the range of motions we expect a user might create when playing withthe Astrojax toy.

1 Background

The double pendulum is a classic dynamics problem. We created simulationsof various single and double pendulums this semester using different coordinatesystems and parameters. This project was inspired by the physics toy Astrojax.The toy has three equally sized masses, two of which are fixed to the ends of astring. The middle mass is free to slide along the string. People do tricks withthe toy by moving the upper mass such that the lower masses swing in patternsor jump.

1

Figure 1: Astrojax toy with three masses on a string.

For our simulation, the middle mass was fixed, turning the toy into a doublependulum. The strings acted as tensile-only springs such that the masses couldbe made to jump. This double pendulum was also interesting because the uppermass could be driven.The model was created in MATLAB using the built-in ode45 numerical solver.It was validated using experimental data taken in Olin’s motion capture studio.

2 Learning Objectives

We created this project because we were interested in building upon the pen-dulum simulations we had created in class earlier this semester. We wanted toderive our own equations of motion for a more complex pendulum system. Thismodel involved many parameters we have not encountered before so it seemedto be a good challenge for us. We also wanted to explore creating our modelin Cartesian coordinates, as we have used other coordinate systems in the past.Most importantly, we were excited to use Olin’s motion capture studio. Wewanted to create a model that could be used to predict the motion of our As-trojax toy when a user does tricks with it. Therefore, it was crucial for us tocompare our model to experimental data taken in the motion capture studio.We had not yet validated any of our models this semester so we wanted to createan opportunity for ourselves to do that.

2

3 Model

The Astrojax toy was abstracted as three point masses connected by tensile-only springs and experiencing viscous damping. Thus, the forces acting on themasses are gravity, spring forces, and damping forces. The masses essentiallyhave two possible modes: swinging like a pendulum and projectile motion. Amass enters projectile motion when the upper mass is jerked quickly, causinga large spring force in the string. Projectile motion is simplest in a Cartesiancoordinate system and we wanted to use just one coordinate system for themodel. Because of this, we chose to use a Cartesian coordinate system for theentire model as opposed to other possible coordinate systems such as sphericalcoordinates or rotating reference frames.

Figure 2: Model with masses in pendulum motion.

3

Figure 3: Model with masses in projectile motion. d1, d2 represent distancebetween masses.

The position of the upper mass m1 is known for all time as that informationis the input to the function. Because of this, the forces acting on it are not ofinterest. Therefore, it is only necessary to solve for the forces acting on m2 andm3. The magnitude and direction of the gravitation forces is constant for alltime. It is equal to the mass x gravitational acceleration and is directed alongthe −z axis.

There are two spring forces: one in the string between m1 and m2 and onein the string between m2 and m3. Because the strings are tensile-only springs,they exert forces on the masses but only in tension. In compression, they donot exert any forces on the masses. Spring force is equal to the product ofthe spring constant (found experimentally) and the length that the spring isstretched beyond its free length. It is directed along the line connecting the twomasses. For each iteration of ode45, the distances d1 and d2 from m1 to m2

and m2 to m3, respectively, are calculated. If the distance between two massesis greater than that string’s unstretched length, the spring force in that stringbecomes nonzero. The direction of the force is found by comparing the currentpositions of each mass.

4

Figure 4: Spring forces along strings, pointing from lower mass to upper mass.

Drag is a force that opposes motion. Viscous damping was chosen for thismodel instead of aerodynamic drag due to the relatively low speed of the motion.The force of drag is proportional to the velocity of the mass by a constant thatis determined experimentally. In the simulation, it is initially assumed that theforce from drag is zero. This prevents numerical errors that occur when themasses start with no initial velocity. For each iteration of ode45, if the velocityin non-zero, the magnitude of the current drag force is calculated. Its directionis always opposite that of the velocity vector.

5

Figure 5: Drag forces opposing motion. Velocity represented by dashed line.

Given the forces described above, free body diagrams for m2 and m3 canbe created. Note that the spring force equals zero when the distance betweenmasses is less than or equal to the string’s unstretched length. The drag forceequals zero when the mass has no velocity, such as at the beginning of a trial.

Figure 6: Free body diagram of m2 showing forces from gravity, drag, and bothstrings.

6

Figure 7: Free body diagram of m3 showing forces from gravity, drag, and thesecond string.

Equations of motion can then be derived from the forces acting on each massin the x, y, and z directions.

m̈2x = (Fs1x − Fs2x + Fd2x)/m2 (1)

m̈2y = (Fs1y − Fs2y + Fd2y)/m2 (2)

m̈2z = (Fs1z − Fs2z + Fd2z)/m2 − g (3)

m̈3x = (Fs2x + Fd3x)/m3 (4)

m̈3y = (Fs2y + Fd3y)/m3 (5)

m̈3z = (Fs2z + Fd3z)/m3 − g (6)

The simulation requires initial conditions of the positions and velocities ofm2 and m3, as well as a position vector for m1. At each time step, the velocityand acceleration of m2 and m3 are calculated and packed into a vector. Thenumerical solver in MATLAB, ode45, takes that vector as an input and returnsthe position and velocity of m2 and m3 at each time step. This information canbe used to plot the trajectory of each mass to observe its behavior and compareit to our experimental data.

4 Experimental Validation

Before the model could be completed, various system paramenters needed tobe measured. We first measured the masses of the balls and the lengths of thestrings. We also needed to determine the spring constant k of the strings. Itwas found by stretching the string in an Instron machine and measuring the

7

force exerted on it and its extension. The spring constant was determined to be280N/m.

Figure 8: Experimental setup to determine k.

8

Figure 9: Close-up of string held in Instron.

The only remaining parameter to find was the damping coefficient c. Beforethis could be determined, we had to collect data from the motion capture studio.

Olin’s motion capture studio includes eight high-speed cameras that capturemotion at 200 frames per second. Three reflective markers were attached to eachmass. Using the motion capture software we were able to locate each markerand identify rigid bodies. This allowed the software to track each mass in three-dimensional space. Five tests were captured that represented the various modesof our simulation. The data was then extracted and the software returned thex, y, and z positions of the center of mass of each ball over time.

The first test was used to find the damping coefficient c, the last unknownparameter. It was found by comparing experimental data in x, y, and z tothe results of our simulation for a simple case: holding m1 and m2 steady andallowing m3 to swing like a single pendulum. The value of c was tweaked slightlyuntil there was a good fit between the experimental data and the simulation.The final value of c was determined to be 7.5x10−4.

9

Four other tests were run for validation. Because we did not know how wellthe data would match our simulation, we wanted to run trials with varyingamounts of chaos. They increased in complexity from a three-dimensional pen-dulum with m1 stationary to m1 being driven randomly. The results of all fivecases are presented in the following section.

10

Figure 10: Setup used to keep m1 stationary.

11

Figure 11: Setup used to drive m1 without obscuring markers.

5 Results

For most of the experiments, a very close fit between the data and our simu-lation was acheived. Here, results are presented that compare the two sets ofinformation. Five cases are presented to validate our simulation. In the firsttwo, m1 was held stationary as in Figure 10.

The first trial was the single pendulum case. This allowed us to find our cvalue and also validated that our simple pendulum motion was accurate.

Figure 12: Trajectory of m2 for a single pendulum.

12

Figure 13: Position of m2 for a single pendulum.

The second trial was a double pendulum with m1 held stationary. Thisvalidated our double pendulum in three dimensions with non-rigid rods.

Figure 14: Trajectory of m2 and m3 for double pendulum with m1 stationary.

13

Figure 15: Position of m2 and m3 for double pendulum with m1 stationary.

In the last three trials, m1 was driven as in Figure 11.In the first trial of this set, m1 was driven by occasionally jerking the string

to make the lower masses jump. They were small jumps and therefore werenot overly chaotic. Our simulation was, for the most part, able to predict thecomplicated trajectory we created in the motion capture studio for this trial.

Figure 16: Trajectory of m2 and m3 for double pendulum with m1 driven bysmall jumps.

14

Figure 17: Position of m2 and m3 for double pendulum with m1 driven by smalljumps.

In the second trial, m1 was driven in a circular manner. All three massesstarted at rest, were swung in a circular motion, and then were allowed to slowto a rest again. The motion was smooth and the experimental data matchesour simulation well.

Figure 18: Trajectory of m2 and m3 for double pendulum with m1 driven in acircular motion.

15

Figure 19: Position of m2 and m3 for double pendulum with m1 driven in acircular motion.

In the final trial, m1 was driven randomly with fast, sharp jerks in variousdirections. It was a chaotic trial, meant to test the limits of the accuracyof our simulation. As can be seen in Figures 20 and 21, our simulation didnot predict the motion of the trial well. Its general behavior in the first fewseconds agrees with the experiment, but once the mass began moving and jerkingaround quickly, the simulation was unable to predict its motion. See the ModelLimitations and Improvement section for an explanation of why this motioncaused inaccuracy in our simulation.

Figure 20: Trajectory of m2 and m3 for double pendulum with m1 driven ran-domly.

16

Figure 21: Position of m2 and m3 for double pendulum with m1 driven ran-domly.

6 Model Limitations and Improvement

For most cases, our simulation is a good model of our dynamic system. Itsgreatest limitation is that it does not handle chaotic motion well. We believethe reason for this is that our strings are modeled as ideal springs. In reality,they do not behave exactly as ideal springs. The experience some energy loss asthey pull the masses closer to one another. We expect that the force-extensionrelationship is not perfectly linear as we have modeled it. It should instead belinear when the strings are being stretched and cubic when they are returningto their free length. This is a common response for springy materials. However,this would have been a significant challenge to implement and we decided thatit was outside the scope of this project. In a future iteration of our model wewould model the strings as non-ideal springs.

7 Diagnosis

We encountered two main issues with our project.First, it was difficult to extract useful data from the motion capture studio.

Because our masses are so small, it was difficult to arrange the markers inunique patterns while ensuring that they were far enough apart for the camerasto continuously track them. This caused significant noise in our data as the

17

software lost track of markers or confused them with one another. We wereeventually able to get the motion capture software to average the markers forus and return the center of mass of each rigid body. In doing so it also filteredthe data. The result still had some random spikes so we created a MATLABscript that would remove those outliers for us. The final result wound up beingmostly smooth data that was simple to use.

Second, as was stated in the Model Limitations and Improvement section,our model does not predict chaotic motion well. After examining the data for along time we were able to determine the cause of the problem. Our spring forceswere too strong when the strings were returning to their unstretched length. Wediscovered that this was the problem when we noticed that m2 and m3 in oursimulation were bouncing more quickly than in the experiment and we watchedthe animations slowly to be sure that that was the issue. Again, we believe thatthere is a solution but implementing it would have been outside of the scope ofthis project.

8 Reflection

We both found this project to be a good learning experience. We created ourown equations of motion which was one of our initial goals. We feel that doing sowrapped up our semester nicely and ensured that we were comfortable modelinga dynamic system. We also greatly enjoyed taking data in the motion capturestudio and using it to validate our simulation. It was satisfying to see thatwe had created a useful model for the conditions that we were interested inexploring.

9 Conclusion

We successfully created a model for a double pendulum in three dimensions. Thestrings connecting the three masses are modeled as tensile-only springs. We alsoincorporated viscous damping into the model. For most cases we tested, oursimulation was able to predict the motion of the Astrojax toy. It failed whenwe tested it with extremely chaotic motion. However, that extreme motion isunlikely to occur when someone is actually playing with the toy. Therefore, weare pleased with the behavior of our model and its ability to predict the motionof the toy given a variety of moves that a user could perform.

10 Future Usage

Because of the complexity of the system, this would probably be too difficult ofa problem to include on a problem set or an exam. However, we found it to be afun and rewarding project and would highly recommend it to future teams. Wewould suggest that those teams use what we have done and try to implementnon-ideal spring models for the strings.

18

11 References and Acknowledgements

We used lecture notes and our learning from previous problem sets and examsto do this project. We would like to acknowledge our professor, Chris Lee, forproviding this material and for being a resource for us to go to with questions.We would also like to thank Luke Metz for helping us gather and process usefuldata from the motion capture studio.

19

APPENDIX

1 % Astrojax , 3 mass , moving in 3 dimensions ( with v i s c ou sdamping )

2 % Danie l Dichter and Kate Maschan3 % Dynamics f i n a l p ro j e c t , Fa l l 20134 % Coordinate system : c a r t e s i a n5

6 f unc t i on a s t r o j a x V c o n i c a l 27 c l e a r8 c l c9

10 % Def ine system parameters11 g = 9 . 8 ; % grav i ty (m/ s ˆ2)12 m2 = 0 .02 257 ; % mass o f m2 ( kg )13 m3 = 0 .02 257 ; % mass o f m3 ( kg )14 l 1 = 0 . 3 7 6 5 ; % length o f s t r i n g 1 (m)15 l 2 = 0 . 4 1 2 ; % length o f s t r i n g 2 (m)16 k = 280 ; % spr ing constant (N/m)17 c = 0 . 000 75 ; % drag c o e f f i c i e n t , found

exper imenta l l y18

19 endTime=10; % r e l a t i v e time at which to stop s imulat ion ,seconds

20

21 % Load data in .22 a=load ( ’ c o n i c a l ’ ) ;23 et=a . et ;24 et=et−min ( et ) ; % time25 rb1=a . rb1 ; % r i g i d body 1 (x , y , z ) ( upper mass )26 rb2=a . rb2 ; % r i g i d body 2 (x , y , z ) ( middle mass )27 rb3=a . rb3 ; % r i g i d body 3 (x , y , z ) ( lower mass )28

29 % Crop the exper imenta l data to match the s imu la t i on timerange

30 [ ˜ , endInd ] = min ( abs ( et−endTime ) ) ;31 et=et ( 1 : endInd ) ;32 rb1=rb1 ( 1 : endInd , : ) ;33 rb2=rb2 ( 1 : endInd , : ) ;34 rb3=rb3 ( 1 : endInd , : ) ;35

36 run t ime = l i n s p a c e (0 , endTime , 10000) ; % time vec tor f o rode45

37

38 % Def ine i n i t i a l c o n d i t i o n s .39 % Data format : [ r2x ; r2y ; r2z ; v2x ; v2y ; v2z ; r3x ; r3y ;

20

r3z ; v3x ; v3y ; v3z ]40 % dt=et (2 )−et (1 ) ;41 % IC = [ rb2 ( 1 , 1 : 3 ) ’ ; ( rb2 (2 , 1 )−rb2 (1 , 1 ) ) /dt ; ( rb2 (2 , 2 )−

rb2 (1 , 2 ) ) /dt ; ( rb2 (2 , 3 )−rb2 (1 , 3 ) ) /dt ; . . .42 % rb3 ( 1 , 1 : 3 ) ’ ; ( rb3 (2 , 1 )−rb3 (1 , 1 ) ) /dt ; ( rb3 (2 , 2 )−

rb3 (1 , 2 ) ) /dt ; ( rb3 (2 , 3 )−rb3 (1 , 3 ) ) /dt ] ;43 IC=[ rb1 (1 , 1 ) ; rb1 (1 , 2 ) ; rb1 (1 , 3 )−l 1 ; 0 ; 0 ; 0 ; . . .44 rb1 (1 , 1 ) ; rb1 (1 , 2 ) ; rb1 (1 , 3 )−l1−l 2 ; 0 ; 0 ; 0 ] ;45

46 % Pos i t i on o f m1 as a func t i on o f time (known) frommotion capture data

47 f unc t i on r e s = r1 ( t )48 r1x = in t e rp1 ( et , rb1 ( : , 1 ) , t ) ; % i n t e r p o l a t e

because sampling r a t e s d i f f e r49 r1y = in t e rp1 ( et , rb1 ( : , 2 ) , t ) ;50 r1z = in t e rp1 ( et , rb1 ( : , 3 ) , t ) ;51 r e s = [ r1x ; r1y ; r1z ] ;52 end53

54 % Get v e l o c i t y and a c c e l e r a t i o n from p o s i t i o n andv e l o c i t y ( from ode45 )

55 f unc t i on r e s = d e r i v s ( time , Z)56 % Data format : [ r2x ; r2y ; r2z ; v2x ; v2y ; v2z ; r3x

; r3y ; r3z ; v3x ; v3y ; v3z ]57 R1 = r1 ( time ) ; % p o s i t i o n o f m158 r1x = R1(1) ; % in x59 r1y = R1(2) ; % in y60 r1z = R1(3) ; % in z61

62 % p o s i t i o n s and v e l o c i t i e s f o r m2 and m363 r2x = Z(1) ;64 r2y = Z(2) ;65 r2z = Z(3) ;66 v2x = Z(4) ;67 v2y = Z(5) ;68 v2z = Z(6) ;69 r3x = Z(7) ;70 r3y = Z(8) ;71 r3z = Z(9) ;72 v3x = Z(10) ;73 v3y = Z(11) ;74 v3z = Z(12) ;75

76 d1 = s q r t ( ( r1x − r2x ) ˆ2 + ( r1y − r2y ) ˆ2 + ( r1z −r2z ) ˆ2) ; % d i s t ance between m1 and m2

77 d2 = s q r t ( ( r2x − r3x ) ˆ2 + ( r2y − r3y ) ˆ2 + ( r2z −

21

r3z ) ˆ2) ; % d i s t ance between m2 and m378

79 % Drag fo r ce , assume 0 i f not moving80 dr2 = [ 0 ; 0 ; 0 ] ;81 dr3 = [ 0 ; 0 ; 0 ] ;82 speed2 = s q r t ( v2xˆ2+v2yˆ2+v2z ˆ2) ; % speed o f m283 speed3 = s q r t ( v3xˆ2+v3yˆ2+v3z ˆ2) ; % speed o f m384

85 % I f the mass i s moving , add drag f o r c e86 i f speed2 > 087 v2 hat = [ v2x ; v2y ; v2z ] / speed2 ;88 dr2 = −c∗ speed2 ∗ v2 hat ; % ( v i s c ou s damping ) [

dr2x ; dr2y ; dr2z ]89 end90 i f speed3 > 091 v3 hat = [ v3x ; v3y ; v3z ] / speed3 ;92 dr3 = −c∗ speed3 ∗ v3 hat ; % ( v i s c ou s damping ) [

dr3x ; dr3y ; dr3z ]93 end94

95 % Spring fo r ce , assume s t r i n g s are unst re tched96 spr ing1 = [ 0 ; 0 ; 0 ] ;97 spr ing2 = [ 0 ; 0 ; 0 ] ;98

99 % I f s t r i n g s are s t re tched , add spr ing f o r c e100 i f d1 > l 1101 s1 hat = [ r1x − r2x ; r1y−r2y ; r1z−r2z ] / d1 ; %

po in t s m2 to m1102 spr ing1 = ( k∗( d1−l 1 ) ) ∗ s1 hat ; % magnitude o f

spr ing1 f o r c e103 end104 i f d2 > l 2105 s2 hat = [ r2x − r3x ; r2y−r3y ; r2z−r3z ] / d2 ; %

po in t s m3 to m2106 spr ing2 = ( k∗( d2−l 2 ) ) ∗ s2 hat ; % magnitude o f

spr ing2 f o r c e107 end108

109 % Calcu la te x , y , z a c c e l e r a t i o n s (m/ s ˆ2) f o r m2& m3

110 a2x = ( spr ing1 (1 ) − spr ing2 (1 ) + dr2 (1 ) ) /m2;111 a2y = ( spr ing1 (2 ) − spr ing2 (2 ) + dr2 (2 ) ) /m2;112 a2z = ( spr ing1 (3 ) − spr ing2 (3 ) + dr2 (3 ) ) /m2 −g ;113 a3x = ( spr ing2 (1 ) + dr3 (1 ) ) /m3;114 a3y = ( spr ing2 (2 ) + dr3 (2 ) ) /m3;115 a3z = ( spr ing2 (3 ) + dr3 (3 ) ) /m2 −g ;

22

116

117 r e s = [ v2x ; v2y ; v2z ; a2x ; a2y ; a2z ; v3x ; v3y ;v3z ; a3x ; a3y ; a3z ] ;

118 end119

120 [T, ode ] = ode45 ( @derivs , run time , IC ) ;121

122 % Data format : [ r2x ; r2y ; r2z ; v2x ; v2y ; v2z ; r3x ; r3y ;r3z ; v3x ; v3y ; v3z ]

123 c l e a r R1 r1x r1y r1z r2x r2y r2z v2x v2y v2z a2x a2y a2z. . . % c l e a r out v a r i a b l e s from workspace

124 r3x r3y r3z v3x v3y v3z a3x a3y a3z125

126 % Simulat ion data127 r2x = ode ( : , 1) ;128 r2y = ode ( : , 2) ;129 r2z = ode ( : , 3) ;130 v2x = ode ( : , 4) ;131 v2y = ode ( : , 5) ;132 v2z = ode ( : , 6) ;133 r3x = ode ( : , 7) ;134 r3y = ode ( : , 8) ;135 r3z = ode ( : , 9) ;136 v3x = ode ( : , 10) ;137 v3y = ode ( : , 11) ;138 v3z = ode ( : , 12) ;139

140 % Plot r e s u l t s===================================================

141

142 % I n d i v i d u a l axes p l o t s143

144 f i g u r e (1 )145 c l f146

147 c1 =0; % black148 c2 =0.7 ; % l i g h t gray149 width =2; % l i n e width150

151 subplot ( 3 , 2 , 1 )152 hold on153 p lo t (T, r2x , ’ Color ’ , [ c1 c1 c1 ] , ’ LineWidth ’ , width )154 p lo t ( et , rb2 ( : , 1 ) , ’ Color ’ , [ c2 c2 c2 ] , ’ LineWidth ’ , width )155 t i t l e ( ’ Mass 2 ’ )156 y l a b e l ( ’ x d i sp lacement ( meters ) ’ )157

23

158 subplot ( 3 , 2 , 2 )159 hold on160 p lo t (T, r3x , ’ Color ’ , [ c1 c1 c1 ] , ’ LineWidth ’ , width )161 p lo t ( et , rb3 ( : , 1 ) , ’ Color ’ , [ c2 c2 c2 ] , ’ LineWidth ’ , width )162 t i t l e ( ’ Mass 3 ’ )163 l egend ( ’ S imulat ion ’ , ’ Experiment ’ )164

165 subplot ( 3 , 2 , 3 )166 hold on167 p lo t (T, r2y , ’ Color ’ , [ c1 c1 c1 ] , ’ LineWidth ’ , width )168 p lo t ( et , rb2 ( : , 2 ) , ’ Color ’ , [ c2 c2 c2 ] , ’ LineWidth ’ , width )169 y l a b e l ( ’ y d i sp lacement ’ )170

171 subplot ( 3 , 2 , 4 )172 hold on173 p lo t (T, r3y , ’ Color ’ , [ c1 c1 c1 ] , ’ LineWidth ’ , width )174 p lo t ( et , rb3 ( : , 2 ) , ’ Color ’ , [ c2 c2 c2 ] , ’ LineWidth ’ , width )175

176 subplot ( 3 , 2 , 5 )177 hold on178 p lo t (T, r2z , ’ Color ’ , [ c1 c1 c1 ] , ’ LineWidth ’ , width )179 p lo t ( et , rb2 ( : , 3 ) , ’ Color ’ , [ c2 c2 c2 ] , ’ LineWidth ’ , width )180 x l a b e l ( ’Time ( seconds ) ’ )181 y l a b e l ( ’ z d i sp lacement ’ )182

183 subplot ( 3 , 2 , 6 )184 hold on185 p lo t (T, r3z , ’ Color ’ , [ c1 c1 c1 ] , ’ LineWidth ’ , width )186 p lo t ( et , rb3 ( : , 3 ) , ’ Color ’ , [ c2 c2 c2 ] , ’ LineWidth ’ , width )187 x l a b e l ( ’Time ( seconds ) ’ )188

189 s e t ( f i n d a l l ( gcf , ’ type ’ , ’ t ex t ’ ) , ’ f o n t S i z e ’ ,12 , ’ fontWeight ’, ’ bold ’ ) % make the text b igge r and bo lder

190

191 % 3D side−by−s i d e comparison−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−

192

193 f i g u r e (2 )194 c l f195 subplot ( 1 , 2 , 1 )196 hold on197

198 co l 1 =0.8 ; % co lo r , f o r l i g h t gray199 co l 2 =0.6 ; % co lo r , f o r darker l i g h t gray200 v =[45 , 10 ] ; % 3D view angle201

24

202 R1 = r1 (T( end ) ) ;203 r1x = R1(1) ;204 r1y = R1(2) ;205

206 p lo t3 ( r1x , r1y , r1z , ’ bo ’ ) % m1207 p lo t3 ( [ r1x , r2x ( end ) ] , [ r1y , r2y ( end ) ] , [ r1z , r2z ( end ) ] , ’ k

−− ’ ) % s t r i n g 1208 p lo t3 ( r2x ( end ) , r2y ( end ) , r2z ( end ) , ’ ro ’ ) % m2209 p lo t3 ( [ r2x ( end ) , r3x ( end ) ] , [ r2y ( end ) , r3y ( end ) ] , [ r2z ( end )

, r3z ( end ) ] , ’ k−− ’ ) % s t r i n g 2210 p lo t3 ( r3x ( end ) , r3y ( end ) , r3z ( end ) , ’ go ’ ) % m3211

212 p lo t3 ( r2x , r2y , r2z , ’ Color ’ , [ c o l 1 co l 1 co l 1 ] , ’ LineWidth ’, 2 ) % m2 t r a j e c t o r y

213 p lo t3 ( r3x , r3y , r3z , ’ Color ’ , [ c o l 2 co l 2 co l 2 ] , ’ LineWidth ’, 2 ) % m3 t r a j e c t o r y

214

215 a x i s square216 a x i s equal217 g r id on218

219 x l a b e l ( ’ x ’ )220 y l a b e l ( ’ y ’ )221 z l a b e l ( ’ z ’ )222 t i t l e ( ’ S imulat ion ’ )223 s e t ( f i n d a l l ( gcf , ’ type ’ , ’ t ex t ’ ) , ’ f o n t S i z e ’ ,12 , ’ fontWeight ’

, ’ bold ’ )224

225 view ( v ) % s e t the 3D viewing ang le226

227 %%%%%%%%%%%%228

229 subplot ( 1 , 2 , 2 )230 hold on231

232 p lo t3 ( rb1 ( end , 1 ) , rb1 ( end , 2 ) , rb1 ( end , 3 ) , ’ bo ’ ) % m1233 p lo t3 ( [ rb1 ( end , 1 ) , rb2 ( end , 1 ) ] , [ rb1 ( end , 2 ) , rb2 ( end , 2 ) ] , [

rb1 ( end , 3 ) , rb2 ( end , 3 ) ] , ’ k−− ’ ) % s t r i n g 1234 p lo t3 ( rb2 ( end , 1 ) , rb2 ( end , 2 ) , rb2 ( end , 3 ) , ’ ro ’ ) % m2235 p lo t3 ( [ rb2 ( end , 1 ) , rb3 ( end , 1 ) ] , [ rb2 ( end , 2 ) , rb3 ( end , 2 ) ] , [

rb2 ( end , 3 ) , rb3 ( end , 3 ) ] , ’ k−− ’ ) % s t r i n g 2236 p lo t3 ( rb3 ( end , 1 ) , rb3 ( end , 2 ) , rb3 ( end , 3 ) , ’ go ’ ) % m3237

238 p lo t3 ( rb2 ( : , 1 ) , rb2 ( : , 2 ) , rb2 ( : , 3 ) , ’ Color ’ , [ c o l 1 co l 1co l 1 ] , ’ LineWidth ’ , 2 ) % m2 t r a j e c t o r y

239 p lo t3 ( rb3 ( : , 1 ) , rb3 ( : , 2 ) , rb3 ( : , 3 ) , ’ Color ’ , [ c o l 2 co l 2

25

co l 2 ] , ’ LineWidth ’ , 2 ) % m3 t r a j e c t o r y240

241 a x i s square242 a x i s equal243 g r id on244

245 x l a b e l ( ’ x ’ )246 y l a b e l ( ’ y ’ )247 z l a b e l ( ’ z ’ )248 t i t l e ( ’ Experiment ’ )249 s e t ( f i n d a l l ( gcf , ’ type ’ , ’ t ex t ’ ) , ’ f o n t S i z e ’ ,12 , ’ fontWeight ’

, ’ bold ’ )250

251 view ( v ) % s e t the 3D viewing ang le252

253 end

end

26