Message Boards Message Boards

0
|
11177 Views
|
7 Replies
|
3 Total Likes
View groups...
Share
Share this post:

Newbie Kinematics question

novio8
Posted 9 years ago

Hi, I'm calculating motion data from list of poses

 motionPlan = {
       {0, 0, 0, 0, 0, 0, 0},
       {3.4, 0, 0.08, 0, 0, 0, 0},
       {3.5, 0, 0.077, 0, 0, 0, 0},
       {4, 0, 0.08, 0, 0, 0, 0},
       {4.1, 0, 0.077, 0, 0, 0, 0},
       {4.2, 0, 0.08, 0, 0, 0, 0},
       {4.3, 0, 0.083, 0, 0, 0, 0},
       {4.4, 0, 0.08, 0, 0, 0, 0},
       {5, 0, 0.1, 0, 0, 0, 0}   };

Utility function for generating a sequence of steps between two poses:

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

This generates a list of numbers between two pose divided into the number of frames per second. I would like to smooth out the motion data and change Acceleration.

Please help Mario
POSTED BY: novio8
7 Replies
Sort By:

Hi Mario,

I've used the original dimensions of the platform because I don't know what your base is.

I've also changed the rotation from radians to degrees.

So, here is the interpolation function:

splinedMotionPlan[motionPlan_, 
  frameRate_] := (fn2 = 
   Interpolation[
    Transpose[{motionPlan[[All, 1]], motionPlan[[All, 2 ;; 7]]}], 
    Method -> "Spline", InterpolationOrder -> 2];
  fn2[Range[0, motionPlan[[-1, 1]], 1/frameRate]][[All]])

fn2 is the second order interpolation function. Note that if you change the interpolation order to 1, the motion will be the same as the original linear motion. motionPlan[[-1, 1]] is the last time in the motion plan, so the Range function will generate a series of time slots from 0 to the maximum time in 1/frameRate periods.

this makes the ExportMotionPlan function much simpler:

ExportMotionPlan[poses_List, legoffsets_: {0, 0, 0, 0, 0, 0}] := 
 Module[{framerate = 10, lengths, tabledata1, tabledata2, 
   splinedPoses},
  lengths = Map[PoseLegLengths, splinedMotionPlan[poses, framerate]];
  tabledata1 = 
   MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];

  {Export[
    FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath.txt"}], {{"LegLengths", tabledata1}}, 
    "ModelicaCombiTimeTable"], First[Last[tabledata1]]};

  lengths = Map[# - legoffsets &, lengths];
  tabledata2 = 
   MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];

  {Export[
    FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath2.txt"}], {{"LegLengths", tabledata2}}, 
    "ModelicaCombiTimeTable"], First[Last[tabledata2]]};
  ]

A notebook is attached. As an exercise, you might try to make the interpolation order one of the arguments. Be careful of the splines; they could sent you into wild overshoots.

Eric

Attachments:
POSTED BY: Eric Johnstone
novio8
novio8
Posted 9 years ago

Hi Eric,

1.Yes I'v changed the dimensions of Stewart Platform

2.Using Spline method has it's advantages, but one has to be careful when

there are few data (less than 25 knots) or the time interval is far from

equispaced .

  1. I was not successful applynig Spline interpolation in Code bellow, please help

Mario

    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}]

    Function giving the lengths of each leg as a function of the pose coordinates:

    PoseLegLengths[pose_] := Map[Norm, PoseLegEndpoints[N[pose]] - basePoints]



    {PoseLegLengths[{0, 0, 0, 0, 0, 0}], PoseLegLengths[{0, 0.01, 0, 0, 0, 0}]}

    {{1.13137, 1.13137, 1.13137, 1.13137, 1.13137, 1.13137}, {1.13793, 1.13793, 
      1.13793, 1.13793, 1.13793, 1.13793}}

    Utility function for generating a sequence of steps between two poses:

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

    Function to tabulate and export a sequence of leg lengths corresponding to a platform shifting between a list of poses at the rate of one pose per second, with a specified fixed offset applied to the length each leg:

    ExportMotionPlan[poses_List, 
      legoffsets_: {1.1313669977935081`, 1.1313669977935081`, 1.1313669977935084`,
         1.131366997793508`, 1.1313669977935086`, 1.131366997793508`}] := 
     Module[{framerate = 50, 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[# &, lengths];
      tabledata = MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];
      {Export[FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
          "PlatformPath.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]]}
      ]

    motionPlan = {
       {0, 0, 0, 0, 0, 0, 0},
       {3.4, 0, 0.40, 0, 0, 0, 0},
       {3.5, 0, 0.47, 0, 0, 0, 0},
       {4, 0, 0.48, 0, 0, 0, 0},
       {4.1, 0, 0.477, 0, 0, 0, 0},
       {4.2, 0, 0.48, 0, 0, 0, 0},
       {4.3, 0, 0.483, 0, 0, 0, 0},
       {4.4, 0, 0.48, 0, 0, 0, 0},
       {5, 0, 0.5, 0, 0, 0, 0}, {
        7, 0, 0.5, 0, 10, 0, 0},
       {7.1, 0, 0.197, 0, 0, 0, 0},
       {7.3, 0, 0.2, 0, 0, 10, 0}

}; ExportMotionPlan[motionPlan];
POSTED BY: novio8

Hi Mario,

Here is what happens when you use the spline interpolation on your motionPlan:

    fn1=Interpolation[Transpose[{motionPlan[[All,1]],motionPlan[[All,2;;7]]}],Method->"Spline",InterpolationOrder->1];
    fn2=Interpolation[Transpose[{motionPlan[[All,1]],motionPlan[[All,2;;7]]}],Method->"Spline",InterpolationOrder->2];

    Show[ListLinePlot[fn1[Range[0,10.5,.1]][[All,2]],PlotRange->{-.5,1.7}],
         ListLinePlot[fn2[Range[0,10.5,.1]][[All,2]],PlotRange->{-.5,1.7}]]

overshoot

The overshoot is unacceptable. In the first part of the motion, the spline even goes in the opposite direction. We need to find a way to stay in the linear motionPlan but use computed acceleration and deceleration as the plan points are left and approached.

Unless you have changed the dimensions of the Stewart platform in the demo, the range of values in your motion plan exceed the dimensions of the actuators.

If we assume that the time intervals must be constant, we will have to change the distance intervals to get acceleration and deceleration. That will require some thought.

Eric
POSTED BY: Eric Johnstone
novio8
novio8
Posted 9 years ago

Hi Eric,

I'm learning too, especially from your suggestions. So where do I input Spline Interpolation in my Code:

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}]

Function giving the lengths of each leg as a function of the pose coordinates:

PoseLegLengths[pose_] := Map[Norm, PoseLegEndpoints[N[pose]] - basePoints]



{PoseLegLengths[{0, 0, 0, 0, 0, 0}], PoseLegLengths[{0, 0.01, 0, 0, 0, 0}]}

{{1.13137, 1.13137, 1.13137, 1.13137, 1.13137, 1.13137}, {1.13793, 1.13793, 
  1.13793, 1.13793, 1.13793, 1.13793}}

Utility function for generating a sequence of steps between two poses:

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

Function to tabulate and export a sequence of leg lengths corresponding to a platform shifting between a list of poses at the rate of one pose per second, with a specified fixed offset applied to the length each leg:

ExportMotionPlan[poses_List, 
  legoffsets_: {1.1313669977935081`, 1.1313669977935081`, 1.1313669977935084`,
     1.131366997793508`, 1.1313669977935086`, 1.131366997793508`}] := 
 Module[{framerate = 50, 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[# &, lengths];
  tabledata = MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];
  {Export[FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath.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]]}
  ]

motionPlan = {
   {0, 0, 0, 0, 0, 0, 0},
   {3.4, 0, 0.40, 0, 0, 0, 0},
   {3.5, 0, 0.47, 0, 0, 0, 0},
   {4, 0, 0.48, 0, 0, 0, 0},
   {4.1, 0, 0.477, 0, 0, 0, 0},
   {4.2, 0, 0.48, 0, 0, 0, 0},
   {4.3, 0, 0.483, 0, 0, 0, 0},
   {4.4, 0, 0.48, 0, 0, 0, 0},
   {5, 0, 0.5, 0, 0, 0, 0}, {
    7, 0, 0.5, 0, 10, 0, 0},
   {7.1, 0, 0.197, 0, 0, 0, 0},
   {7.3, 0, 0.2, 0, 0, 10, 0},
   {7.4, 0, 0.197, 0, 0, 0, 10},
   {8, 0, 0.205, 0, 0, 0, 0},
   {8.1, 0, 0.2, 0, 0, 0, 0},
   {8.3, 0, 0.205, 0, 0, 0, 0},
   {8.4, 0, 0.2, 0, 0, 0, 0},
   {9, 0.05, 0.205, 0, 0, 0, 0},
   {9.3, 0, 0.205, 0, 0, 0, 0},
   {10.1, -0.05, 0.205, 0, 0, 0, 0},
   {10.5, 0.05, 0.21, 0, 0, 0, 0}
   };
ExportMotionPlan[motionPlan];

Mario
POSTED BY: novio8

Hi Mario,

I was wondering when you would inquire about acceleration! If the motion plan is implemented directly, the platform makes sudden jerks as it changes from one linear motion to another. Jon McLoone's suggestion is a good one, but it will overshoot the points in the motion plan. There will not be any discontinuities if the spline method is used. As usual, I'm learning a lot from this discussion.

Here is the motionPlan:

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}
  }

Jon's code needs a little massaging:

fn1=Interpolation[Transpose[{motionPlan[[All,1]],motionPlan[[All,2;;7]]}],Method->"Spline",InterpolationOrder->1];
fn2=Interpolation[Transpose[{motionPlan[[All,1]],motionPlan[[All,2;;7]]}],Method->"Spline",InterpolationOrder->2];

This generates spline functions that go through the points in the motion plan. Here is what they produce

Show[ListLinePlot[fn1[Range[0, 4.5, .1]][[All, 2]],  PlotRange -> {-.05, .05}], 
 ListLinePlot[fn2[Range[0, 4.5, .1]][[All, 2]],  PlotRange -> {-.05, .05}]]

orders

The straight lines are the current motion plan. The curved line is the spline that goes through the points in the motion plan.

The alternative is to control the acceleration by manipulating the time divisions. That is, to use uneven time intervals. Is this possible in SystemModeler?

Eric
POSTED BY: Eric Johnstone
novio8
novio8
Posted 9 years ago

Hi John, thank you for your answer, but I just can't figure it out. I have the following code

to generate motion data:

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}]

Function giving the lengths of each leg as a function of the pose coordinates:

PoseLegLengths[pose_] := Map[Norm, PoseLegEndpoints[N[pose]] - basePoints]



{PoseLegLengths[{0, 0, 0, 0, 0, 0}], PoseLegLengths[{0, 0.01, 0, 0, 0, 0}]}

{{1.13137, 1.13137, 1.13137, 1.13137, 1.13137, 1.13137}, {1.13793, 1.13793, 
  1.13793, 1.13793, 1.13793, 1.13793}}

Utility function for generating a sequence of steps between two poses:

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

Function to tabulate and export a sequence of leg lengths corresponding to a platform shifting between a list of poses at the rate of one pose per second, with a specified fixed offset applied to the length each leg:

ExportMotionPlan[poses_List, 
  legoffsets_: {1.1313669977935081`, 1.1313669977935081`, 1.1313669977935084`,
     1.131366997793508`, 1.1313669977935086`, 1.131366997793508`}] := 
 Module[{framerate = 50, 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[# &, lengths];
  tabledata = MapIndexed[Prepend[#, (First[#2] - 1)/framerate] &, lengths];
  {Export[FileNameJoin[{NotebookDirectory[], "DocumentationFiles", 
      "PlatformPath.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]]}
  ]

motionPlan = {
   {0, 0, 0, 0, 0, 0, 0},
   {3.4, 0, 0.40, 0, 0, 0, 0},
   {3.5, 0, 0.47, 0, 0, 0, 0},
   {4, 0, 0.48, 0, 0, 0, 0},
   {4.1, 0, 0.477, 0, 0, 0, 0},
   {4.2, 0, 0.48, 0, 0, 0, 0},
   {4.3, 0, 0.483, 0, 0, 0, 0},
   {4.4, 0, 0.48, 0, 0, 0, 0},
   {5, 0, 0.5, 0, 0, 0, 0}, {
    7, 0, 0.5, 0, 10, 0, 0},
   {7.1, 0, 0.197, 0, 0, 0, 0},
   {7.3, 0, 0.2, 0, 0, 10, 0},
   {7.4, 0, 0.197, 0, 0, 0, 10},
   {8, 0, 0.205, 0, 0, 0, 0},
   {8.1, 0, 0.2, 0, 0, 0, 0},
   {8.3, 0, 0.205, 0, 0, 0, 0},
   {8.4, 0, 0.2, 0, 0, 0, 0},
   {9, 0.05, 0.205, 0, 0, 0, 0},
   {9.3, 0, 0.205, 0, 0, 0, 0},
   {10.1, -0.05, 0.205, 0, 0, 0, 0},
   {10.5, 0.05, 0.21, 0, 0, 0, 0}
   };
ExportMotionPlan[motionPlan];

Any suggestion is much appreciated.

Mario
POSTED BY: novio8

Try indexing the frames and interpolating...

fn = Interpolation[Transpose[{Range[Length[motionPlan]], motionPlan}]];

At the indexes it will match the data... In[2]:= fn[1] Out[12]= {0., 0., 0., 0., 0., 0., 0.} In[3]:= fn[2] Out[3]= {3.4, 0., 0.08, 0., 0., 0., 0.}

But for non-integer values you get interpolation... In[4]:= fn[1.75] Out[4]= {3.00391, 0., 0.0712578, 0., 0., 0., 0.}

Use options InterpolationOrder, and Method to decide how smooth you want the interpolation.
POSTED BY: Jon McLoone
Reply to this discussion
Community posts can be styled and formatted using the Markdown syntax.
Tag limit exceeded
Note: Only the first five people you tag will receive an email notification; the other tagged names will appear as links to their profiles.
Reply Preview
Attachments
Remove
or Discard

Group Abstract Group Abstract