Head

GCC Code Coverage Report

Directory:	src/		Exec	Total	Coverage
File:	src/speed_limiter.cpp	Lines:	0	35	0.0 %
Date:	2022-09-12 09:50:59	Branches:	0	14	0.0 %


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the PAL Robotics nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/*
 * Author: Enrique Fernández
 */

#include "speed_limiter.h"

#include <algorithm>

template <typename T>
T clamp(T x, T min, T max) {
  return std::min(std::max(min, x), max);
}

namespace dynamicgraph {

SpeedLimiter::SpeedLimiter(bool has_velocity_limits,
                           bool has_acceleration_limits, bool has_jerk_limits,
                           double min_velocity, double max_velocity,
                           double min_acceleration, double max_acceleration,
                           double min_jerk, double max_jerk)
    : has_velocity_limits(has_velocity_limits),
      has_acceleration_limits(has_acceleration_limits),
      has_jerk_limits(has_jerk_limits),
      min_velocity(min_velocity),
      max_velocity(max_velocity),
      min_acceleration(min_acceleration),
      max_acceleration(max_acceleration),
      min_jerk(min_jerk),
      max_jerk(max_jerk) {}

double SpeedLimiter::limit(double& v, double v0, double v1, double dt) {
  const double tmp = v;

  limit_jerk(v, v0, v1, dt);
  limit_acceleration(v, v0, dt);
  limit_velocity(v);

  return tmp != 0.0 ? v / tmp : 1.0;
}

double SpeedLimiter::limit_velocity(double& v) {
  const double tmp = v;

  if (has_velocity_limits) {
    v = clamp(v, min_velocity, max_velocity);
  }

  return tmp != 0.0 ? v / tmp : 1.0;
}

double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) {
  const double tmp = v;

  if (has_acceleration_limits) {
    const double dv_min = min_acceleration * dt;
    const double dv_max = max_acceleration * dt;

    const double dv = clamp(v - v0, dv_min, dv_max);

    v = v0 + dv;
  }

  return tmp != 0.0 ? v / tmp : 1.0;
}

double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) {
  const double tmp = v;

  if (has_jerk_limits) {
    const double dv = v - v0;
    const double dv0 = v0 - v1;

    const double dt2 = 2. * dt * dt;

    const double da_min = min_jerk * dt2;
    const double da_max = max_jerk * dt2;

    const double da = clamp(dv - dv0, da_min, da_max);

    v = v0 + dv0 + da;
  }

  return tmp != 0.0 ? v / tmp : 1.0;
}

}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*********************************************************************
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2013, PAL Robotics, S.L.
5			* All rights reserved.
6			*
7			* Redistribution and use in source and binary forms, with or without
8			* modification, are permitted provided that the following conditions
9			* are met:
10			*
11			* * Redistributions of source code must retain the above copyright
12			* notice, this list of conditions and the following disclaimer.
13			* * Redistributions in binary form must reproduce the above
14			* copyright notice, this list of conditions and the following
15			* disclaimer in the documentation and/or other materials provided
16			* with the distribution.
17			* * Neither the name of the PAL Robotics nor the names of its
18			* contributors may be used to endorse or promote products derived
19			* from this software without specific prior written permission.
20			*
21			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32			* POSSIBILITY OF SUCH DAMAGE.
33			*********************************************************************/
34
35			/*
36			* Author: Enrique Fernández
37			*/
38
39			#include "speed_limiter.h"
40
41			#include <algorithm>
42
43			template <typename T>
44			T clamp(T x, T min, T max) {
45			return std::min(std::max(min, x), max);
46			}
47
48			namespace dynamicgraph {
49
50			SpeedLimiter::SpeedLimiter(bool has_velocity_limits,
51			bool has_acceleration_limits, bool has_jerk_limits,
52			double min_velocity, double max_velocity,
53			double min_acceleration, double max_acceleration,
54			double min_jerk, double max_jerk)
55			: has_velocity_limits(has_velocity_limits),
56			has_acceleration_limits(has_acceleration_limits),
57			has_jerk_limits(has_jerk_limits),
58			min_velocity(min_velocity),
59			max_velocity(max_velocity),
60			min_acceleration(min_acceleration),
61			max_acceleration(max_acceleration),
62			min_jerk(min_jerk),
63			max_jerk(max_jerk) {}
64
65			double SpeedLimiter::limit(double& v, double v0, double v1, double dt) {
66			const double tmp = v;
67
68			limit_jerk(v, v0, v1, dt);
69			limit_acceleration(v, v0, dt);
70			limit_velocity(v);
71
72			return tmp != 0.0 ? v / tmp : 1.0;
73			}
74
75			double SpeedLimiter::limit_velocity(double& v) {
76			const double tmp = v;
77
78			if (has_velocity_limits) {
79			v = clamp(v, min_velocity, max_velocity);
80			}
81
82			return tmp != 0.0 ? v / tmp : 1.0;
83			}
84
85			double SpeedLimiter::limit_acceleration(double& v, double v0, double dt) {
86			const double tmp = v;
87
88			if (has_acceleration_limits) {
89			const double dv_min = min_acceleration * dt;
90			const double dv_max = max_acceleration * dt;
91
92			const double dv = clamp(v - v0, dv_min, dv_max);
93
94			v = v0 + dv;
95			}
96
97			return tmp != 0.0 ? v / tmp : 1.0;
98			}
99
100			double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt) {
101			const double tmp = v;
102
103			if (has_jerk_limits) {
104			const double dv = v - v0;
105			const double dv0 = v0 - v1;
106
107			const double dt2 = 2. * dt * dt;
108
109			const double da_min = min_jerk * dt2;
110			const double da_max = max_jerk * dt2;
111
112			const double da = clamp(dv - dv0, da_min, da_max);
113
114			v = v0 + dv0 + da;
115			}
116
117			return tmp != 0.0 ? v / tmp : 1.0;
118			}
119
120			} // namespace dynamicgraph