Main MRPT website > C++ reference for MRPT 1.9.9
CVehicleSimul_Holo.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "kinematics-precomp.h" // Precompiled header
11 
13 #include <mrpt/math/wrap2pi.h>
14 
15 using namespace mrpt::kinematics;
16 
18 {
19  resetStatus();
20  resetTime();
21 }
22 
24 {
25  // Control:
26  if (m_vel_ramp_cmd.issue_time >= 0 &&
27  m_time > m_vel_ramp_cmd.issue_time) // are we executing any cmd?
28  {
29  const double t = m_time - m_vel_ramp_cmd.issue_time;
30  const double T = m_vel_ramp_cmd.ramp_time;
31  const double vxi = m_vel_ramp_cmd.init_vel.vx;
32  const double vyi = m_vel_ramp_cmd.init_vel.vy;
33  const double wi = m_vel_ramp_cmd.init_vel.omega;
34  const double vxf = m_vel_ramp_cmd.target_vel_x;
35  const double vyf = m_vel_ramp_cmd.target_vel_y;
36 
37  // "Blending" for vx,vy
38  if (t <= m_vel_ramp_cmd.ramp_time)
39  {
40  m_odometric_vel.vx = vxi + t * (vxf - vxi) / T;
41  m_odometric_vel.vy = vyi + t * (vyf - vyi) / T;
42  }
43  else
44  {
47  }
48 
49  // Ramp rotvel until aligned:
50  const double Aang =
52  if (std::abs(Aang) < mrpt::DEG2RAD(1.0))
53  {
54  m_odometric_vel.omega = .0; // we are aligned.
55  }
56  else
57  {
58  const double wf =
59  mrpt::sign(Aang) * std::abs(m_vel_ramp_cmd.rot_speed);
60  if (t <= m_vel_ramp_cmd.ramp_time)
61  {
62  m_odometric_vel.omega = wi + t * (wf - wi) / T;
63  }
64  else
65  {
67  }
68  }
69  }
70 }
71 
74  double vel, double dir, double ramp_time, double rot_speed)
75 {
76  ASSERT_ABOVE_(ramp_time, 0);
77 
79  m_vel_ramp_cmd.ramp_time = ramp_time;
80  m_vel_ramp_cmd.rot_speed = rot_speed;
82  m_vel_ramp_cmd.target_vel_x = cos(dir) * vel;
83  m_vel_ramp_cmd.target_vel_y = sin(dir) * vel;
84  m_vel_ramp_cmd.dir = dir;
85 }
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::rot_speed
double rot_speed
Definition: CVehicleSimul_Holo.h:66
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:171
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::dir
double dir
Definition: CVehicleSimul_Holo.h:66
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: lightweight_geom_data.h:195
mrpt::kinematics::CVehicleSimul_Holo::CVehicleSimul_Holo
CVehicleSimul_Holo()
Definition: CVehicleSimul_Holo.cpp:17
mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus
void resetStatus()
Definition: CVehicleSimulVirtualBase.cpp:79
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::ramp_time
double ramp_time
Definition: CVehicleSimul_Holo.h:66
mrpt::kinematics::CVehicleSimul_Holo::m_vel_ramp_cmd
TVelRampCmd m_vel_ramp_cmd
the last cmd received from the user.
Definition: CVehicleSimul_Holo.h:72
wrap2pi.h
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::target_vel_x
double target_vel_x
Definition: CVehicleSimul_Holo.h:66
mrpt::math::TTwist2D::vx
double vx
Velocity components: X,Y (m/s)
Definition: lightweight_geom_data.h:2152
mrpt::math::TTwist2D::vy
double vy
Definition: lightweight_geom_data.h:2152
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::target_vel_y
double target_vel_y
Definition: CVehicleSimul_Holo.h:66
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
kinematics-precomp.h
mrpt::kinematics::CVehicleSimulVirtualBase::resetTime
void resetTime()
Reset all simulator variables to 0 (except the.
Definition: CVehicleSimulVirtualBase.cpp:88
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd
Definition: CVehicleSimul_Holo.h:61
mrpt::kinematics::CVehicleSimul_Holo::internal_clear
void internal_clear() override
Resets all pending cmds.
Definition: CVehicleSimul_Holo.cpp:72
mrpt::kinematics::CVehicleSimul_Holo::internal_simulControlStep
void internal_simulControlStep(const double dt) override
Definition: CVehicleSimul_Holo.cpp:23
CVehicleSimul_Holo.h
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::issue_time
double issue_time
time when the cmd was issued.
Definition: CVehicleSimul_Holo.h:65
mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
Definition: CVehicleSimulVirtualBase.h:124
mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry
mrpt::math::TPose2D m_odometry
Definition: CVehicleSimulVirtualBase.h:125
mrpt::kinematics
Definition: CKinematicChain.h:19
mrpt::kinematics::CVehicleSimul_Holo::TVelRampCmd::init_vel
mrpt::math::TTwist2D init_vel
Definition: CVehicleSimul_Holo.h:67
mrpt::sign
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: core/include/mrpt/core/bits_math.h:68
mrpt::math::TTwist2D::omega
double omega
Angular velocity (rad/s)
Definition: lightweight_geom_data.h:2154
mrpt::kinematics::CVehicleSimulVirtualBase::m_time
double m_time
simulation running time
Definition: CVehicleSimulVirtualBase.h:118
mrpt::kinematics::CVehicleSimul_Holo::sendVelRampCmd
void sendVelRampCmd(double vel, double dir, double ramp_time, double rot_speed)
Sends a velocity cmd to the holonomic robot.
Definition: CVehicleSimul_Holo.cpp:73
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST