block KinematicPTP2 "Move as fast as possible from start to end position within given kinematic constraints with output signals q, qd=der(q), qdd=der(qd)"
parameter Real q_begin[:] = {0} "Start position"
annotation (Dialog(groupImage = "modelica://Modelica/Resources/Images/Blocks/Sources/KinematicPTP2.png"));
parameter Real q_end[:] = {1} "End position";
parameter Real qd_max[:](each final min = Modelica.Constants.small) = {1} "Maximum velocities der(q)";
parameter Real qdd_max[:](each final min = Modelica.Constants.small) = {1} "Maximum accelerations der(qd)";
parameter Modelica.SIunits.Time startTime = 0 "Time instant at which movement starts";
extends Modelica.Blocks.Icons.Block;
final parameter Integer nout = max([size(q_begin, 1); size(q_end, 1); size(qd_max, 1); size(qdd_max, 1)]) "Number of output signals (= dimension of q, qd, qdd, moving)";
output Modelica.SIunits.Time endTime "Time instant at which movement stops";
Modelica.Blocks.Interfaces.RealOutput q[nout] "Reference position of path planning"
annotation (Placement(transformation(extent = {
{100, 70},
{120, 90}})));
Modelica.Blocks.Interfaces.RealOutput qd[nout] "Reference speed of path planning"
annotation (Placement(transformation(extent = {
{100, 20},
{120, 40}})));
Modelica.Blocks.Interfaces.RealOutput qdd[nout] "Reference acceleration of path planning"
annotation (Placement(transformation(extent = {
{100, -40},
{120, -20}})));
Modelica.Blocks.Interfaces.BooleanOutput moving[nout] "= true, if end position not yet reached; = false, if end position reached or axis is completely at rest"
annotation (Placement(transformation(extent = {
{100, -90},
{120, -70}})));
protected
parameter Real p_q_begin[nout] = if size(q_begin, 1) == 1 then ones(nout) * q_begin[1] else q_begin;
parameter Real p_q_end[nout] = if size(q_end, 1) == 1 then ones(nout) * q_end[1] else q_end;
parameter Real p_qd_max[nout] = if size(qd_max, 1) == 1 then ones(nout) * qd_max[1] else qd_max;
parameter Real p_qdd_max[nout] = if size(qdd_max, 1) == 1 then ones(nout) * qdd_max[1] else qdd_max;
parameter Real p_deltaq[nout] = p_q_end - p_q_begin;
constant Real eps = 10 * Modelica.Constants.eps;
Boolean motion_ref;
Real sd_max_inv;
Real sdd_max_inv;
Real sd_max;
Real sdd_max;
Real sdd;
Real aux1[nout];
Real aux2[nout];
SI.Time Ta1;
SI.Time Ta2;
SI.Time Tv;
SI.Time Te;
Boolean noWphase;
SI.Time Ta1s;
SI.Time Ta2s;
SI.Time Tvs;
SI.Time Tes;
Real sd_max2;
Real s1;
Real s2;
Real s3;
Real s;
Real sd;
equation
for i in 1:nout loop
aux1[i] = p_deltaq[i] / p_qd_max[i];
aux2[i] = p_deltaq[i] / p_qdd_max[i];
end for;
for i in 1:nout loop
moving[i] = if eps < abs(q_begin[i] - q_end[i]) then motion_ref else false;
end for;
if sd_max_inv <= eps or sdd_max_inv <= eps then
sd_max = 0;
sdd_max = 0;
Ta1 = 0;
Ta2 = 0;
noWphase = false;
Tv = 0;
Te = 0;
Ta1s = 0;
Ta2s = 0;
Tvs = 0;
Tes = 0;
sd_max2 = 0;
s1 = 0;
s2 = 0;
s3 = 0;
s = 0;
else
sd_max = max(abs(aux1)) ^ (-1);
sdd_max = max(abs(aux2)) ^ (-1);
Ta1 = sqrt(sdd_max ^ (-1));
Ta2 = sd_max / sdd_max;
noWphase = Ta1 <= Ta2;
Tv = if noWphase then Ta1 else sd_max ^ (-1);
Te = if noWphase then Ta1 + Ta1 else Tv + Ta2;
Ta1s = Ta1 + startTime;
Ta2s = Ta2 + startTime;
Tvs = Tv + startTime;
Tes = Te + startTime;
sd_max2 = sdd_max * Ta1;
s1 = 0.5 * (sdd_max * (if noWphase then Ta1 * Ta1 else Ta2 * Ta2));
s2 = s1 + (if noWphase then sd_max2 * (Te - Ta1) - 0.5 * sdd_max * (Te - Ta1) ^ 2 else sd_max * (Tv - Ta2));
s3 = s2 + sd_max * (Te - Tv) - 0.5 * sdd_max * (Te - Tv) * (Te - Tv);
if time < startTime then
s = 0;
elseif noWphase then
if time < Ta1s then
s = 0.5 * sdd_max * (time - startTime) * (time - startTime);
elseif time < Tes then
s = s1 + sd_max2 * (time - Ta1s) - 0.5 * sdd_max * (time - Ta1s) * (time - Ta1s);
else
s = s2;
end if;
elseif time < Ta2s then
s = 0.5 * sdd_max * (time - startTime) * (time - startTime);
elseif time < Tvs then
s = s1 + sd_max * (time - Ta2s);
elseif time < Tes then
s = s2 + sd_max * (time - Tvs) - 0.5 * sdd_max * (time - Tvs) * (time - Tvs);
else
s = s3;
end if;
end if;
q = p_q_begin + p_deltaq * s;
endTime = Tes;
qd = p_deltaq * sd;
qdd = p_deltaq * sdd;
sd = der(s);
sdd = der(sd);
motion_ref = time < endTime;
sd_max_inv = max(abs(aux1));
sdd_max_inv = max(abs(aux2));
annotation (
defaultComponentName = "kinematicPTP",
Icon(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(
points = {
{-80, 78},
{-80, -82}},
color = {192, 192, 192}),
Polygon(
points = {
{-80, 90},
{-88, 68},
{-72, 68},
{-80, 88},
{-80, 90}},
lineColor = {192, 192, 192},
fillColor = {192, 192, 192},
fillPattern = FillPattern.Solid),
Line(
points = {
{-90, 0},
{17, 0}},
color = {192, 192, 192}),
Line(points = {
{-80, 0},
{-70, 0},
{-70, 70},
{-50, 70},
{-50, 0},
{-15, 0},
{-15, -70},
{5, -70},
{5, 0},
{18, 0}}),
Text(
extent = {
{34, 96},
{94, 66}},
textString = "q"),
Text(
extent = {
{40, 44},
{96, 14}},
textString = "qd"),
Text(
extent = {
{32, -18},
{99, -44}},
textString = "qdd"),
Text(
extent = {
{-32, -74},
{97, -96}},
textString = "moving")}),
Diagram(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(
points = {
{-80, 78},
{-80, -70}},
color = {95, 95, 95}),
Polygon(
points = {
{-80, 94},
{-86, 74},
{-74, 74},
{-80, 94},
{-80, 94}},
lineColor = {95, 95, 95},
fillColor = {95, 95, 95},
fillPattern = FillPattern.Solid),
Line(
points = {
{-90, 0},
{82, 0}},
color = {95, 95, 95}),
Polygon(
points = {
{90, 0},
{68, 6},
{68, -6},
{90, 0}},
lineColor = {95, 95, 95},
fillColor = {95, 95, 95},
fillPattern = FillPattern.Solid),
Line(
points = {
{-80, 0},
{-70, 0},
{-70, 70},
{-30, 70},
{-30, 0},
{20, 0},
{20, -70},
{60, -70},
{60, 0},
{68, 0}},
color = {0, 0, 255},
thickness = 0.5),
Text(
extent = {
{-72, 96},
{-15, 81}},
textString = "acceleration"),
Text(
extent = {
{69, 18},
{91, 6}},
textString = "time")}),
Documentation(
info = "<html>\n<p>\nThe goal is to move as <strong>fast</strong> as possible from start position <strong>q_begin</strong>\nto end position <strong>q_end</strong>\nunder given <strong>kinematical constraints</strong>. The positions can be translational or\nrotational definitions (i.e., q_begin/q_end is given). In robotics such a movement is called <strong>PTP</strong> (Point-To-Point).\nThis source block generates the <strong>position</strong> q(t), the\n<strong>speed</strong> qd(t) = der(q), and the <strong>acceleration</strong> qdd = der(qd)\nas output. The signals are constructed in such a way that it is not possible\nto move faster, given the <strong>maximally</strong> allowed <strong>velocity</strong> qd_max and\nthe <strong>maximally</strong> allowed <strong>acceleration</strong> qdd_max:\n</p>\n\n<p>\n<img src=\"modelica://Modelica/Resources/Images/Blocks/Sources/KinematicPTP2.png\"\n alt=\"KinematicPTP2.png\">\n</p>\n\n<p>\nIf vectors q_begin/q_end have more than 1 element,\nthe output vectors are constructed such that all signals\nare in the same periods in the acceleration, constant velocity\nand deceleration phase. This means that only one of the signals\nis at its limits whereas the others are synchronized in such a way\nthat the end point is reached at the same time instant.\n</p>\n\n<p>\nThis element is useful to generate a reference signal for a controller\nwhich controls, e.g., a drive train, or to drive\na flange according to a given acceleration.\n</p>\n\n</html>",
revisions = "<html>\n<ul>\n<li><em>March 24, 2007</em>\n by <a href=\"http://www.robotic.dlr.de/Martin.Otter/\">Martin Otter</a>:<br>\n Non-standard Modelica function \"constrain(..)\" replaced by standard\n Modelica implementation (via internal function position()).<br>\n New output signal \"moving\" added.</li>\n<li><em>June 27, 2001</em>\n by Bernhard Bachmann.<br>\n Bug fixed that element is also correct if startTime is not zero.</li>\n<li><em>Nov. 3, 1999</em>\n by <a href=\"http://www.robotic.dlr.de/Martin.Otter/\">Martin Otter</a>:<br>\n Vectorized and moved from Rotational to Blocks.Sources.</li>\n<li><em>June 29, 1999</em>\n by <a href=\"http://www.robotic.dlr.de/Martin.Otter/\">Martin Otter</a>:<br>\n realized.</li>\n</ul>\n</html>"));
end KinematicPTP2;