Hybrid Homework 2 C

Hybrid Dynamics
Homework 2
Josef Nasim 208667139

David Dayan 941170037
May 12, 2021
1
Part A
1. Write the system’s non-holonomic constraints of zero lateral slippage in the form wj (q) ,
q=0 for j=0,1,2, and also in matrix form as Wb (q)q̇b + Ws (q)q̇s = 0
The constraint we choose will be the the lateral velocity of each the axles will be zero. We will first
find the velocities of each wheeled axle. (insert velocities for each wheel) from 0 to 2. Namely we will find
v · ei2 = 0
Now we will define a coordinate system for each of the links of the form eij with the respect to the
fixed world coordinate frame given. Where i identifies the link, and j the coordinate axis number. For
example e12 is the first link and the second axis ( the e02 axis). The ei2 axis will always point perpendicular
to the length of the link and will represent the direction of lateral movement of the wheel. Since we only
care about this direction, these are the coordinate axe we will find
link 0 e02 = −sin(θ) · e1 + cos(θ) · e2
link 1 e02 = −sin(θ + φ1 ) · e1 + cos(θ + φ1 ) · e2
link 2 e02 = −sin(θ + φ2 ) · e1 + cos(θ + φ2 ) · e2
The velocities for each link are:
link 0 v0 = (ẋ, ẏ, 0)T
link 1 v1 = (ẋ + bθ̇sin(θ) + Lsin(φ1 + θ)(φ˙1 + θ̇), ẏ − Lcos(φ1 + θ)(φ˙1 + θ̇) − bθ̇cos(θ), 0)T
link 2 v2 = (ẋ − bθ̇sin(θ) − Lsin(φ2 θ)(φ˙2 − θ̇), ẏ + Lcos(φ2 + θ)(φ˙2 + θ̇) + bθ̇cos(θ), 0)T
We will know find the constraints vei2 and put into the from W q̇
 
ẋ
 
−sin(θ) cos(θ) 0 0 0  ẏ 

Wq= −sin(φ1 + θ) cos(φ1 + θ) −L − bcos(φ1 ) −L 0  θ̇ 
  
−sin(φ2 + θ) cos(φ2 + θ) −L − bcos(φ2 ) 0 L φ˙1 

φ˙2
From here we can get Wb q̇b and Ws q̇s
2
    
−sin(θ) cos(θ) 0 ẋ 0 0 ˙
φ
Wb q̇b + Wb q̇b = −sin(φ1 + θ) cos(φ1 + θ) −L − bcos(φ1 ) ẏ  + −L 0  ˙1 = 0
φ2
−sin(φ2 + θ) cos(φ2 + θ) −L − bcos(φ2 ) θ̇ 0 L
2. Find an analytic condition for a singular configuration where det(Wb ) = 0. For the given
numerical values b= 0.15m,l= 0.1m, φ1 = 70◦ , find two different values of the joint angle φ2
which correspond to a singular configuration. Using a computer program, draw the robot in
both singular configurations, and give a general geometric characterization of the singularity
condition det(Wb ) = 0.
det(Wb ) = 0.15sin(φ2 + 1.2217) + 0.1sin(φ2 ) + 0.0940 = 0
From here the solutions found are −70.0◦ and 164.0561◦

The singularities drawn are
3
3. Plot 3 graphs of x(t), y(t), θ(t) as a function of time, and another plot of the trajectory
of the middle link’s center in xy plane (use command axis equal).
Figure 1 x(t)[m] versus t[s]
Figure 2 y(t)[m] versus t[s]
4
Figure 3 θ(t)[radians] versus t[s]
Figure 4 x(t)[m] versus y(t)[m]
4. For ψ = π4 find conditions on α, βwhich guarantee that the robot does not pass through a
singular configuration during its entire motion.
5
Once again we will find det(Wb ) = 0. After simplifying a couple steps our final result is this.
0 = −0.7071β1 sin(t) − α1
In order for a singularity to occur this condition must occur. Therefore we want the equality not too hold.
6
Part B
1. Write explicit expression for the system’s kinetic T (q, q̇) And the two nonholonomic con-
straints of zero lateral slippage in the form wj(q)q=0 for j=1,2.
The kinetic energy for a planar rigid body is: T = 0.5m(vc · vc ) + 0.5Ic (ω · ω)
Where vc is the velocity of the center of mass and ω is the angular velocity of the body. We can con-
sider this equation because the link attached to the axels are of negligible weight compared to the weight
of the load, which is located at the center of mass.
We have the vc is: vc = [ẋ − dθ̇sin(θ), ẏ + dθ̇cos(θ), 0]
And that ω = [0, 0, θ̇]

From here we can calculate T:
T = 0.5Ic θ̇2 + 0.5m((ẏ + dθ̇cos(θ))2 + (ẋ − dθ̇sin(θ))2 )
The constraint we choose will be the the lateral velocity of each the axles will be zero. We will first
find the velocities of center of each wheeled axle. .Namely we will find v · ei2 = 0. Where e − i2 is the
direction of lateral movement.
Now we will define a coordinate system for each of the links of the form eij with the respect to the fixed
world coordinate frame given. Where i identifies the axel and j the coordinate axis number. For example
eb2 is the back axel and the second axis ( the e02 axis). The ei2 axis will always point perpendicular to the
length of the link attached to the center of the axel and will represent the direction of lateral movement of
the pair of wheels. Since we only care about this direction, these are the coordinate axe we will find
back axel: eb2 = −sin(θ) · e1 + cos(θ) · e1;

front axel: ef 2 = −sin(θ + φ) · e1 + cos(θ + φ) · e1;
The velocities of the axis are defined as vp the velocity of point p and the velocity of front axle as vf .
vp = [ẋ, ẏ, 0]T
vf = [ẋ − Lθ̇sin(θ) − b(θ̇ + φ̇)sin(θ + φ), ...ẏ + Lθ̇cos(θ) + b(θ̇ + φ̇)cos(θ + φ), 0]T T
7
Now we will find vp · eb2 = 0 and vf · ef2 = 0 and put into the form of wj (q)q̇ = 0
 
ẋ
−sin(θ) cos(θ) 0  ẏ 
0  
wj (q)q̇ =
−sin(φ + θ) cos(φ + θ) b + Lcos(φ) b  θ̇ 
φ̇
2. Write the vehicle’s constrained equations of motion, using two Lagrange’s multipliers λ1 , λ2
Physical values for simulations are: m=30Kg, l=0.6m, b=0.2m, d=0.25m, Ic=0.15Kgm2
We are dealing with a under-actuated robot with non-holonomic constraints. There are the body coor-
dinates qb and the shape coordinates qs . In our case qb = [x, y, θ]T and qs = φ.We also have two constraints
so our lambda = [lambda1 , lambda2 ]T
The general form of the equations of motions for this type of motion where the torque does
the actuation is:(We will consider this formulation or the equation of motion because of the preceding
question requires actuation through torque)
    
Mbb Mbs −WbT q̈b −Bb − Gb
T
Mbs Mss −WsT  q̈s  =  τ − Bs − Gs 
Wb Ws 0 λ −Ẇb q̇b − Ẇs q̇s
Let us find each Component of the matrix. Starting with WS and Wb

−sin(θ) cos(θ) 0 0
Wb = , Ws =
−sin(φ + θ) cos(φ + θ) b + Lcos(φ) b
Where Wb q̇b represents the constraint equations for the body coordinate and Ws q̇s for the shape coordinates.
The equation for the matrix M:

∂ 2T

Mbb Mbs
M= =
M bsT Mss ∂ q̇i ∂ q̇j
Where the partial derivatives of the Kinetic Energy with respect to the body coordinates only correspond
to Mbb and for Mss only to the shape coordinate. For Mbs it corresponds to one partial derivation with
respect to a body coordinate than a shape coordinate.From here it follows M is:
 
m 0 −dmsin(θ) 0
 0 m dmcos(θ) 0
M =
dmsin(θ) dmcos(θ) md2 + Ic 0

0 0 0 0
8
The equation for B is :
n
∂ 2T

Bb X ∂T
Bij = = −
Bs ∂ q˙i ∂qj ∂qi
j=1
Where the partial derivatives of the Kinetic Energy with respect to the body coordinates only correspond
to Bb . For Bs it corresponds to one partial derivation with respect to a body coordinate than a shape
coordinate.From here it follows B is:
 
−dθ̇2 mcos(θ)
−dθ̇2 msin(θ)
B= 
 0 
0
3.Conduct numerical simulation of the vehicle’s motion under initial conditions q(0) = q̇(0) = 0
and actuation torque at the joint given by τ (t) = 0.5cos(t), for time range of 0t100sec. Plot
the following graphs from numerical simulations:
A) Steering angle φ(t) in degrees as a function of time.
9
As we can see from the graph φ has an oscillatory behavior which is to be expected as τ is also oscillatory.
B) Body orientation angle θ(t) in degrees as a function of time.
As we can see from the graph θ has an oscillatory behavior which is opposite to φ which characterises the
motion of the twist-cart.
C)Velocity of the point P projected along body direction e01 ,as a function of time.
10
As the twist-cart starts its motion its hard to accelerate but once it has enough momentum the cart can ad-
vance more smoothly, we can see this behavior in the graph by the small divot at the beginning of the motion
D)Trajectory of the point P in x-y plane.
11
The motion of the twist car is oscillatory around a straight line due to the oscillations of the torque.
E)Plots of the two constraint forces λ1 , λ2
12
In this graph of the constraint forces we see that they work in opposite faces which can also be seen in the
characterised motion of the twist-cart.
F)Plot of the X̂ component of the total ground reaction force applied by the ground on the vehicle’s
wheels (constraint forces), as a function of time. In addition, plot on the same graph the quantity mr̈c · x̂
as a function of time, where rc is the vehicle’s center-of-mass position.
13
As we can see the graphs of m · r̈c and the total force f they are identical which satisfies f = m · r̈c .The
acceleration was obtained from the x component of the acceleration of the center of mass, and the forces
along the x direction were obtained by multiplying the force vector ~λ by the first column of W which
corresponds to the generalized coordinate x
G) the two slippage velocities.
14
The slippage velocities as constrained by the definition of the problem should be zero and as we can see
they are of the order of 10− 4 which can be attributed to the truncation errors in the numerical solutions.
4.Conduct numerical simulation of the vehicle’s motion assuming that the steering angle
(in radians) is directly prescribed as φ(t) = π4 cos(t), for time range of 0t100sec. Initial condi-
tions q(0) = q̇(0) = 0 Draw graphs as in items (b)-(f ) above, and also a time plot of the torque
τ (t)
B) Body orientation angle θ(t) in degrees as a function of time.
15
θ for the new conditions
C)Velocity of the point P projected along body direction e01 ,as a function of time..
16
17
As we can see from the figure the cart moves backwards accelerating with every cycle represented by the
length of the ”waves” as it moves
E)Plots of the two constraint forces λ1 , λ2
18
This graph of the constraint forces seems to have an error as the forces should be in opposite phase yet we
were unable to find the mistake
F)Plot of the X̂ component of the total ground reaction force applied by the ground on the vehicle’s
wheels (constraint forces), as a function of time. In addition, plot on the same graph the quantity mr̈c · x̂
as a function of time, where rc is the vehicle’s center-of-mass position.
19
As a result of the error in the previous graph the forces and acceleration do not match yet they are very
similar
G) Torque tau as a function of time.
20
Here we see the graph of the torque, it oscillates with a growing amplitude as it requires a stronger force
to turn with the amplitude of φ which remains constant
5. Conduct numerical simulation under the same conditions as item 4 above, except for
a change in the body’s center-of-mass position d=0.15m. Draw plots as in items (c),(d) only.
21
We can see from the graph that if the center of mass is moved further back in the twist-cart the cart will
start moving forwards rather than backwards.
22
From this graph we can see that the motion is almost identical to the previous section except that now the
twist-cart moves forwards.
6.Conduct numerical simulation under the same conditions as item 5 above with d=0.15m,
except for changing the input’s amplitude φ(t) = π3 cos(t) Draw plots as in items (c),(d) only.
23
We can see that increasing the amplitude of the oscillations causes the cart to change direction once more.
24
With regards to the motion of the cart we can see that its similar to the previous section yet it has sharper
turns and moves in the opposite direction.
7.Conclusion
In this homework project we were able to see in the 3-link skating robot the constraints and singularities
of it to understand the motion of such a robot. One of the singularities occur when the links have the
same intersection point along the direction of the link. This motion of the robot is constrained only in one
direction in this configuration.
For Part B We can see that the rotation of the axle will cause almost linear path. It can also be seen that
the smaller the actuation by φ the smaller the difference from a straight line it will be. Highlighted in
the difference between question 4 and question 5. We can also see that this will result in a slower overall
average velocity.
Appendix
Part A
1 %Question 1
2
25
3 syms x y th p1 p2 dx dy dth dp1 dp2 L b
4
5 q=[x ; y ; th ; p1 ; p2 ] ;
6 qd=[dx ; dy ; dth ; dp1 ; dp2 ] ;
7
8 e1 =[1 0 0 ] ;
9 e2 =[0 1 0 ] ;
10 e3 =[0 0 1 ] ;
11
12 e01=c o s ( th ) ∗ e1 + s i n ( th ) ∗ e2 ;
13 e02=−s i n ( th ) ∗ e1 +c o s ( th ) ∗ e2 ;
14
15 e11=c o s ( th+p1 ) ∗ e1 + s i n ( th+p1 ) ∗ e2 ;

16 e12=−s i n ( th+p1 ) ∗ e1 +c o s ( th+p1 ) ∗ e2 ;
17
18 e21=c o s ( th+p2 ) ∗ e1 + s i n ( th+p2 ) ∗ e2 ;

19 e22=−s i n ( th+p2 ) ∗ e1 +c o s ( th+p2 ) ∗ e2 ;
20
21 r 0 =[x , y , 0 ] ;
22 r 1 =[x−b∗ c o s ( th )−L∗ c o s ( th+p1 ) , y−b∗ s i n ( th )−L∗ s i n ( th+p1 ) , 0 ] ;
23 r 2 =[x+b∗ c o s ( th )+L∗ c o s ( th+p2 ) , y+b∗ s i n ( th )+L∗ s i n ( th+p2 ) , 0 ] ;
24
25 v0 =[ dx , dy , 0 ] ;
26 v1 =[dx + L∗ s i n ( p1 + th ) ∗ ( dp1 + dth ) + b∗ s i n ( th ) ∗ dth , dy − L∗ c o s ( p1 + th ) ∗ ( dp1
+ dth ) − b∗ c o s ( th ) ∗ dth , 0 ] ;
27 v2 =[dx − L∗ s i n ( p2 + th ) ∗ ( dp2 + dth ) − b∗ s i n ( th ) ∗ dth , dy + L∗ c o s ( p2 + th ) ∗ ( dp2
+ dth ) + b∗ c o s ( th ) ∗ dth , 0 ] ;
28
29 w0=v0 ∗ e02 . ’==0;

30 w1=v1 ∗ e12 . ’==0;
31 w2=v2 ∗ e22 . ’==0;
32 w0=c o l l e c t (w0 , [ dx dy dth dp1 dp2 ] ) ;
35
36 [W zb ]= eq u a ti onsToMatrix ( [ w0 , w1 , w2 ] , [ dx dy dth dp1 dp2 ] )

37 Wb=s i m p l i f y (W( 1 : 3 , 1 : 3 ) )
38 Ws=s i m p l i f y (W( 1 : 3 , 4 : 5 ) )
39 %%
40 %Question 2
41
42 c o n d i t i o n=d e t ( subs (Wb, [ b L p1 ] , [ 0 . 1 5 0 . 1 (70∗ p i /180) ] ) ) ;

43 c o n d i t i o n=s i m p l i f y ( expand ( c o n d i t i o n ) ) ==0;
44 s o l=e v a l ( s o l v e ( c o n d i t i o n , p2 , ’ Real ’ , t r u e ) )
45 s o l d e g r e e=rad2deg ( s o l )
46 %%
47 %Question 3
26
48
49 syms t
50 qbd=[dx dy dth ] . ’ ;
51 qsd =[dp1 dp2 ] . ’ ;
52 X s t a t e=−i n v (Wb) ∗ (Ws∗ qsd )−qbd ;
53 a lp h a=p i / 4 ;
54 b e t a=p i / 4 ;
55 psy=p i / 4 ;
56 omega=1;
57 p h i 1=a lp h a+b e ta ∗ s i n ( omega∗ t−psy ) ;
58 p h i 2=a lp h a+b e ta ∗ s i n ( omega∗ t+psy ) ;
59 phi1d= d i f f ( phi1 , t ) ;
60 phi2d= d i f f ( phi2 , t ) ;
61 X s t a t e=subs ( Xstate , [ p1 p2 dp1 dp2 ] , [ phi1 phi2 phi1d phi2d ] ) ;
62 odeeqn= X s t a t e ==0;
63 syms ex ( t ) ey ( t ) e th ( t )
64 odeeqn=subs ( odeeqn , [ x y th dx dy dth ] , . . .
65 [ ex ey e t h d i f f ( ex , t ) d i f f ( ey , t ) d i f f ( eth , t ) ] )
66 odeeqn=subs ( odeeqn , [ b L ] , [ 0 . 1 5 0 . 1 ] ) ;
67 [ V1 S ] = o d e T o V e c t o r F i e l d ( odeeqn ) ;
68 M1 = matlabFunction (V1 , ’ v a r s ’ , { ’ t ’ , ’Y ’ }) ;
69 yInit1 = [0 0 0 ] ;
70 i n t e r v a l =[0 10∗2∗ p i /omega ] ;
71 [ t3 , y3 ]= ode45 (M1, i n t e r v a l , y I n i t 1 ) ;
72 % p l o t ( t3 , y3 )
73 p l o t ( t3 , y3 ( : , 1 ) , ’ LineWidth ’ , 3 )%y
74 p l o t ( t3 , y3 ( : , 2 ) , ’ LineWidth ’ , 3 )%x
75 p l o t ( t3 , y3 ( : , 3 ) , ’ LineWidth ’ , 3 )%th
76 p l o t ( y3 ( : , 2 ) , y3 ( : , 1 ) , ’ LineWidth ’ , 3 )
77 %%
78 %Question 4
79 syms a1 b1
80 assume ( a1 , ’ Real ’ ) ; assume ( b1 , ’ Real ’ ) ;
81 psy=p i / 4 ;
82 omega=1;
83 ph1=a1+b1∗ s i n ( omega∗ t−psy ) ;
84 ph2=a1+b1∗ s i n ( omega∗ t+psy ) ;
85 c o n d i t i o n 2=subs (Wb, [ b L p1 p2 ] , [ 0 . 1 5 0 . 1 ph1 ph2 ] )
86 c o n d i t i o n 2=s i m p l i f y ( expand ( de t ( c o n d i t i o n 2 ) ) )==0
87 s o l=s o l v e ( c o n d i t i o n 2 , a1 )
88 s o l=s i m p l i f y ( expand ( s o l ) )
27
Part B
1 %Question 1
2
3 syms x y th p hi dx dy dth dphi L d m I c b

4
5 q=[x ; y ; th ; p h i ] ;
6 qd=[dx ; dy ; dth ; dphi ] ;
7
8 e1 =[1 0 0 ] ;
9 e2 =[0 1 0 ] ;
10 e3 =[0 0 1 ] ;
11
12 e01=c o s ( th ) ∗ e1 + s i n ( th ) ∗ e2 ;
13 e02=−s i n ( th ) ∗ e1 +c o s ( th ) ∗ e2 ;
14
15 e11=c o s ( th+p h i ) ∗ e1 + s i n ( th+p h i ) ∗ e2 ;

16 e12=−s i n ( th+p h i ) ∗ e1 +c o s ( th+p hi ) ∗ e2 ;
17
18 p=[x y 0 ] ;
19 %f i s t h e p o i n t i n th e middle o f t h e f r o n t a x l e
20 f =[x+L∗ c o s ( th )+b∗ c o s ( th+p hi ) , y+L∗ s i n ( th )+b∗ s i n ( th+ph i ) , 0 ] ;
21 r c =[x+d∗ c o s ( th ) , y+d∗ s i n ( th ) , 0 ] ;
22 vc =[dx−d∗ dth ∗ s i n ( th ) , dy+d∗ dth ∗ c o s ( th ) , 0 ] ;
23
24 w=[0 0 dth ] ;
25 T=(1/2) ∗m∗ ( vc ∗ vc . ’ ) +(1/2) ∗ I c ∗ dth ˆ 2 ;
26 % T=s i m p l i f y ( expand (T) )
27 pdot =[dx , dy , 0 ] ;
28 f d o t =[dx−L∗ dth ∗ s i n ( th )−b ∗ ( dth+dphi ) ∗ s i n ( th+ph i ) , . . .
29 dy+L∗ dth ∗ c o s ( th )+b ∗ ( dth+dphi ) ∗ c o s ( th+p hi ) , 0 ] ;
30 c o n d i t i o n 1= pdot ∗ e02 . ’ ;
31 c o n d i t i o n 2= f d o t ∗ e12 . ’ ;
32 c o n d i t i o n 2=s i m p l i f y ( expand ( c o n d i t i o n 2 ) ) ;
33 %%
34 %Question 2
35
36 syms tau ddx ddy ddth ddphi lambda1 lambda2

37
38 qdd=[ddx ddy ddth ddphi ] ;

39 M=h e s s i a n (T, qd ) ;
40 M=s i m p l i f y (M) ;
41 B=s i m p l i f y ( j a c o b i a n ( g r a d i e n t (T, qd ) , q ) ∗qd −g r a d i e n t (T, q ) ) ;
42 Fq=[0 0 0 tau ] ;
43 w1=c o l l e c t ( c o n d i t i o n 1 , [ dx dy dth dphi ] ) ==0;
44 w2=c o l l e c t ( c o n d i t i o n 2 , [ dx dy dth dphi ] ) ==0;
45 [W zb ]= eq u a ti onsToMatrix ( [ w1 , w2 ] , [ dx dy dth dphi ] ) ;
28
46
47 Mbb=M( 1 : 3 , 1 : 3 ) ;
48 Mbs=M( 1 : 3 , 4 ) ;
49 Mss=M( 4 , 4 ) ;
50
51 Bb=B ( 1 : 3 ) ;
52 Bs=B( 4 ) ;
53
54 Wb=W( 1 : 2 , 1 : 3 ) ;
55 Ws=W( 1 : 2 , 4 ) ;
56 Wd=[−dth ∗ c o s ( th ) −dth ∗ s i n ( th ) 0 0 ; −(dphi+dth ) ∗ c o s ( p hi+th ) . . .
57 −(dphi+dth ) ∗ s i n ( ph i+th ) −L∗ dphi ∗ s i n ( ph i ) 0 ] ;
58 Wbd=Wd( 1 : 2 , 1 : 3 ) ;
59 Wsd=Wd( 1 : 2 , 4 ) ;
60
61
62 qb=[x y th ] ; qbd=[dx dy dth ] ; qbdd=[ddx ddy ddth ] ;

63 qs=p h i ; qsd=dphi ; qsdd=ddphi ;
64 lambda =[ lambda1 ; lambda2 ] ;
65
66 Eqns=[Mbb Mbs −Wb. ’ ; Mbs . ’ Mss −Ws. ’ ; Wb Ws [ 0 ; 0 ] [ 0 ; 0 ] ] ∗ . . .

67 [ qbdd . ’ ; qsdd . ’ ; lambda]==[−Bb ; tau−Bs;−Wbd∗qbd . ’ −Wsd∗ qsd . ’ ] ;
68 %lambda2=−tau /b
69 s o l l a m b d a 1=s o l v e ( subs ( Eqns ( 1 ) , lambda2 ,− tau /b ) , lambda1 ) ;
70 s o l l a m b d a 1=s i m p l i f y ( expand ( s o l l a m b d a 1 ) ) ;
71 Lambda=subs ( lambda , [ lambda2 lambda1 ] , [ − tau /b s o l l a m b d a 1 ] ) ;
72
73 %%
74 %Question 3
75
76 syms t ex ( t ) ey ( t ) e t h ( t ) e p h i ( t )
77
78
79 s t a t e 1 =( i n v ( [ Mbb Mbs −Wb. ’ ; Mbs . ’ Mss −Ws. ’ ; Wb Ws [ 0 ; 0 ] [ 0 ; 0 ] ] ) ) ∗ . . .

80 [−Bb ; tau−Bs;−Wbd∗qbd . ’ −Wsd∗ qsd . ’ ] − [ qbdd . ’ ; qsdd . ’ ; lambda ] ;
81
82 s t a t e 1=subs ( s t a t e 1 , [ tau m L b d I c ] , [ 0 . 5 ∗ c o s ( t ) 30 0 . 6 0 . 2 0 . 2 5 0 . 1 5 ] ) ;
83 s t a t e=s t a t e 1 ( 1 : 4 ) ;
84 s t a t e=subs ( s t a t e , [ x y th ph i dx dy dth dphi ddx ddy ddth ddphi ] , . . .
85 [ ex ey e t h e p h i d i f f ( ex , t ) d i f f ( ey , t ) d i f f ( eth , t ) d i f f ( ephi , t ) . . .
86 d i f f ( ex , t , 2 ) d i f f ( ey , t , 2 ) d i f f ( eth , t , 2 ) d i f f ( ephi , t , 2 ) ] ) ;
87 s t a t e=s i m p l i f y ( expand ( s t a t e ) ) ;
88 odeeqn= s t a t e ==0;
89
90 [ V1 S ] = o d e T o V e c t o r F i e l d ( odeeqn ) ;
92 yInit1 = [ 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ; 0 ] ;
29
93 i n t e r v a l =[0 1 0 0 ] ;
95 %%
96
97 %a
98 p l o t ( t3 , y3 ( : , 7 ) . ∗ ( 1 8 0 / p i ) , ’ LineWidth ’ , 3 )
99 t i t l e ( ’ \ phi ( t ) ’ )
100 xlabel ( ’ t [ sec ] ’ )
101 y l a b e l ( ’ \ p h i ( t ) [ deg ] ’ )
102 %%
103 %b
105 t i t l e ( ’\ theta ( t ) ’ )
106 xlabel ( ’ t [ sec ] ’ )
107 y l a b e l ( ’ \ t h e t a ( t ) [ deg ] ’ )
108 %%
109 %c
110 p l o t ( t3 , ( y3 ( : , 4 ) . ∗ c o s ( y3 ( : , 1 ) )+y3 ( : , 6 ) . ∗ s i n ( y3 ( : , 1 ) ) ) , ’ LineWidth ’ , 3 )
111 t i t l e ( ’ V e l o c i t y o f P∗ e 1 ˆ\ prime ’ )
112 xlabel ( ’ t [ sec ] ’ )
113 y l a b e l ( ’ [m/ s ] ’ )
114 %%
115 %d
117 axis equal
118 t i t l e ( ’ t r a j e c t o r y of point P in the x , y plane ’ )
119 x l a b e l ( ’ x [m] ’ )
120 y l a b e l ( ’ y [m] ’ )
121 %%
122 %e
123 % plambda1=s t a t e 1 ( 5 ) ;
124 % plambda2=s t a t e 1 ( 6 ) ;
125 plambda1=− ( ( y3 ( : , 2 ) . ∗ y3 ( : , 4 ) . ∗ c o s ( y3 ( : , 1 ) ) + y3 ( : , 2 ) . ∗ y3 ( : , 6 ) . ∗ s i n ( y3 ( : , 1 ) ) )
. ∗ ( ( 2 2 5 . ∗ c o s ( y3 ( : , 1 ) ) . ˆ 2 ) /4 + ( 2 2 5 . ∗ s i n ( y3 ( : , 1 ) ) . ˆ 2 ) /4 − 243/4) ) / ( ( 8 1 . ∗ c o s
( y3 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y3 ( : , 1 ) ) . ˆ 2 ) /40) − ( 5 . ∗ c o s ( t 3 ) . ∗ ( ( 3 . ∗ c o s ( y3
( : , 1 ) ) . ˆ 2 ) /2 + ( 3 . ∗ s i n ( y3 ( : , 1 ) ) . ˆ 2 ) /2 + ( 9 . ∗ c o s ( y3 ( : , 7 ) ) . ∗ c o s ( y3 ( : , 1 ) ) . ˆ 2 )
/2 + ( 9 . ∗ c o s ( y3 ( : , 7 ) ) . ∗ s i n ( y3 ( : , 1 ) ) . ˆ 2 ) /2 − ( 8 1 . ∗ c o s ( y3 ( : , 7 ) + y3 ( : , 1 ) ) . ∗
c o s ( y3 ( : , 1 ) ) ) /40 − ( 8 1 . ∗ s i n ( y3 ( : , 7 ) + y3 ( : , 1 ) ) . ∗ s i n ( y3 ( : , 1 ) ) ) /40) )
/ ( 2 . ∗ ( ( 8 1 . ∗ c o s ( y3 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y3 ( : , 1 ) ) . ˆ 2 ) /40) ) ;
126 plambda1=plambda1 ( : , 1 5 ) ;
127 plambda2= −(5∗ c o s ( t 3 ) ) / 2 ;
128 p l o t ( t3 , plambda1 , ’ LineWidth ’ , 3 )
129 h o l d on
130 p l o t ( t3 , plambda2 , ’ LineWidth ’ , 3 )
131 t i t l e ( ’ C o n s t r a i n t f o r c e s \ lambda 1 and \ lambda 2 ’ )
132 l e g e n d ( ’ \ lambda 1 ’ , ’ \ lambda 2 ’ )
133 xlabel ( ’ t [ sec ] ’ )
30
134 y l a b e l ( ’ \lambda [N] ’ )
135 hold o f f
136 %%
137 %f
138 % vc =[dx−d∗ dth ∗ s i n ( th ) , dy+d∗ dth ∗ c o s ( th ) , 0 ] ;
139 % ac =[ddx−d∗ ddth ∗ s i n ( th )−d∗ dth ˆ2∗ c o s ( th ) , ddy+d∗ ddth ∗ c o s ( th )−d∗ dth ˆ2∗ s i n ( th )
,0];
140 xcom=30∗( −0.25∗ c o s ( y3 ( : , 1 ) ) . ∗ y3 ( : , 2 ) . ˆ 2 + ( [ 0 ; d i f f ( y3 ( : , 4 ) ) . / d i f f ( t 3 ) ] ) −
0 . 2 5 ∗ ( [ 0 ; d i f f ( y3 ( : , 2 ) ) . / d i f f ( t 3 ) ] ) . ∗ s i n ( y3 ( : , 1 ) ) ) ;
141 f c o n s=−s i n ( y3 ( : , 1 ) ) . ∗ plambda1−s i n ( y3 ( : , 7 )+y3 ( : , 1 ) ) . ∗ plambda2 ;
142 p l o t ( t3 , xcom , ’ LineWidth ’ , 2 )
143 h o l d on
144 p l o t ( t3 , f c o n s , ’ LineWidth ’ , 2 , ’ L i n e S t y l e ’ , ’−− ’ )
145 t i t l e ( ’ C o n s t r a i n t f o r c e comparison ’ )
146 l e g e n d ( ’ ma c ’ , ’ f ’ )
147 xlabel ( ’ t [ sec ] ’ )
148 y l a b e l ( ’ \lambda [N] ’ )
149 hold o f f
150 %%
151 %g
152 vp=−s i n ( y3 ( : , 1 ) ) . ∗ y3 ( : , 4 )+c o s ( y3 ( : , 1 ) ) . ∗ y3 ( : , 6 ) ;
153 v f =(y3 ( : , 4 ) −0.6∗ y3 ( : , 2 ) . ∗ s i n ( y3 ( : , 1 ) ) −0.2∗( y3 ( : , 2 )+y3 ( : , 8 ) ) . ∗ s i n ( y3 ( : , 1 )+y3
( : , 7 ) ) ) .∗( − s i n ( y3 ( : , 1 )+y3 ( : , 7 ) ) ) + . . .
154 ( y3 ( : , 6 ) +0.6∗ y3 ( : , 2 ) . ∗ c o s ( y3 ( : , 1 ) ) +0.2∗( y3 ( : , 2 )+y3 ( : , 8 ) ) . ∗ c o s ( y3 ( : , 1 )+y3
( : , 7 ) ) ) . ∗ ( c o s ( y3 ( : , 1 )+y3 ( : , 7 ) ) ) ;
155 p l o t ( t3 , vp , ’ LineWidth ’ , 2 )
156 h o l d on
157 p l o t ( t3 , vf , ’ LineWidth ’ , 2 , ’ L i n e S t y l e ’ , ’−− ’ )
158 t i t l e ( ’ Slippage v e l o c i t i e s ’ )
159 l e g e n d ( ’P ’ , ’ f ’ )
160 xlabel ( ’ t [ sec ] ’ )
161 y l a b e l ( ’ [m/ s ] ’ )
162 hold o f f
163 %%
164 %Question 4
165
166 phin =( p i / 4 ) ∗ c o s ( t ) ;
167 phind= d i f f ( phin , t ) ;
168 phindd= d i f f ( phin , t , 2 ) ;
169 s t a t e 4=i n v ( [ Mbb z e r o s ( 3 , 1 ) −Wb. ’ ; Mbs . ’ −1 −Ws . ’ ;Wb z e r o s ( 2 , 1 ) z e r o s ( 2 , 2 ) ] )
∗...
170 ( −[Mbs∗ qsdd+Bb ; Mss∗ qsdd+Bs ;Ws∗ qsdd+Wbd∗qbd . ’+Wsd∗ qsd ] ) −[qbdd . ’ ; tau ; lambda
];
171 s t a t e 4=subs ( s t a t e 4 , [m L b d I c p hi dphi ddphi ] , [ 3 0 0 . 6 0 . 2 0 . 2 5 0 . 1 5 phin
phind phindd ] ) ;
172 s t a t e 3=s t a t e 4 ( 1 : 3 ) ;
173 s t a t e=subs ( s t a t e 3 , [ x y th dx dy dth ddx ddy ddth ] , . . .
31
174 [ ex ey e t h d i f f ( ex , t ) d i f f ( ey , t ) d i f f ( eth , t ) . . .
175 d i f f ( ex , t , 2 ) d i f f ( ey , t , 2 ) d i f f ( eth , t , 2 ) ] ) ;
178
179 [ V2 S2 ] = o d e T o V e c t o r F i e l d ( odeeqn ) ;
181 yInit1 = [ 0 ; 0 ; 0 ; 0 ; 0 ; 0 ] ;
182 i n t e r v a l =[0 1 0 0 ] ;
184 %%
185 %b4
187 t i t l e ( ’\ theta ( t ) ’ )
188 xlabel ( ’ t [ sec ] ’ )
189 y l a b e l ( ’ \ t h e t a ( t ) [ deg ] ’ )
190 %%
191 %c4
194 xlabel ( ’ t [ sec ] ’ )
195 y l a b e l ( ’ [m/ s ] ’ )
196 %%
197 %d4
199 axis equal
201 x l a b e l ( ’ x [m] ’ )
202 y l a b e l ( ’ y [m] ’ )
203 %%
204 %e4
205 nlambda1 =(( y4 ( : , 2 ) . ∗ y4 ( : , 4 ) . ∗ c o s ( y4 ( : , 1 ) ) + y4 ( : , 2 ) . ∗ y4 ( : , 6 ) . ∗ s i n ( y4 ( : , 1 ) ) )
. ∗ ( 2 1 6 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) − 9 0 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4
( : , 1 ) ) + ( 2 4 3 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /4 + ( 2 4 3 . ∗ s i n ( y4 ( : , 1 ) +
( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /4 + 3 2 4 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 − ( 2 2 5 . ∗ c o s ( y4 ( : , 1 )
+ ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /4 − ( 2 2 5 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s (
t 4 ) ) / 4 ) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /4 − 9 0 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s (
y4 ( : , 1 ) ) − 2 7 0 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s
( t 4 ) ) / 4 ) − 2 7 0 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s
( t 4 ) ) / 4 ) + ( 2 2 5 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4
) ) / 4 ) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /2 + 3 6 ) ) / ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗
s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗
s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 )
) / 4 ) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s
( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 +
( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i
. ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) )
32
/ 2 0 ) + ( ( ( p i . ∗ c o s ( t 4 ) ) /20 + y4 ( : , 4 ) . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ ( y4
( : , 2 ) − ( p i . ∗ s i n ( t 4 ) ) /4 ) + y4 ( : , 6 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ ( y4
( : , 2 ) − ( p i . ∗ s i n ( t 4 ) ) /4 ) − ( 3 . ∗ y4 ( : , 2 ) . ∗ p i . ∗ s i n ( t 4 ) . ∗ s i n ( ( p i . ∗ c o s ( t 4 ) ) / 4) )
/ 2 0 ) . ∗ ( 4 5 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 − ( 2 4 3 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4
( : , 1 ) ) ) /4 + 4 5 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 + 1 3 5 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) )
/ 4 ) + 1 3 5 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) − ( 2 4 3 . ∗ c o s ( y4 ( : , 1 ) + ( p i
. ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) ) / 4) ) / ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 )
) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 )
) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗
s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) )
. ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4
( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) )
/ 4 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) −
( 1 5 . ∗ y4 ( : , 2 ) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ∗
c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) )
. ˆ 2 ) /2 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) ) /40 + ( 5 4 . ∗
c o s ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 9 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) )
/ 4 ) . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /2 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗
c o s ( t 4 ) ) / 4 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) ) ) /40 − ( 3 . ∗ s i n ( y4
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /2 − ( 9 . ∗ s i n ( y4 ( : , 1 )
+ ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) / 2) )
/ ( 2 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) )
. ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) )
/5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗
s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) )
. ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) )
/ 4 ) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4
) ) / 4 ) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) ) + ( 1 5 . ∗ y4 ( : , 2 ) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) )
. ∗ ( ( 6 . ∗ s i n ( y4 ( : , 1 ) ) ) /5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 . ∗
s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /2 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + (
p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) ) /40 + ( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s ( t 4 )
) / 4 ) . ˆ 2 ) /5 + ( 9 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i
. ∗ c o s ( t 4 ) ) / 4) ) /2 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) + ( p i
. ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) ) /40 − ( 3 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s (
y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /2 − ( 9 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4
( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) / 2) ) / ( 2 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 )
/5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) )
/5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + (
p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) )
/ 4 ) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4)
. ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗
s i n ( y4 ( : , 1 ) ) ) /20) ) ;
206
207 nlambda1=nlambda1 ( : , 1 4 1 ) ;
208 nlambda2 =(( y4 ( : , 2 ) . ∗ y4 ( : , 4 ) . ∗ c o s ( y4 ( : , 1 ) ) + y4 ( : , 2 ) . ∗ y4 ( : , 6 ) . ∗ s i n ( y4 ( : , 1 ) ) )
. ∗ ( 4 5 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 − ( 2 4 3 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 )
33
) ) /4 + 4 5 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 + 1 3 5 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) +
1 3 5 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) − ( 2 4 3 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s (
t 4 ) ) / 4 ) . ∗ c o s ( y4 ( : , 1 ) ) ) / 4) ) / ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 )
/5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) )
. ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n
( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) )
. ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4
( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) )
/ 4 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) +
( ( ( 2 4 3 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /4 + ( 2 4 3 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) / 4) . ∗ ( ( p i . ∗ c o s ( t 4 ) ) /20
+ y4 ( : , 4 ) . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ ( y4 ( : , 2 ) − ( p i . ∗ s i n ( t 4 ) ) /4 ) +
y4 ( : , 6 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ ( y4 ( : , 2 ) − ( p i . ∗ s i n ( t 4 ) ) /4 ) −
( 3 . ∗ y4 ( : , 2 ) . ∗ p i . ∗ s i n ( t 4 ) . ∗ s i n ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /20) ) / ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 )
/5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) )
/5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + (
p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) )
/ 4 ) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4)
. ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗
s i n ( y4 ( : , 1 ) ) ) /20) + ( 1 5 . ∗ y4 ( : , 2 ) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ∗ ( ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i
. ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /40 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗
c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /40) ) / ( 2 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4
( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n ( y4
( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4)
. ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4
( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗
s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s (
t 4 ) ) / 4 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) )
− ( 1 5 . ∗ y4 ( : , 2 ) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ∗ ( ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n
( y4 ( : , 1 ) ) . ˆ 2 ) /40 − ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n (
y4 ( : , 1 ) ) ) / 40) ) / ( 2 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 +
( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s
( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 )
) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /40 +
( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗
c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) ) ;
209
210 nlambda2=nlambda2 ( : , 1 4 1 ) ;
211 p l o t ( t4 , nlambda1 , ’ LineWidth ’ , 3 )
212 h o l d on
213 p l o t ( t4 , nlambda2 , ’ LineWidth ’ , 3 )
214 t i t l e ( ’ C o n s t r a i n t f o r c e s \ lambda 1 and \ lambda 2 ’ )
215 l e g e n d ( ’ \ lambda 1 ’ , ’ \ lambda 2 ’ )
216 xlabel ( ’ t [ sec ] ’ )
217 y l a b e l ( ’ \lambda [N] ’ )
218 hold o f f
34
219 %%
220 %f 4
221 xcom4=30∗( −0.25∗ c o s ( y4 ( : , 1 ) ) . ∗ y4 ( : , 2 ) . ˆ 2 + ( [ 0 ; d i f f ( y4 ( : , 4 ) ) . / d i f f ( t 4 ) ] ) −
0 . 2 5 ∗ ( [ 0 ; d i f f ( y4 ( : , 2 ) ) . / d i f f ( t 4 ) ] ) . ∗ s i n ( y4 ( : , 1 ) ) ) ;
222 f c o n s 4=−s i n ( y4 ( : , 1 ) ) . ∗ nlambda1−s i n ( ( p i / 4) ∗ c o s ( t 4 )+y4 ( : , 1 ) ) . ∗ nlambda2 ;
223 p l o t ( t4 , xcom4 , ’ LineWidth ’ , 2 )
224 h o l d on
225 p l o t ( t4 , f c o n s 4 , ’ LineWidth ’ , 2 , ’ L i n e S t y l e ’ , ’−− ’ )
226 t i t l e ( ’ C o n s t r a i n t f o r c e comparison ’ )
227 l e g e n d ( ’ ma c ’ , ’ f ’ )
228 xlabel ( ’ t [ sec ] ’ )
229 y l a b e l ( ’ \lambda [N] ’ )
230 hold o f f
231 %%
232 %tau
233 ntau =(3.∗ y4 ( : , 2 ) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ∗ ( ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n (
y4 ( : , 1 ) ) . ˆ 2 ) /40 − ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n (
y4 ( : , 1 ) ) ) / 40) ) / ( 2 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 +
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) ) − ( ( y4 ( : , 2 ) . ∗ y4
( : , 4 ) . ∗ c o s ( y4 ( : , 1 ) ) + y4 ( : , 2 ) . ∗ y4 ( : , 6 ) . ∗ s i n ( y4 ( : , 1 ) ) ) . ∗ ( 4 5 . ∗ c o s ( y4 ( : , 1 ) )
. ˆ 2 − ( 2 4 3 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) ) ) /4 + 4 5 . ∗ s i n ( y4
( : , 1 ) ) . ˆ 2 + 1 3 5 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) + 1 3 5 . ∗ s i n ( y4 ( : , 1 ) )
. ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) − ( 2 4 3 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4
( : , 1 ) ) ) / 4 ) ) / ( 5 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗
c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗
c o s ( t 4 ) ) / 4 ) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 )
/40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗
c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( (
p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ s i n ( y4 ( : , 1 ) +
( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20 ) ) − ( ( ( 2 4 3 . ∗ c o s ( y4 ( : , 1 ) )
. ˆ 2 ) /4 + ( 2 4 3 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) / 4) . ∗ ( ( p i . ∗ c o s ( t 4 ) ) /20 + y4 ( : , 4 ) . ∗ c o s ( y4
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ ( y4 ( : , 2 ) − ( p i . ∗ s i n ( t 4 ) ) /4 ) + y4 ( : , 6 ) . ∗ s i n ( y4
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ ( y4 ( : , 2 ) − ( p i . ∗ s i n ( t 4 ) ) /4 ) − ( 3 . ∗ y4 ( : , 2 ) . ∗ p i . ∗
s i n ( t 4 ) . ∗ s i n ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /20) ) / ( 5 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n (
y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 3 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 3 6 . ∗ s i n (
y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) ) /5 + ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4)
. ˆ 2 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /40 + ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 . ∗ c o s ( y4
( : , 1 ) ) . ˆ 2 ) /40 + ( 5 4 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 + ( 5 4 . ∗
s i n ( y4 ( : , 1 ) ) . ˆ 2 . ∗ c o s ( ( p i . ∗ c o s ( t 4 ) ) / 4) . ˆ 2 ) /5 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s (
t 4 ) ) / 4 ) . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) )
− ( 3 . ∗ y4 ( : , 2 ) . ˆ 2 . ∗ c o s ( y4 ( : , 1 ) ) . ∗ ( ( 8 1 . ∗ s i n ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s (
y4 ( : , 1 ) ) . ˆ 2 ) /40 − ( 8 1 . ∗ c o s ( y4 ( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n (
35
y4 ( : , 1 ) ) ) / 40) ) / ( 2 . ∗ ( ( 6 . ∗ c o s ( y4 ( : , 1 ) ) . ˆ 2 ) /5 + ( 6 . ∗ s i n ( y4 ( : , 1 ) ) . ˆ 2 ) /5 +
( : , 1 ) + ( p i . ∗ c o s ( t 4 ) ) / 4) . ∗ c o s ( y4 ( : , 1 ) ) . ∗ s i n ( y4 ( : , 1 ) ) ) /20) ) ;
234
235 ntau=ntau ( : , 1 4 1 ) ;
236 p l o t ( t4 , ntau , ’ LineWidth ’ , 3 )
237 t i t l e ( ’ \ tau ( t ) ’ )
238 xlabel ( ’ t [ sec ] ’ )
239 y l a b e l ( ’ \ tau [Nm] ’ )
240 %%
241 %Question 5
∗...
];
phind phindd ] ) ;
245 s t a t e 3=s t a t e 5 ( 1 : 3 ) ;
251
254 yInit1 = [ 0 ; 0 ; 0 ; 0 ; 0 ; 0 ] ;
255 i n t e r v a l =[0 1 0 0 ] ;
257 %%
258 %c5
261 xlabel ( ’ t [ sec ] ’ )
262 y l a b e l ( ’ [m/ s ] ’ )
263 %%
264 %d5
266 axis equal
268 x l a b e l ( ’ x [m] ’ )
269 y l a b e l ( ’ y [m] ’ )
270 %%
36
271 % Question 6
272 phin =( p i / 3 ) ∗ c o s ( t ) ;
273 phind= d i f f ( phin , t ) ;
274 phindd= d i f f ( phin , t , 2 ) ;
∗...
];
phind phindd ] ) ;
278 s t a t e 3=s t a t e 6 ( 1 : 3 ) ;
284
287 yInit1 = [ 0 ; 0 ; 0 ; 0 ; 0 ; 0 ] ;
288 i n t e r v a l =[0 1 0 0 ] ;
290 %%
291 %c6
294 xlabel ( ’ t [ sec ] ’ )
295 y l a b e l ( ’ [m/ s ] ’ )
296 %%
297 %d6
299 axis equal
301 x l a b e l ( ’ x [m] ’ )
302 y l a b e l ( ’ y [m] ’ )
37

Hybrid Homework 2 C

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Hybrid Homework 2 C

Uploaded by

Copyright:

Available Formats

Hybrid Dynamics

Josef Nasim 208667139

May 12, 2021

link 0 e02 = −sin(θ) · e1 + cos(θ) · e2

link 1 e02 = −sin(θ + φ1 ) · e1 + cos(θ + φ1 ) · e2

link 2 e02 = −sin(θ + φ2 ) · e1 + cos(θ + φ2 ) · e2

The velocities for each link are:

link 0 v0 = (ẋ, ẏ, 0)T

From here we can get Wb q̇b and Ws q̇s

det(Wb ) = 0.15sin(φ2 + 1.2217) + 0.1sin(φ2 ) + 0.0940 = 0

From here the solutions found are −70.0◦ and 164.0561◦

Figure 1 x(t)[m] versus t[s]

Figure 2 y(t)[m] versus t[s]

Figure 4 x(t)[m] versus y(t)[m]

We have the vc is: vc = [ẋ − dθ̇sin(θ), ẏ + dθ̇cos(θ), 0]

And that ω = [0, 0, θ̇]

T = 0.5Ic θ̇2 + 0.5m((ẏ + dθ̇cos(θ))2 + (ẋ − dθ̇sin(θ))2 )

back axel: eb2 = −sin(θ) · e1 + cos(θ) · e1;

vp = [ẋ, ẏ, 0]T

Let us find each Component of the matrix. Starting with WS and Wb

The equation for the matrix M:

A) Steering angle φ(t) in degrees as a function of time.

B) Body orientation angle θ(t) in degrees as a function of time.

D)Trajectory of the point P in x-y plane.

E)Plots of the two constraint forces λ1 , λ2

G) the two slippage velocities.

B) Body orientation angle θ(t) in degrees as a function of time.

E)Plots of the two constraint forces λ1 , λ2

G) Torque tau as a function of time.

D)Trajectory of the point P in x-y plane.

D)Trajectory of the point P in x-y plane.

15 e11=c o s ( th+p1 ) ∗ e1 + s i n ( th+p1 ) ∗ e2 ;

18 e21=c o s ( th+p2 ) ∗ e1 + s i n ( th+p2 ) ∗ e2 ;

29 w0=v0 ∗ e02 . ’==0;

36 [W zb ]= eq u a ti onsToMatrix ( [ w0 , w1 , w2 ] , [ dx dy dth dp1 dp2 ] )

42 c o n d i t i o n=d e t ( subs (Wb, [ b L p1 ] , [ 0 . 1 5 0 . 1 (70∗ p i /180) ] ) ) ;

3 syms x y th p hi dx dy dth dphi L d m I c b

15 e11=c o s ( th+p h i ) ∗ e1 + s i n ( th+p h i ) ∗ e2 ;

36 syms tau ddx ddy ddth ddphi lambda1 lambda2

38 qdd=[ddx ddy ddth ddphi ] ;

62 qb=[x y th ] ; qbd=[dx dy dth ] ; qbdd=[ddx ddy ddth ] ;

66 Eqns=[Mbb Mbs −Wb. ’ ; Mbs . ’ Mss −Ws. ’ ; Wb Ws [ 0 ; 0 ] [ 0 ; 0 ] ] ∗ . . .

79 s t a t e 1 =( i n v ( [ Mbb Mbs −Wb. ’ ; Mbs . ’ Mss −Ws. ’ ; Wb Ws [ 0 ; 0 ] [ 0 ; 0 ] ] ) ) ∗ . . .

You might also like