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 away 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 agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.