Hi Mario,

Did you use something like:

ExportMotionPlan[motionPlan,legoffsets_: -{0.17035,0.17035,0.17035,0.17035,0.17035,0.17035}]?

The legoffsets_:{...} idiom is only used in the definition of the function. When you call the function, use:

ExportMotionPlan[ motionPlan, -{0.17035,0.17035,0.17035,0.17035,0.17035,0.17035} ]

Note that using an offset of 1.1313669977935081 is not realistic because the platform height is only 0.15 units. You will get leg lengths, but they won't be very meaningful. The purpose of subtracting the offsets is to get control values for the servo actuators.

Eric

Hi Mario,

You can try this:

ExportMotionPlan[poses_List, legoffsets_: {0, 0, 0, 0, 0, 0}] :=
 Module[{framerate = 10, lengths, tabledata, time1, time2},
  lengths = {poses[[1, 2 ;; 7]]};
  time1 := poses[[1, 1]];

  Do[
   (
    time2 := wp[[1]];
    lengths = 
     Join[lengths, 
      Rest@PoseRange[{Last[lengths], wp[[2 ;; 7]]}, framerate, time1, 
        time2]];
    time1 = time2;
    ),
   {wp, Rest[poses]}];

  AppendTo[lengths, Last[lengths]];
  lengths = Map[PoseLegLengths, lengths];
  tabledata = 
   MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];
  {Export[
    FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath1.txt"}], {{"LegLengths", tabledata}}, 
    "ModelicaCombiTimeTable"], First[Last[tabledata]]};

  lengths = Map[# + legoffsets &, lengths];
  tabledata = 
   MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];
  {Export[
    FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath2.txt"}], {{"LegLengths", tabledata}}, 
    "ModelicaCombiTimeTable"], First[Last[tabledata]]}
  ]

I haven't tested this code, of course, because I don't have SystemModeler.

A good value to test the offsets would be -0.17035, the zero-position leg length.

Eric

Hi Mario,

The purpose legoffsets is not documented. I'm not sure that adding a fixed length to the legs would produce the same motion in the extended actuators that the original actuators produced. For example, suppose each leg is extended 0.17035 units. The height of the platform is 0.15 units and the rest length of the legs (i.e., at neutral position) is 0.17035 units. This would presumably raise the working height of the platform to about twice its original height. But as the lengths of the actuators change, the positions of the extensions (assuming the extensions go from the platform's base to bottoms of the actuators) will change, so the previously fixed base points of the actuators will now be moving.

If actuator extensions are required they should be incorporated before the calculations are made.

For instance, suppose the platformHeight is changed to 0.3 from its original 0,15, the rest lengths are 0.17035 and 0.310675. The difference is 0.140325. If this is added to the 0.17035 lengths by using the legoffsets, the resulting motion is different.

Could someone offer and idea of what the purpose legoffsets might be?

Eric

Hi mario,

It's its own plan. The long and short plans are just longer or shorter. Their structure is the same.

But the motionPlan I implemented doesn't use the Block[]. That was necessary for the substitutions of dx and dtheta.

Inside the Blocks[] you will see the basic structure; a List[] of List[]s.

Eric

Hi Mario,

In this utility function for generating a sequence of steps between two poses:

PoseRange[{pose1_, pose2_}, steps_] := 
 Table[pose1 + t (pose2 - pose1), {t, Range[0, 1, 1/steps]}]

there is a hidden time value. It is the 1 in Range[0, 1, 1/steps]

This generates a list of numbers between 0 and 1 divided into the number of frames per second.

So, for 10 frames per second, it generates

In[89]:= Range[0,1,1/10]
Out[89]= {0,1/10,1/5,3/10,2/5,1/2,3/5,7/10,4/5,9/10,1}

Now, you want to generate poses at arbitrary times, which must be chopped into steps. Suppose you want to go from 2.21 to 3.33 seconds at 10 frames per second. This would generate the series:

In[87]:=  Range[2.21,3.33,1/10]
Out[87]= {2.21,2.31,2.41,2.51,2.61,2.71,2.81,2.91,3.01,3.11,3.21,3.31}

This doesn't get right up to the 3.33 seconds time, because that falls in between the time frame periods. Depending on how important this is to you, it might have to be dealt with or disregarded. Let's disregard it for now and only use time divisions that are perfect multiples of the framerate periods. (By the way, if you don't mind, why are you looking at the Stewart Platform?)

Now that you are adding time to the motion plan, we will need a time element in the motion plan:

{
 {0,    0, 0.000, 0, 0, 0, 0},
 {1,    0, 0.010, 0, 0, 0, 0},
 {4,    0, 0.040, 0, 0, 0, 0},
 {4.5,  0, 0.045, 0, 0, 0, 0}
 }

where the first number is the time.

So the PoseRange[] function now has to include the start and end times for the poses:

PoseRange[{pose1_, pose2_}, steps_, time1_, time2_] := 
 Table[pose1 + t/(time2 - time1) (pose2 - pose1), {t, Range[0, time2 - time1, 1/steps]}]

This will generate however many poses are required between time1 and time2 every 1/framerate seconds.

And now, to generate the poses, the ExportMotionPlan[] has to be modified to accommodate the new motion plan's extra time element:

ExportMotionPlan[poses_List, legoffsets_: {0, 0, 0, 0, 0, 0}] :=
 Module[{framerate = 10, lengths, tabledata, time1, time2},
  lengths = {poses[[1, 2 ;; 7]]};
  time1 := poses[[1, 1]];

  Do[
   (
    time2 := wp[[1]];
    lengths = 
     Join[lengths, 
      Rest@PoseRange[{Last[lengths], wp[[2 ;; 7]]}, framerate, time1, time2]];
    time1 = time2;
    ),
   {wp, Rest[poses]}];

  AppendTo[lengths, Last[lengths]];
  lengths = Map[PoseLegLengths, lengths];
  lengths = Map[# + legoffsets &, lengths];
  tabledata = 
   MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths](*;
  {Export[
    FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath.txt"}], {{"LegLengths", tabledata}}, 
    "ModelicaCombiTimeTable"], First[Last[tabledata]]}*)
  ]

Note that the last line is commented out so that you see results directly in the notebook.

So, with a motionPlan such as:

motionPlan = {
   {0, 0, 0.00, 0, 0, 0, 0},
   {1, 0, 0.01, 0, 0, 0, 0},
   {3, 0, -0.03, 0, 0, 0, 0},
   {4, 0, 0.04, 0, 0, 0, 0},
   {4.5, 0, -0.045, 0, 0, 0, 0}
   };

ExportMotionPlan[motionPlan] // MatrixForm

(
0     0.17035 0.17035 0.17035 0.17035 0.17035 0.17035
1/10    0.171232   0.171232  0.171232 0.171232    0.171232   0.171232
1/5 0.172114    0.172114   0.172114  0.172114 0.172114    0.172114
3/10    0.172998   0.172998  0.172998 0.172998    0.172998   0.172998
2/5 0.173883    0.173883   0.173883  0.173883 0.173883    0.173883
1/2 0.174769    0.174769   0.174769  0.174769 0.174769    0.174769
3/5 0.175657    0.175657   0.175657  0.175657 0.175657    0.175657
7/10    0.176545   0.176545  0.176545 0.176545    0.176545   0.176545
4/5 0.177435    0.177435   0.177435  0.177435 0.177435    0.177435
9/10    0.178326   0.178326  0.178326 0.178326    0.178326   0.178326
1   0.179218  0.179218 0.179218    0.179218   0.179218  0.179218
11/10   0.177435  0.177435 0.177435    0.177435   0.177435  0.177435
6/5 0.175657    0.175657   0.175657  0.175657 0.175657    0.175657
13/10   0.173883  0.173883 0.173883    0.173883   0.173883  0.173883
7/5 0.172114    0.172114   0.172114  0.172114 0.172114    0.172114
3/2 0.17035 0.17035 0.17035 0.17035 0.17035 0.17035
8/5 0.168592    0.168592   0.168592  0.168592 0.168592    0.168592
17/10   0.166839  0.166839 0.166839    0.166839   0.166839  0.166839
9/5 0.165092    0.165092   0.165092  0.165092 0.165092    0.165092
19/10   0.16335   0.16335   0.16335   0.16335   0.16335   0.16335
2   0.161614  0.161614 0.161614    0.161614   0.161614  0.161614
21/10   0.159885  0.159885 0.159885    0.159885   0.159885  0.159885
11/5    0.158162   0.158162  0.158162 0.158162    0.158162   0.158162
23/10   0.156446  0.156446 0.156446    0.156446   0.156446  0.156446
12/5    0.154736   0.154736  0.154736 0.154736    0.154736   0.154736
5/2 0.153033    0.153033   0.153033  0.153033 0.153033    0.153033
13/5    0.151338   0.151338  0.151338 0.151338    0.151338   0.151338
27/10   0.14965   0.14965   0.14965   0.14965   0.14965   0.14965
14/5    0.14797    0.14797    0.14797    0.14797    0.14797    0.14797
29/10   0.146298  0.146298 0.146298    0.146298   0.146298  0.146298
3   0.144635  0.144635 0.144635    0.144635   0.144635  0.144635
31/10   0.150493  0.150493 0.150493    0.150493   0.150493  0.150493
16/5    0.156446   0.156446  0.156446 0.156446    0.156446   0.156446
33/10   0.162482  0.162482 0.162482    0.162482   0.162482  0.162482
17/5    0.168592   0.168592  0.168592 0.168592    0.168592   0.168592
7/2 0.174769    0.174769   0.174769  0.174769 0.174769    0.174769
18/5    0.181006   0.181006  0.181006 0.181006    0.181006   0.181006
37/10   0.187297  0.187297 0.187297    0.187297   0.187297  0.187297
19/5    0.193637   0.193637  0.193637 0.193637    0.193637   0.193637
39/10   0.200021  0.200021 0.200021    0.200021   0.200021  0.200021
4   0.206444  0.206444 0.206444    0.206444   0.206444  0.206444
41/10   0.190914  0.190914 0.190914    0.190914   0.190914  0.190914
21/5    0.175657   0.175657  0.175657 0.175657    0.175657   0.175657
43/10   0.160749  0.160749 0.160749    0.160749   0.160749  0.160749
22/5    0.146298   0.146298  0.146298 0.146298    0.146298   0.146298
9/2 0.132455    0.132455   0.132455  0.132455 0.132455    0.132455
23/5    0.132455   0.132455  0.132455 0.132455    0.132455   0.132455
)

It took a while to figure this out. Examine the code , see what you can understand, and ask about what you don't.

Eric

Hi Mario,

I learned a lot from this discussion. Please keep asking.

Eric

Hi Mario,

I've been playing around with the RotationTransform. Using it in the form that the author does, where he is transforming the vertical vector, {0, 1, 0}, by another vector {Sin[\[Theta]] Cos[\[Phi]], Cos[\[Theta]], Sin[\[Theta]] Sin[\[Phi]]} cannot be generated by successive theta and phi transformations.

So, although my workaround seems to work, it doesn't give the behaviour that the author intended.

RotationTransform[{{0,1,0},{Sin[\[Theta]]Cos[\[Phi]],Cos[\[Theta]],Sin[\[Theta]]Sin[\[Phi]]}}]

seems to work.

Hey, but, in looking at it, if theta is 0, {Sin[\[Theta]]Cos[\[Phi]],Cos[\[Theta]],Sin[\[Theta]]Sin[\[Phi]]} evaluates to {0,Cos[theta],0}, so the phi value doesn't get through.

Could this be the problem?

Eric

Hi Mario,

It took a while to go through the code, but I finally get the problem. I don't know what it is; you may have found a bug in Mathematica. But if you use separate RotationTrransforms like this:

PoseLegEndpoints[{x_, y_, z_, \[Theta]_, \[Phi]_, \[Tau]_}] := 
 Table[Composition[
    TranslationTransform[{x, y, z} + {0, platformHeight, 0}], 
    RotationTransform[\[Theta], {1, 0, 0}],
    RotationTransform[\[Phi], {0, 0, 1}],
    RotationTransform[\[Tau], {0, 1, 0}]][p], {p, 
   relativePlatformPoints}]

the second angle calculates.

For a motion path of

testMotionPlan =
  Block[{dx = 0.05, d\[Theta] = 0.5},
   {
    {0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, d\[Theta], 0},
    {0, 0, 0, 0, 0, 0},
    {0, 0, 0, 0, -d\[Theta], 0},
    {0, 0, 0, 0, 0, 0}
    }
   ];

the platform returns to its rest position at the {0, 0, 0, 0, 0, 0} points.

I used the ExportMotionPlan[] function with the last line commented out, as above.

Use MatrixPlot[] to see how the leg lengths change.

Eric

Maybe I should leave this to the experts, but I've dug my hole now.

As above,

platformHeight=1;
relativePlatformPoints := 
 Table[{Cos[\[Theta]], Sin[\[Theta]], 0}, {\[Theta],  Range[0, 5]/6 2 Pi}]

But the RotationTransform seems to be much simpler:

PoseLegEndpoints[{x_,y_,z_,\[Theta]_,\[Phi]_,\[Tau]_}]:=
Table[
    Composition[
        TranslationTransform[{x,y,z}+{0,0,platformHeight}],
        RotationTransform[\[Theta],{1,0,0}],
        RotationTransform[\[Phi],{0,1,0}],
        RotationTransform[\[Tau],{0,0,1}]
    ]
    [p], {p,relativePlatformPoints}
]

    ListPlot3D[PoseLegEndpoints[{0,0,0,0,0,0}]//N,PlotRange->{{-1.5,1.5},{-1.5,1.5},{-1,2}}]

    Manipulate[
     ListPlot3D[
      PoseLegEndpoints[{x, y, z, \[Theta], \[Phi], \[Tau]}] // N, 
      PlotRange -> {{-1.5, 1.5}, {-1.5, 1.5}, {-1, 2}}], 
        {{x, 0}, -1, 1}, {{y, 0}, -1, 1}, {{z, 0}, -1, 1}, 
        {{\[Theta], 0}, -1, 1}, {{\[Phi], 0}, -1, 1}, {{\[Tau], 0}, -1, 1}]

If we give some values to platformHeight and relativePlatformPoints, and change the {0, platformHeight, 0} to {0, 0,platformHeight}

platformHeight=1;

relativePlatformPoints:=Table[{Cos[\[Theta]],Sin[\[Theta]],0},{\[Theta],Range[0,5]/6 2Pi}]

PoseLegEndpoints[{x_,y_,z_,\[Theta]_,\[Phi]_,\[Tau]_}]:=
    Table[
        Composition[
        TranslationTransform[{x,y,z}+{0,0,platformHeight}],
        RotationTransform[{{0,1,0},
        {Sin[\[Theta]] Cos[\[Phi]],
        Cos[\[Theta]],Sin[\[Theta]] Sin[\[Phi]]}}],
        RotationTransform[\[Tau],{0,1,0}]][p],
        {p,relativePlatformPoints}
    ]

ListPlot3D[PoseLegEndpoints[{0, 0, 0, 0, 0, 0}] // N, 
 PlotRange -> {{-1, 1}, {-1, 1}, {-1, 2}}]

This appears to work. But it needs thorough checking.

EDIT:

After playing around with this, later on, it seems to still need some work. Theta and phi were not working in the above, so I put them in separate RotationTransforms. The plot looks OK now. But it still needs checking. I'm not very experienced with these transformations, although I have had a lot of experience with Stewart platforms.

PoseLegEndpoints[{x_,y_,z_,\[Theta]_,\[Phi]_,\[Tau]_}]:=
   Table[
       Composition[
          TranslationTransform[{x,y,z}+{0,0,platformHeight}],
          RotationTransform[\[Theta],{Sin[\[Theta]] Cos[\[Phi]],Cos[\[Theta]],Sin[\[Theta]] Sin[\[Phi]]}],
          RotationTransform[\[Phi],{Sin[\[Theta]] Cos[\[Phi]],Cos[\[Theta]],Sin[\[Theta]] Sin[\[Phi]]}],
          RotationTransform[\[Tau],{0,0,1}]][p],
    {p,relativePlatformPoints}
]

ListPlot3D[PoseLegEndpoints[{0,0,0,0,0,0}]//N,PlotRange->{{-1,1},{-1,1},{-1,2}}]