block KinematicPTP "Move as fast as possible along a distance within given kinematic constraints"
parameter Real deltaq[:] = {1} "Distance to move"
annotation (Dialog(groupImage = "modelica://Modelica/Resources/Images/Blocks/Sources/KinematicPTP.png"));
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 SIunits.Time startTime = 0 "Time instant at which movement starts";
extends Interfaces.MO(final nout = max([size(deltaq, 1); size(qd_max, 1); size(qdd_max, 1)]));
protected
parameter Real p_deltaq[nout] = if size(deltaq, 1) == 1 then ones(nout) * deltaq[1] else deltaq;
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;
Real sd_max;
Real sdd_max;
Real sdd;
Real aux1[nout];
Real aux2[nout];
SIunits.Time Ta1;
SIunits.Time Ta2;
SIunits.Time Tv;
SIunits.Time Te;
Boolean noWphase;
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;
y = p_deltaq * sdd;
Ta1 = sqrt(sdd_max ^ (-1));
Ta2 = sd_max / sdd_max;
Te = if noWphase then Ta1 + Ta1 else Tv + Ta2;
Tv = if noWphase then Ta1 else sd_max ^ (-1);
noWphase = Ta1 <= Ta2;
sd_max = max(abs(aux1)) ^ (-1);
sdd = if time < startTime then 0 else if noWphase then if time < Ta1 + startTime then sdd_max else if time < Te + startTime then -sdd_max else 0 else if time < Ta2 + startTime then sdd_max else if time < Tv + startTime then 0 else if time < Te + startTime then -sdd_max else 0;
sdd_max = max(abs(aux2)) ^ (-1);
annotation (
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},
{82, 0}},
color = {192, 192, 192}),
Polygon(
points = {
{90, 0},
{68, 8},
{68, -8},
{90, 0}},
lineColor = {192, 192, 192},
fillColor = {192, 192, 192},
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}}),
Text(
extent = {
{2, 80},
{80, 20}},
lineColor = {192, 192, 192},
textString = "acc"),
Text(
extent = {
{-150, -150},
{150, -110}},
textString = "deltaq=%deltaq")}),
Diagram(
coordinateSystem(
preserveAspectRatio = true,
extent = {
{-100, -100},
{100, 100}}),
graphics = {
Line(
points = {
{-80, 78},
{-80, -72}},
color = {95, 95, 95}),
Polygon(
points = {
{-80, 91},
{-86, 71},
{-75, 71},
{-80, 91},
{-80, 91}},
lineColor = {95, 95, 95},
fillColor = {95, 95, 95},
fillPattern = FillPattern.Solid),
Line(
points = {
{-90, 0},
{82, 0}},
color = {95, 95, 95}),
Polygon(
points = {
{89, 0},
{68, 5},
{68, -5},
{89, 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 = {
{-73, 95},
{-16, 80}},
textString = "acceleration"),
Text(
extent = {
{66, 20},
{88, 8}},
textString = "time")}),
Documentation(
info = "<html>\n<p>\nThe goal is to move as <strong>fast</strong> as possible along a distance\n<strong>deltaq</strong>\nunder given <strong>kinematical constraints</strong>. The distance can be a positional or\nangular range. In robotics such a movement is called <strong>PTP</strong> (Point-To-Point).\nThis source block generates the <strong>acceleration</strong> qdd of this signal\nas output:\n</p>\n\n<p>\n<img src=\"modelica://Modelica/Resources/Images/Blocks/Sources/KinematicPTP.png\"\n alt=\"KinematicPTP.png\">\n</p>\n\n<p>\nAfter integrating the output two times, the position q is\nobtained. The signal is 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<p>\nIf several distances are given (vector deltaq has more than 1 element),\nan acceleration output vector is 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 a drive train or in combination with model\nModelica.Mechanics.Rotational.<strong>Accelerate</strong> to drive\na flange according to a given acceleration.\n</p>\n\n</html>",
revisions = "<html>\n<p><strong>Release Notes:</strong></p>\n<ul>\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 KinematicPTP;