0
$\begingroup$

I get the error reported as

NDSolve::ndnum: Encountered non-numerical value for a derivative at t == 0.`

when trying to run 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.(Join[
      vd1[t] + 
       KP.eP1[t, {q1, q2, q3, q4, q5, q6, q7}], ωd1[t] + 
       KO.eO1[t, {q1, q2, q3, q4, q5, q6, q7}]]) + nullJprojector.qpd]
q01 = {0, π/3, 0, π/3, 0, π/3, 0};

sol1 = NDSolve[Join[{Equal[D[qf1[t], t], qp1[t, qf1[t]]], Equal[qf1[t0], q01]}], qf1, {t, t0, t1}]

where I'm trying to solve for qf1[t].

I already checked if qp1[t0, q01] is numerical

qp1[t0, q01]

gives

{-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 π))}

so I can't understand why the derivative at time t==0 is not a numerical value.

I have written only the code that seems necessary for the question to simplify readability, if necessary I can post the whole notebook.

The NDSolve is used for finding the backward kinematic of a redundant robotic arm.

Your help is greatly appreciated.

EDIT

Here's the code of the notebook, sorry for the incomplete question

Direct Kinematic


Denavit-Hartenberg
Parametri numerici (m)
In[5]:= d1 = 0.34; d3 = 0.4; d5 = 0.4; d7=0.16; de=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+de, q7, "R"}}
Out[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}}
Homogenous transform
T02[q1_, q2_] = DHFKine[DHTable[q1, q2, q3, q4, q5, q6, q7], 2];
T03[q1_, q2_, q3_] = DHFKine[DHTable[q1, q2, q3, q4, q5, q6, q7], 3];
T04[q1_, q2_, q3_, q4_] = DHFKine[DHTable[q1, q2, q3, q4, q5, q6, q7], 4];
T05[q1_, q2_, q3_, q4_, q5_] = DHFKine[DHTable[q1, q2, q3, q4, q5, q6, q7], 5];
T06[q1_, q2_, q3_, q4_, q5_, q6_] = DHFKine[DHTable[q1, q2, q3, q4, q5, q6, q7], 6];
T07[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = DHFKine[DHTable[q1, q2, q3, q4, q5, q6, q7], 7];
Position and rotation decomposition
In[14]:= p07[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = RigidPosition[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[q1, q2, q3, q4, q5, q6, q7]];


Global POE
Twist definition for the seven rotational joints

In[17]:= \[Omega]1={0,0,1}; p1={0,0,0}; \[Gamma]1=RevoluteTwist[p1,\[Omega]1];
\[Omega]2={0,0,1}; p2={0,0,0}; \[Gamma]2=RevoluteTwist[p2,\[Omega]2];
\[Omega]3={0,0,1}; p3={0,0,0}; \[Gamma]3=RevoluteTwist[p3,\[Omega]3];
\[Omega]4={0,0,1}; p4={0,0,0}; \[Gamma]4=RevoluteTwist[p4,\[Omega]4];
\[Omega]5={0,0,1}; p5={0,0,0}; \[Gamma]5=RevoluteTwist[p5,\[Omega]5];
\[Omega]6={0,0,1}; p6={0,0,0}; \[Gamma]6=RevoluteTwist[p6,\[Omega]6];
\[Omega]7={0,0,1}; p7={0,0,0}; \[Gamma]7=RevoluteTwist[p7,\[Omega]7];

Initial off-set
In[24]:= Tse0 = HomogeneousTranslZ[d1+d3+d5+d7];
Trasformazione Subscript[T, se] composta in funzione degli angoli di giunto
In[25]:= Tse[q1_, q2_, q3_, q4_, q5_, q6_, q7_]= Simplify[ForwardKinematics[{\[Gamma]1, q1}, {\[Gamma]2, q2},{\[Gamma]3, q3},{\[Gamma]4, q4},{\[Gamma]5, q5},{\[Gamma]6, q6},{\[Gamma]7, q7}, Tse0]];
Orientation and position decomposition
In[26]:= pse[q1_, q2_, q3_, q4_, q5_, q6_, q7_] = RigidPosition[Tse[q1, q2, q3, q4, q5, q6, q7]];
Rse[q1_,q2_,q3_,q4_,q5_,q6_,q7_] = RigidOrientation[Tse[q1,q2,q3,q4,q5,q6,q7]] ; 
Inversa Subscript[T, se]^-1
In[28]:= Tes = Simplify[RigidInverse[Tse[q1, q2, q3, q4, q5, q6, q7]]]
Out[28]= {{Cos[q1+q2+q3+q4+q5+q6+q7],Sin[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_]=RPToHomogeneous[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[q1,q2,q3,q4,q5,q6,q7]].SpatialJacobian[{\[Gamma]1, q1}, {\[Gamma]2, q2},{\[Gamma]3, q3},{\[Gamma]4, q4},{\[Gamma]5, q5},{\[Gamma]6, q6},{\[Gamma]7, q7}, Tse0];
Out[29]= {{1,0,0,0.},{0,1,0,0.},{0,0,1,-1.3},{0,0,0,1}}


RObot construction for representation
Starting configuration
In[31]:= q0 ={0,\[Pi]/3, 0, \[Pi]/3, 0, \[Pi]/3, 0};
Link are represented as cylinder and end-effector as a cone
In[32]:= r1 = 0.06; r2=0.04;
l0 =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]:= joint2[q1_, q2_] := Graphics3D[{Orange,Opacity[0.3],Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T02[q1,q2].{0,-l0/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]:= joint4[q1_,q2_,q3_,q4_]:= Graphics3D[{Orange,Opacity[0.3],Specularity[White,30],Cylinder[Transpose[Drop[Transpose[{T04[q1,q2,q3,q4].{0,-l0/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]:= joint6[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,-l0/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 (end-effector)
In[43]:= link7[q1_,q2_,q3_,q4_,q5_,q6_,q7_]:= Graphics3D[{Green,Opacity[0.3],Specularity[White,30],Cone[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] 
Composizione del robot in configurazione di partenza
In[44]:= robotref = {link1,joint2@@q0[[{1,2}]],link2@@q0[[{1,2}]],link3@@q0[[{1,2,3}]],joint4@@q0[[{1,2,3,4}]],link4@@q0[[{1,2,3,4}]],link5@@q0[[{1,2,3,4,5}]],joint6@@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}] 
Out[45]= 
Verifica di assenza di singolarità nella configurazione di partenza
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]]]]]] 
Out[46]= 0.01728


Trajectory generation

Definition of starting and ending time and position
In[47]:= q0 = {0,\[Pi]/3, 0, \[Pi]/3, 0, \[Pi]/3, 0};
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 end-effector position
In[53]:= Pd1[t_]=p0+(a1-p0)*((t-t0)/(t1-t0))
Out[53]= {0.69282 +0.0690599 t,0. -0.0666667 t,0.08 +0.173333 t}
Orientation interpolation through SLERP and unitary quaternion
In[54]:= quat0=MatToQuat[R07@@q0[[{1,2,3,4,5,6,7}]]];
quat1= MatToQuat[RigidOrientation[HomogeneousRotY[Pi]]];
Quat1[t_]=((t1-t)/(t1-t0))*quat0 +t*quat1
q0
Out[56]= {0. +1.85037*10^-17 (3-t),0.,0.333333 (3-t)+1. t,0.}
Out[57]= {0,\[Pi]/3,0,\[Pi]/3,0,\[Pi]/3,0}
Desired angular velocity
In[58]:= \[Omega]d1[t_] = Drop[2*QuatInv[Quat1[t]]*D[Quat1[t],t],1];
DEsired linear velocity
In[59]:= vd1[t_]=D[Pd1[t],t];
Position and orientation error (orientation error defined with vector part of quaternion)
In[60]:= eP1[t_,{q1_,q2_,q3_,q4_,q5_,q6_,q7_}] = Pd1[t]-p07[q1,q2,q3,q4,q5,q6,q7];
eO1[t_,{q1_,q2_,q3_,q4_,q5_,q6_,q7_}] := QuatVectPart[QuatToMat[Quat1[t]].Transpose[R07[q1,q2,q3,q4,q5,q6,q7]]]
Gain matrix
In[62]:= KP = 20IdentityMatrix[3];
In[63]:= KO = 20IdentityMatrix[3];
Joint range (\[PlusMinus] il valore indicato)
In[64]:= jointrange =AssociationThread[{jr1,jr2,jr3,jr4,jr5,jr6,jr7}->{.95\[Pi], 2\[Pi]/3, .95\[Pi], 2\[Pi]/3, .95\[Pi], 2\[Pi]/3, .97\[Pi]}];
Robotic arm has a redundant number of joints, so kinematic can be inverted trying also to maximize a given target function, here chosen to keep the joint angle as close as possible to the center of their range
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^2/(2*jr5)^2+q6^2/(2*jr6)^2+q7^2/(2*jr7)^2)/.jointrange;
Out[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 target function
In[66]:= gradw[q1_,q2_,q3_,q4_,q5_,q6_,q7_] = (D[w[q1,q2,q3,q4,q5,q6,q7],{{q1,q2,q3,q4,q5,q6,q7}}])/.jointrange
Out[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.(Join[vd1[t]+KP.eP1[t,{q1,q2,q3,q4,q5,q6,q7}],\[Omega]d1[t]+KO.eO1[t,{q1,q2,q3,q4,q5,q6,q7}]])+nullJprojector.qpd]

Integration of kinematic
Starting condition
In[84]:= q01 = {0,\[Pi]/3,0,\[Pi]/3,0,\[Pi]/3,0};
qp1[t0,q01]
Out[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]))}
Numerical integration
In[77]:= sol1=NDSolve[Join[{Equal[D[qf1[t],t],qp1[t,qf1[t]]],Equal[qf1[t0],q01]}],qf1,{t,t0,t1}]
During evaluation of In[77]:= NDSolve::ndnum: Encountered non-numerical value for a derivative at t == 0.`.
Out[77]= NDSolve[{(qf1^\[Prime])[t]==qp1[t,qf1[t]],qf1[0]=={0,\[Pi]/3,0,\[Pi]/3,0,\[Pi]/3,0}},qf1,{t,0,3}]
In[70]:= sol1[t_]=Evaluate[qf1[t]/.sol1[[1]]];
During evaluation of In[70]:= ReplaceAll::reps: {(qf1^\[Prime])[t]==qp1[t,qf1[t]],qf1[0]=={0,\[Pi]/3,0,\[Pi]/3,0,\[Pi]/3,0}} is neither a list of replacement rules nor a valid dispatch table, and so cannot be used for replacing.
During evaluation of In[70]:= Set::write: Tag NDSolve in NDSolve[{(qf1^\[Prime])[t]==qp1[t,qf1[t]],qf1[0]=={0,\[Pi]/3,0,\[Pi]/3,0,\[Pi]/3,0}},qf1,{t,0,3}][t_] is Protected.
In[71]:= Plot[{qsol1[t][[1]],qsol1[t][[2]],qsol1[t][[3]],qsol1[t][[4]],qsol1[t][[5]],qsol1[t][[6]],qsol1[t][[7]]},{t,t0,t1},PlotLegends->{"Subscript[q, 1] (rad)","Subscript[q, 2] (rad)","Subscript[q, 3] (rad)","Subscript[q, 4] (rad)","Subscript[q, 5] (rad)","Subscript[q, 6] (rad)","Subscript[q, 7] (rad)"},AxesLabel->{"t (s)"}]
During evaluation of In[71]:= Part::partw: Part 2 of qsol1[0.0000612857] does not exist.
During evaluation of In[71]:= Part::partw: Part 2 of qsol1[0.0612858] does not exist.
During evaluation of In[71]:= Part::partw: Part 2 of qsol1[0.12251] does not exist.
During evaluation of In[71]:= General::stop: Further output of Part::partw will be suppressed during this calculation.
Out[71]=    Subscript[q, 1] (rad)
    Subscript[q, 2] (rad)
    Subscript[q, 3] (rad)
    Subscript[q, 4] (rad)
    Subscript[q, 5] (rad)
    Subscript[q, 6] (rad)
    Subscript[q, 7] (rad)
$\endgroup$
  • 1
    $\begingroup$ Without definitions for e.g. J[] or eP1[], other people can't evaluate your code, and thus won't be able to help you. Please make sure everything that is needed to evaluate your code is all accounted for. $\endgroup$ – J. M. is computer-less Jan 6 at 11:38
  • $\begingroup$ Function not defined DHFKine[] $\endgroup$ – Alex Trounev Jan 6 at 14:00

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Browse other questions tagged or ask your own question.