digital integration – Problem with NDSolve :: ndnum error when trying to solve a differential equation system


I receive the error as

NDSolve :: ndnum: A non-numeric value encountered for a derivative at t == 0.`

trying to execute the following code:

t0 = 0; t1 = 3;
qp1[t_?NumericQ, {q1_?NumericQ, q2_?NumericQ, q3_?NumericQ, 
   q4_?NumericQ, q5_?NumericQ, q6_?NumericQ, q7_?NumericQ}] : =
Module[{k0 = 1, Jn, pseudoJ, nullJprojector, qpd},
  Jn = J[q1, q2, q3, q4, q5, q6, q7];
pseudoJ = PseudoInverse[Jn];
nullJprojector = (IdentityMatrix[7] - pseudoJ.Jn);
qpd = k0 gradw[q1, q2, q3, q4, q5, q6, q7];
pseudoJ. (Rejoin[
      vd1
KP.eP1[t, {q1, q2, q3, q4, q5, q6, q7}], ωd1
KO.eO1[t, {q1, q2, q3, q4, q5, q6, q7}]]) + nullJprojector.qpd]q01 = {0, π / 3, 0, π / 3, 0, π / 3, 0};

sol1 = NDSolve[Join[{Equal[RE[QF1[Join[{Equal[D[QF1[Joindre[{Égal[RÉ[qf1[Join[{Equal[D[qf1

where i'm trying to solve qf1

I've already checked if qp1[t0, q01] is digital

qp1[t0, q01]

given

{-0.096225 + 6.38378 * 10 ^ -16 (1/14) .0. -
1.48241 * 10 ^ -16 (1/14). (3 / (8 π)), -0.944202 +
1.45017 * 10 ^ -16 (1/14) .0. +
4.44089 * 10 ^ -16 (1/14). (3 / (8 π)), -3.09999 * 10 ^ -16 +
1.11022 * 10 ^ -16 (1/14) .0. + 5.24446 * 10 ^ -17 (1/14). (3 / (8 π))
1.38803 - 3.76983 * 10 ^ -16 (1/14) .0. -
1.9984 * 10 ^ -15 (1/14). (3 / (8 π)),
3.68611 * 10 ^ -16 - 1.11022 * 10 ^ -16 (1/14) .0. -
5.02284 * 10 ^ -17 (1/14). (3 / (8 π)), -1.77717 +
5.13488 * 10 ^ -16 (1/14) .0. -
2.22045 * 10 ^ -16 (1/14). (3 / (8 π)), -0.096225 -
1.57363 * 10 ^ -16 (1/14). (3 / (8 π))}

I do not understand why the derivative at the moment t == 0 is not a numerical value.

I only wrote the code that seems necessary to the question to simplify readability, if necessary, I can post the entire notebook.

NDSolve is used to search the back kinematics of a redundant robotic arm.

Your help is greatly appreciated.

EDIT

Here is the code of the notebook, sorry for the incomplete question

Direct kinematics


Denavit-Hartenberg
List of parameters (m)
In[5]: = d1 = 0.34; d3 = 0.4; d5 = 0.4; d7 = 0.16; of = 0.1;
Denavit-Hartenebrg table
In[6]: = DHTable[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = {{0, - [Pi]/ 2, d1, q1, "R"}, {0, [Pi]/ 2, 0, q2, "R"}, {0, - [Pi]/ 2, d3, q3, "R"}, {0, [Pi]/ 2.0, q4, "R"}, {0, - [Pi]/ 2, d5, q5, "R"}, {0, [Pi]/ 2, 0, q6, "R"}, {0, 0, d7 + from, q7, "R"}}
Outside[6]= {{0, - ([Pi]/2),0.34,q1,R}, {0, [Pi]/ 2.0, q2, R}, {0, - ([Pi]/2),0.4,q3,R}, {0, [Pi]/ 2.0, q4, R}, {0, - ([Pi]/2),0.4,q5,R}, {0, [Pi]/2,0,q6,R},{0,0,0.26,q7,R}}
Homogeneous transformation
T02[q1_, q2_] = DHFKine[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7], 2];
T03[q1_, q2_, q3_] = DHFKine[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7], 3];
T04[q1_, q2_, q3_, q4_] = DHFKine[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7], 4];
T05[q1_, q2_, q3_, q4_, q5_] = DHFKine[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7], 5];
T06[q1_, q2_, q3_, q4_, q5_, q6_] = DHFKine[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7], 6];
T07[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = DHFKine[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7], 7];
Decomposition of position and rotation
In[14]: = p07[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = RigidPosition[T07[T07[T07[T07[q1, q2, q3, q4, q5, q6, q7]];
R07[q1_,q2_,q3_,q4_,q5_,q6_,q7_] = RigidOrientation[T07[q1,q2,q3,q4,q5,q6,q7]];
Geometric Jacobian
In[16]: = J[q1_,q2_,q3_,q4_,q5_,q6_,q7_] = DHJacob0[DhTable[DhTable[DHTable[DHTable[q1, q2, q3, q4, q5, q6, q7]];


Global POE
Twist definition for the seven rotating joints

In[17]: = [Omega]1 = {0,0,1}; p1 = {0,0,0}; [Gamma]1 = RevoluteTwist[P1[P1[p1[p1[Omega]1];
[Omega]2 = {0.0,1}; p2 = {0,0,0}; [Gamma]2 = RevoluteTwist[P2[P2[p2[p2[Omega]2];
[Omega]3 = {0.0,1}; p3 = {0,0,0}; [Gamma]3 = RevoluteTwist[P3[P3[p3[p3[Omega]3];
[Omega]4 = {0.0,1}; p4 = {0,0,0}; [Gamma]4 = RevoluteTwist[P4[P4[p4[p4[Omega]4];
[Omega]5 = {0.0,1}; p5 = {0,0,0}; [Gamma]5 = RevoluteTwist[P5[P5[p5[p5[Omega]5];
[Omega]6 = {0,0,1}; p6 = {0,0,0}; [Gamma]6 = RevoluteTwist[P6[P6[p6[p6[Omega]6];
[Omega]7 = {0.0,1}; p7 = {0,0,0}; [Gamma]7 = RevoluteTwist[P7[P7[p7[p7[Omega]7];

Initial offset
In[24]: = Tse0 = HomogeneTranslZ[d1+d3+d5+d7];
Trasformazione index[T, se] composta in funzione degli angoli di giunto
In[25]: = Tse[q1_, q2_, q3_, q4_, q5_, q6_, q7_]= Simplify[ForwardKinematics[{[ForwardKinematics[{[ForwardKinematics[{[ForwardKinematics[{[Gamma]1, q1}, {[Gamma]2, q2}, {[Gamma]3, q3}, {[Gamma]4, q4}, {[Gamma]5, q5}, {[Gamma]6, q6}, {[Gamma]7, q7}, Tse0]];
Position orientation and decomposition
In[26]: = pse[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = RigidPosition[Tse[Tse[Tse[Tse[q1, q2, q3, q4, q5, q6, q7]];
cSR[q1_,q2_,q3_,q4_,q5_,q6_,q7_] = RigidOrientation[Tse[q1,q2,q3,q4,q5,q6,q7]];
Inversa Index[T, se]^ -1
In[28]: = Tes = simplify[RigidInverse[Tse[RigidInverse[Tse[RigidInverse[Tse[RigidInverse[Tse[q1, q2, q3, q4, q5, q6, q7]]]Outside[28]= {{Cos[q1+q2+q3+q4+q5+q6+q7],Peach[q1+q2+q3+q4+q5+q6+q7], 0,0.}, {- Sin[q1+q2+q3+q4+q5+q6+q7], Cos[q1+q2+q3+q4+q5+q6+q7], 0,0.}, {0,0,1, -1.3}, {0,0,0,1}}
Jacobian with Global POE
In[29]: = gsprimes[q1_,q2_,q3_,q4_,q5_,q6_, q7_]= RPToHomogene[Eye[3], -pse[q1,q2,q3,q4,q5,q6,q7]]js[{[Theta]1 _, [Theta]2 _, [Theta]3 _, [Theta]4 _, [Theta]5 _, [Theta]6 _, [Theta]7 _, [Theta]8 _, [Theta]9_}]= RigidAdjoint[gsprimesr[gsprimesr[gsprimesr[gsprimesr[q1,q2,q3,q4,q5,q6,q7]].Jacobien Spatial[{[{[{[{[Gamma]1, q1}, {[Gamma]2, q2}, {[Gamma]3, q3}, {[Gamma]4, q4}, {[Gamma]5, q5}, {[Gamma]6, q6}, {[Gamma]7, q7}, Tse0];
Outside[29]= {{1,0,0,0.}, {0,1,0,0.}, {0,0,1, -1.3}, {0,0,0,1}}


RObot construction for representation
Initial configuration
In[31]: = q0 = {0, [Pi]/ 30, [Pi]/ 30, [Pi]/ 30};
The bonds are represented by a cylinder and the effector by a cone
In[32]: = r1 = 0.06; r2 = 0.04;
10 = 0.05;
Link 1
In[34]: = link1: = Graphics3D[{Orange,Opacity[0.3], Specularity[White,30],Cylinder[{{0,0,0},{0,0,d1}},r1]}, Boxed-> False];
Joint 2
In[35]: = seal2[q1_, q2_] : = Graphics3D[{Orange,Opacity[0.3], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T02[q1,q2]. {0, -10 / 2,0,1}, T02[q1,q2]. {0, l0 / 2,0,1}}], -1]], r1]}, Boxed-> False]Link 2
In[36]: = link2[q1_, q2_] : = Graphics3D[{Orange,Opacity[0.3], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T02[q1,q2]. {0,0,0,1}, T02[q1,q2]. {0,0, d3 / 2,1}}], -1]], r1]}, Boxed-> False]Link 3
In[37]: = link3[q1_,q2_,q3_]: = Graphics3D[{Blue,Opacity[0.2], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T03[q1,q2,q3]. {0,0,0,1}, T03[q1,q2,q3]. {0, d3 / 2,0,1}}], -1]], r1]}, Boxed-> False]Joint 4
In[38]: = seal4[q1_,q2_,q3_,q4_]: = Graphics3D[{Orange,Opacity[0.3], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T04[q1,q2,q3,q4]. {0, -10 / 2,0,1}, T04[q1,q2,q3,q4]. {0, l0 / 2,0,1}}], -1]], r2]}, Boxed-> False]Link 4
In[39]: = link4[q1_,q2_,q3_,q4_]: = Graphics3D[{Orange,Opacity[0.3], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T04[q1,q2,q3,q4]. {0,0,0,1}, T04[q1,q2,q3,q4]. {0,0, d5 / 2,1}}], -1]], r2]}, Boxed-> False]Link 5
In[40]: = link5[q1_,q2_,q3_,q4_,q5_]: = Graphics3D[{Blue,Opacity[0.2], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T05[q1,q2,q3,q4,q5]. {0,0,0,1}, T05[q1,q2,q3,q4,q5]. {0, d5 / 2,0,1}}], -1]], r2]}, Boxed-> False]Joint 6
In[41]: = attached6[q1_,q2_,q3_,q4_,q5_,q6_]: = Graphics3D[{Orange,Opacity[0.3], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T06[q1,q2,q3,q4,q5,q6]. {0, -10 / 2,0,1}, T06[q1,q2,q3,q4,q5,q6]. {0, l0 / 2,0,1}}], -1]], r2]}, Boxed-> False]Link 6
In[42]: = link6[q1_,q2_,q3_,q4_,q5_,q6_]: = Graphics3D[{Orange,Opacity[0.2], Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T06[q1,q2,q3,q4,q5,q6]. {0,0,0,1}, T06[q1,q2,q3,q4,q5,q6]. {0,0, d7,1}}], -1]], r2]}, Boxed-> False]Link 7 (final effector)
In[43]: = link7[q1_,q2_,q3_,q4_,q5_,q6_,q7_]: = Graphics3D[{Green,Opacity[0.3], Specularity[White,30],Cone[Transpose[Drop[Transpose[{T07[Transpose[Drop[Transpose[{T07[Transposer[Laisseztomber[Transposer[{T07[Transpose[Drop[Transpose[{T07[q1,q2,q3,q4,q5,q6,q7]. {0,0, -de, 1}, T07[q1,q2,q3,q4,q5,q6,q7]. {0,0,0,1}}], -1]], r2]}, Boxed-> False]Composition of the robot in the configuration of the partnership
In[44]: = robotref = {link1, joint2 @@ q0[[{1,2}]], link2 @@ q0[[{1,2}]], link3 @@ q0[[{1,2,3}]], seal4 @@ q0[[{1,2,3,4}]], link4 @@ q0[[{1,2,3,4}]], link5 @@ q0[[{1,2,3,4,5}]], join6 @@ q0[[{1,2,3,4,5,6}]], link6 @@ q0[[{1,2,3,4,5,6}]], link7 @@ q0};
Show[robotref,Boxed-> False,PlotRange-> {{-1,1},{-1,1},{-0.5,1}},ImageSize-> {400,400}] 
Outside[45]=
Verification of the quality of the information for the configuration of the partnership
In[46]: = N[Det[J[q0[[1]], q0[[2]], q0[[3]], q0[[4]], q0[[[[[5]], q0[[[[[6]], q0[[[[[7]]].Transpose[J[q0[[1]], q0[[2]], q0[[3]], q0[[4]], q0[[[[[5]], q0[[[[[6]], q0[[[[[7]]]]]]Outside[46]= 0.01728


Path generation

Definition of start and end time and position
In[47]: = q0 = {0, [Pi]/ 30, [Pi]/ 30, [Pi]/ 30};
t0 = 0; t1 = 3; t2 = 5; t3 = 7;
p0 = Drop[T07@@q0[[{1,2,3,4,5,6,7}]]. {0,0,0,1}, -1];
a1 = {0.9, -0.2, 0.6};
a2 = {0.9, 0.0, 0.6};
a3 = {0.9, 0.2, 0.6};
First trajectory (p0 to a1)
Desired position of the effector
In[53]: = Pd1[t_]= p0 + (a1-p0) * ((t-t0) / (t1-t0))
Outside[53]= {0,69282 +0,0690599 t, 0. -0,0666667 t, 0,08 +0,173333 t}
Orientation interpolation by SLERP and unit quaternion
In[54]: = quat0 = MatToQuat[R07@@q0[[{1,2,3,4,5,6,7}]]];
quat1 = MatToQuat[RigidOrientation[HomogeneRotY[RigidOrientation[HomogeneousRotY[RigidOrientation[HomogeneRotY[RigidOrientation[HomogeneousRotY[Pi]]];
Quat1[t_]= ((t1-t) / (t1-t0)) * quat0 + t * quat1
q0
Outside[56]= {0. + 1.85037 * 10 ^ -17 (3-t), 0.333333 (3-t) +1. t, 0.}
Outside[57]= {0, [Pi]/ 3.0, [Pi]/ 3.0, [Pi]/ 3.0}
Desired angular velocity
In[58]: = [Omega]d1[t_] = Drop[2*QuatInv[Quat1[2*QuatInv[Quat1[2*QuatInv[Quat1[2*QuatInv[Quat1
Desired linear speed
In[59]: = vd1[t_]= D[Pd1[Pd1[Pd1[Pd1
Position and orientation error (orientation error defined with the vector part of the quaternion)
In[60]: = eP1[t_,{q1_,q2_,q3_,q4_,q5_,q6_,q7_}] = Pd1
EO1[t_,{q1_,q2_,q3_,q4_,q5_,q6_,q7_}] : = QuatVectPart[QuatToMat[Quat1[QuatToMat[Quat1[QuatToMat[Quat1[QuatToMat[Quat1
In[62]: = KP = 20IdentityMatrix[3];
In[63]: = KO = 20IdentityMatrix[3];
Common range ([PlusMinus] he values ​​indicato)
In[64]: = jointrange = AssociationThread[{Jr1jr2jr3jr4jr5jr6jr7}->{95[{Jr1jr2jr3jr4jr5jr6jr7}->{95[{jr1jr2jr3jr4jr5jr6jr7}->{95[{jr1jr2jr3jr4jr5jr6jr7}->{95[Pi], 2 [Pi]/ 3, .95 [Pi], 2 [Pi]/ 3, .95 [Pi], 2 [Pi]/ 3, .97 [Pi]}];
The robotic arm has a redundant number of joints, so you can reverse the kinematics by also trying to maximize a given target function, chosen here to keep the articulation angle as close to the center as possible. their reach.
Target function:
w[q1_,q2_,q3_,q4_,q5_,q6_,q7_]= - (1 / (2 * 7)). (Q1 ^ 2 / (2 * jr1) ^ 2 + q2 ^ 2 / (2 * jr2) ^ 2 + q3 ^ 2 / (2 * jr3) ^ 2 + q4 ^ 2 / (2 * jr4) ^ 2 + q5 ## EQU1 ## ;
Outside[80]= - (1/14). (0.0280668 q1 ^ 2 + (9 q2 ^ 2) / (16 [Pi]^ 2) +0.0280668 q3 ^ 2 + (9 q4 ^ 2) / (16 [Pi]^ 2) +0.0280668 q5 ^ 2 + (9 q6 ^ 2) / (16 [Pi]^ 2) +0,0269213 q7 ^ 2) +
Gradient of the target function
In[66]: = gradw[q1_,q2_,q3_,q4_,q5_,q6_,q7_] = (D[w[w[w[w[q1,q2,q3,q4,q5,q6,q7], {{q1, q2, q3, q4, q5, q6, q7}}]) /. jointrange
Outside[66]= {- (1/14). (0.0561336 q1), - (1/14). ((9 q2) / (8 [Pi]^ 2)), - (1/14). (0.0561336 q3), - (1/14). ((9 q4) / (8 [Pi]^ 2)), - (1/14). (0.0561336 q5), - (1/14). ((9 q6) / (8 [Pi]^ 2)), - (1/14). (0,0538427 q7)}
In[83]: = qp1[t_?NumericQ,{q1_?NumericQ,q2_?NumericQ,q3_?NumericQ,q4_?NumericQ,q5_?NumericQ,q6_?NumericQ,q7_?NumericQ}]: =
Module[{k0 = 1, Jn, pseudoJ, nullJprojector, qpd},
Jn = J[q1,q2,q3,q4,q5,q6,q7];
pseudoJ = PseudoInverse[Jn];
nullJprojector = (IdentityMatrix[7]-pseudoJ.Jn);
qpd = k0 gradw[q1,q2,q3,q4,q5,q6,q7];
pseudoJ. (Rejoin[vd1[vd1[vd1[vd1
Condition of departure
In[84]: = q01 = {0, [Pi]/ 3.0, [Pi]/ 3.0, [Pi]/ 3.0};
qp1[t0,q01]
Outside[85]= {-0.096225 + 6.38378 * 10 ^ -16 (1/14) .0.-1.48241 * 10 ^ -16 (1/14). (3 / (8 [Pi])), - 0.944202 + 1.45017 * 10 ^ -16 (1/14) .0. + 4.44089 * 10 ^ -16 (1/14). (3 / (8 [Pi])), - 3.09999 * 10 ^ -16 + 1.11022 * 10 ^ -16 (1/14) .0. + 5.24446 * 10 ^ -17 (1/14). (3 / (8 [Pi])), 1.38803-3.76983 * 10 ^ -16 (1/14) .0. -1.9984 * 10 ^ -15 (1/14). (3 / (8 [Pi])), 3.68611 * 10 ^ -16-1.11022 * 10 ^ -16 (1/14) .0.-5.02284 * 10 ^ -17 (1/14). (3 / (8 [Pi])), - 1.77717 + 5.13488 * 10 ^ -16 (1/14) .0.-2,22045 * 10 ^ -16 (1/14). (3 / (8 [Pi])), 0.096225-1.57363 * 10 ^ -16 (1/14). (3 / (8 [Pi]))}
Digital integration
In[77]: = sol1 = NDSolve[Join[{Equal[RE[QF1[Join[{Equal[D[QF1[Joindre[{Égal[RÉ[qf1[Join[{Equal[D[qf1
Outside[77]= NDSolve[{(qf1^[Prime])
During the evaluation of In[70]: = ReplaceAll :: reps: {(qf1 ^ [Prime])
During the evaluation of In[70]: = Set :: write: NDSolve in NDSolve[{(qf1^[Prime])
In[71]: = Plot[{Qsol1[{Qsol1[{qsol1[{qsol1
During the evaluation of In[71]: = Part :: partw: Part 2 of qsol1[0.0612858] does not exist.
During the evaluation of In[71]: = Part :: partw: Part 2 of qsol1[0.12251] does not exist.
During the evaluation of In[71]: = General :: stop: The output of Part :: partw will be removed during this calculation.
Outside[71]= Index[q, 1] (Rad)
Index[q, 2] (Rad)
Index[q, 3] (Rad)
Index[q, 4] (Rad)
Index[q, 5] (Rad)
Index[q, 6] (Rad)
Index[q, 7] (Rad)