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