0

When I'm trying to calculate the value of DP, it subtracts the PCurrent matrix from each element of PGoal. What am I doing wrong?

OP4 = MatrixForm[{Cos[
      T1] (L1 + L2 Cos[T2] + L3 Cos[T2 + T3]), (L1 + L2 Cos[T2] + 
       L3 Cos[T2 + T3]) Sin[T1], L2 Sin[T2] + L3 Sin[T2 + T3]}];
OP4 = OP4 /. {L1 -> 0.3, L2 -> 0.3, L3 -> 0.15}
Print["OP4=", MatrixForm[OP4]];
OJTrans = 
  MatrixForm[{{(-(L1 + L2 Cos[T2] + L3 Cos[T2 + T3])) Sin[T1], 
     Cos[T1] ((-L2) Sin[T2] - L3 Sin[T2 + T3]), ((-L3) Cos[T1]) Sin[
       T2 + T3]}, {Cos[T1] (L1 + L2 Cos[T2] + L3 Cos[T2 + T3]), 
     Sin[T1] ((-L2) Sin[T2] - L3 Sin[T2 + T3]), ((-L3) Sin[T1]) Sin[
       T2 + T3]}, {0, L2 Cos[T2] + L3 Cos[T2 + T3], L3 Cos[T2 + T3]}}];
OJTrans = OJTrans /. {L1 -> 0.3, L2 -> 0.3, L3 -> 0.15}
ThetaInitial = {{0}, {Pi/4}, {Pi/2}};
PGoal = {{0.35}, {0.05}, {0.35}};
Print["PGO=", MatrixForm[PGoal]];
ThetaEstimate = ThetaInitial;
For[i = 1, i <= 5, i++,
  Print["IterationNumber", i];
  PCurrent = 
   OP4 /. {T1 -> ThetaEstimate[[1, 1]], T2 -> ThetaEstimate[[2, 1]], 
     T3 -> ThetaEstimate[[3, 1]]};
  Print["PCurrent=", MatrixForm[PCurrent]];
  DP = PGoal - PCurrent;
  Print["DP=", MatrixForm[DP]];
  ];

This is what i'm getting

SequenceForm["DP=", 
MatrixForm[{{
   0.35 - MatrixForm[{0.4060660171779821, 0., 0.3181980515339463}]}, {
   0.05 - MatrixForm[{0.4060660171779821, 0., 0.3181980515339463}]}, {
   0.35 - MatrixForm[{0.4060660171779821, 0., 0.3181980515339463}]}}]]
pinkpanta
  • 23
  • 2

1 Answers1

0

Do not include wrappers (e.g., MatrixForm) within definitions. Use MatrixForm[mat = {...}]rather than mat = MatrixForm[{ ... }]

Clear["Global`*"]

OP4 = {Cos[
     T1] (L1 + L2 Cos[T2] + L3 Cos[T2 + T3]), (L1 + L2 Cos[T2] + 
      L3 Cos[T2 + T3]) Sin[T1], L2 Sin[T2] + L3 Sin[T2 + T3]};
OP4 = OP4 /. {L1 -> 0.3, L2 -> 0.3, L3 -> 0.15};
Print["OP4=", MatrixForm[OP4]];

enter image description here

OJTrans = {{(-(L1 + L2 Cos[T2] + L3 Cos[T2 + T3])) Sin[T1], 
    Cos[T1] ((-L2) Sin[T2] - L3 Sin[T2 + T3]), ((-L3) Cos[T1]) Sin[
      T2 + T3]}, {Cos[T1] (L1 + L2 Cos[T2] + L3 Cos[T2 + T3]), 
    Sin[T1] ((-L2) Sin[T2] - L3 Sin[T2 + T3]), ((-L3) Sin[T1]) Sin[
      T2 + T3]}, {0, L2 Cos[T2] + L3 Cos[T2 + T3], L3 Cos[T2 + T3]}};
OJTrans = OJTrans /. {L1 -> 0.3, L2 -> 0.3, L3 -> 0.15};
ThetaInitial = {{0}, {Pi/4}, {Pi/2}};
PGoal = {{0.35}, {0.05}, {0.35}};
Print["PGO=", MatrixForm[PGoal]];

enter image description here

ThetaEstimate = ThetaInitial;
For[i = 1, i <= 5, i++, Print["IterationNumber", i];
  PCurrent = 
   OP4 /. {T1 -> ThetaEstimate[[1, 1]], T2 -> ThetaEstimate[[2, 1]], 
     T3 -> ThetaEstimate[[3, 1]]};
  Print["PCurrent=", MatrixForm[PCurrent]];
  DP = PGoal - PCurrent;
  Print["DP=", MatrixForm[DP]];];

enter image description here

Bob Hanlon
  • 157,611
  • 7
  • 77
  • 198