search

mechmotum / bicycle-kickplate-model

Nonlinear and linear bicycle model that includes realistic tires and a kickplate perturbation input.
BSD 3-Clause "New" or "Revised" License
2 stars 2 forks source link

Setting q7 (steer angle) to zero causes a divide-by-zero in the velocity constraints #8

Open moorepants opened 1 year ago

moorepants commented 1 year ago

The time derivative of the holonomic constraint gives you an equation that looks like:

u4*f1(q4, q5, q7) + u5*f2(q4, q5, q7) + u7*f3(q4, q5, q7) = 0

If u4 and u7 are taken as independent speeds then you subtract and divide by f2.

More specifically, if q4,q7=0 then f1 and f3 become zero.

In [19]: sm.trigsimp(nonholonomic[2].xreplace({q4:0, q7:0})).evalf()
Out[19]: -(d1*cos(q5(t)) + d2*sin(q5(t)) + d3*cos(q5(t)))*u5(t)

The other two no-slop constraints

u1, u2, u5, u6 u1, u2, u3, u4, u5, u8

You can see the divide by the sin(q7) here:

In [30]: sm.trigsimp(A_nh.LUsolve(B_nh).xreplace({q3: 0, q5: 0, q4: 0}))
Out[30]: 
Matrix([
[                                                                                                                                             d3*rr*u4(t)*sin(q7(t))/(d1 + d3*cos(q7(t))) + rr*u6(t)],
[(d1*u3(t)*sin(q7(t)) - d2*u4(t)*sin(q7(t)) + d3*(d2 + rf - rr)*u4(t)*sin(2*q7(t))/(2*(d1 + d3*cos(q7(t)))) + rf*(-u4(t)*sin(q7(t)) + u8(t)) + rr*u4(t)*sin(q7(t)) - rr*u6(t)*cos(q7(t)))/sin(q7(t))],
[                                                                                                                                                          -d3*u4(t)*sin(q7(t))/(d1 + d3*cos(q7(t)))]])

Or more clearly:

In [31]: sm.trigsimp(A_nh.LUsolve(B_nh).xreplace({u3: 0, u4: 0, q3: 0, q5: 0, q4: 0}))
Out[31]: 
Matrix([
[                                   rr*u6(t)],
[(rf*u8(t) - rr*u6(t)*cos(q7(t)))/sin(q7(t))],
[                                          0]])
moorepants commented 1 month ago

It is logical that if you set all q's to zero and the configuration is the nominal configuration that $u_2$ should be zero. There would be no lateral velocity at either tire contact. I wonder if the limit of nonholonomic constraint as q_7 -> 0 gives zero for u_2.