moveit2
The MoveIt Motion Planning Framework for ROS 2.
velocity_profile_atrap.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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 #pragma once
36 
37 #include <kdl/velocityprofile.hpp>
38 #include <iostream>
39 
41 {
51 class VelocityProfileATrap : public KDL::VelocityProfile
52 {
53 public:
60  VelocityProfileATrap(double max_vel = 0, double max_acc = 0, double max_dec = 0);
61 
75  void SetProfile(double pos1, double pos2) override;
76 
84  void SetProfileDuration(double pos1, double pos2, double duration) override;
85 
103  bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3);
104 
114  bool setProfileStartVelocity(double pos1, double pos2, double vel1);
115 
120  double firstPhaseDuration() const
121  {
122  return t_a_;
123  }
128  double secondPhaseDuration() const
129  {
130  return t_b_;
131  }
136  double thirdPhaseDuration() const
137  {
138  return t_c_;
139  }
140 
146  bool operator==(const VelocityProfileATrap& other) const;
147 
152  double Duration() const override;
158  double Pos(double time) const override;
164  double Vel(double time) const override;
170  double Acc(double time) const override;
175  void Write(std::ostream& os) const override;
180  KDL::VelocityProfile* Clone() const override;
181 
182  friend std::ostream& operator<<(std::ostream& os, const VelocityProfileATrap& p); // LCOV_EXCL_LINE
183 
184  ~VelocityProfileATrap() override;
185 
186 private:
188  void setEmptyProfile();
189 
190 private:
192  const double max_vel_;
193  const double max_acc_;
194  const double max_dec_;
195  double start_pos_;
196  double end_pos_;
197 
199  double start_vel_;
200 
202  double a1_, a2_, a3_;
203  double b1_, b2_, b3_;
204  double c1_, c2_, c3_;
205 
207  double t_a_;
208  double t_b_;
209  double t_c_;
210 };
211 
212 std::ostream& operator<<(std::ostream& os,
213  const VelocityProfileATrap& p); // LCOV_EXCL_LINE
214 
215 } // namespace pilz_industrial_motion_planner
A PTP Trajectory Generator of Asymmetric Trapezoidal Velocity Profile. Differences to VelocityProfile...
double Vel(double time) const override
Get velocity at given time.
double Pos(double time) const override
Get position at given time.
double thirdPhaseDuration() const
get the time of third phase
VelocityProfileATrap(double max_vel=0, double max_acc=0, double max_dec=0)
Constructor.
void SetProfile(double pos1, double pos2) override
compute the fastest profile Algorithm:
bool setProfileStartVelocity(double pos1, double pos2, double vel1)
Profile with start velocity Note: This function is not general and is currently only used for live co...
double secondPhaseDuration() const
get the time of second phase
void SetProfileDuration(double pos1, double pos2, double duration) override
Profile scaled by the total duration.
KDL::VelocityProfile * Clone() const override
returns copy of current VelocityProfile object
bool setProfileAllDurations(double pos1, double pos2, double duration1, double duration2, double duration3)
Profile with given acceleration/constant/deceleration durations. Each duration must obey the maximal ...
bool operator==(const VelocityProfileATrap &other) const
Compares two Asymmetric Trapezoidal Velocity Profiles.
friend std::ostream & operator<<(std::ostream &os, const VelocityProfileATrap &p)
void Write(std::ostream &os) const override
Write basic information.
double Acc(double time) const override
Get given acceleration/deceleration at given time.
double firstPhaseDuration() const
get the time of first phase
p
Definition: pick.py:62
std::ostream & operator<<(std::ostream &os, const VelocityProfileATrap &p)