VehicleAckermann_ControllerTwistFrontSteerPID.cpp
Go to the documentation of this file.
1 /*+-------------------------------------------------------------------------+
2  | MultiVehicle simulator (libmvsim) |
3  | |
4  | Copyright (C) 2014 Jose Luis Blanco Claraco (University of Almeria) |
5  | Copyright (C) 2017 Borys Tymchenko (Odessa Polytechnic University) |
6  | Distributed under GNU General Public License version 3 |
7  | See <http://www.gnu.org/licenses/> |
8  +-------------------------------------------------------------------------+ */
9 
11 #include "xml_utils.h"
12 
13 using namespace mvsim;
14 using namespace std;
15 
17  DynamicsAckermann& veh)
18  : ControllerBase(veh),
19  setpoint_lin_speed(0),
20  setpoint_ang_speed(0),
21  KP(100),
22  KI(0),
23  KD(0),
24  max_torque(100.0)
25 {
26  // Get distance between wheels:
28  m_veh.m_wheels_info[WHEEL_FL].y - m_veh.m_wheels_info[WHEEL_FR].y;
29  m_r2f_L = m_veh.m_wheels_info[WHEEL_FL].x - m_veh.m_wheels_info[WHEEL_RL].x;
30  ASSERT_(m_dist_fWheels > 0.0);
31  ASSERT_(m_r2f_L > 0.0);
32 }
33 
34 // See base class docs
38 {
39  // For each wheel:
40  // 1) Compute desired velocity set-point (in m/s)
41  // 2) Run the PI/PID for that wheel independently (in newtons)
42 
43  // 1st: desired steering angle:
44  // --------------------------------
45  if (setpoint_ang_speed == 0)
46  {
47  co.steer_ang = 0.0;
48  }
49  else
50  {
51  const double R = setpoint_lin_speed / setpoint_ang_speed;
52  co.steer_ang = atan(m_r2f_L / R);
53  }
54 
55  // Desired velocities for each wheel:
56  std::vector<mrpt::math::TPoint2D>
57  desired_wheel_vels; // In local vehicle frame
58  m_veh.getWheelsVelocityLocal(
59  desired_wheel_vels, vec3(setpoint_lin_speed, 0.0, setpoint_ang_speed));
60 
61  ASSERT_(desired_wheel_vels.size() == 4);
62 
63  // Rotate to obtain the actual desired longitudinal velocity for each wheel:
64  // FL:
65  double vel_fl, vel_fr;
66  double desired_fl_steer_ang, desired_fr_steer_ang;
67  m_veh.computeFrontWheelAngles(
68  co.steer_ang, desired_fl_steer_ang, desired_fr_steer_ang);
69  {
70  const mrpt::poses::CPose2D wRotInv(0, 0, -desired_fl_steer_ang);
71  mrpt::math::TPoint2D vel_w;
72  wRotInv.composePoint(
73  desired_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w);
74  vel_fl = vel_w.x;
75  }
76  {
77  const mrpt::poses::CPose2D wRotInv(0, 0, -desired_fr_steer_ang);
78  mrpt::math::TPoint2D vel_w;
79  wRotInv.composePoint(
80  desired_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w);
81  vel_fr = vel_w.x;
82  }
83 
84  // Compute each wheel actual velocity (from an "odometry" estimation of
85  // velocity, not ground-truth!):
86  double act_vel_fl, act_vel_fr;
87  {
88  std::vector<mrpt::math::TPoint2D>
89  odo_wheel_vels; // In local vehicle frame
90  m_veh.getWheelsVelocityLocal(
91  odo_wheel_vels, m_veh.getVelocityLocalOdoEstimate());
92  ASSERT_(odo_wheel_vels.size() == 4);
93 
94  const double actual_fl_steer_ang =
95  m_veh.getWheelInfo(DynamicsAckermann::WHEEL_FL).yaw;
96  const double actual_fr_steer_ang =
97  m_veh.getWheelInfo(DynamicsAckermann::WHEEL_FR).yaw;
98 
99  {
100  const mrpt::poses::CPose2D wRotInv(0, 0, -actual_fl_steer_ang);
101  mrpt::math::TPoint2D vel_w;
102  wRotInv.composePoint(
103  odo_wheel_vels[DynamicsAckermann::WHEEL_FL], vel_w);
104  act_vel_fl = vel_w.x;
105  }
106  {
107  const mrpt::poses::CPose2D wRotInv(0, 0, -actual_fr_steer_ang);
108  mrpt::math::TPoint2D vel_w;
109  wRotInv.composePoint(
110  odo_wheel_vels[DynamicsAckermann::WHEEL_FR], vel_w);
111  act_vel_fr = vel_w.x;
112  }
113  }
114 
115  // Apply controller:
116  for (int i = 0; i < 2; i++)
117  {
118  m_PID[i].KP = KP;
119  m_PID[i].KI = KI;
120  m_PID[i].KD = KD;
121  m_PID[i].max_out = max_torque;
122  }
123 
124  co.rl_torque = .0;
125  co.rr_torque = .0;
126  co.fl_torque = -m_PID[0].compute(
127  vel_fl - act_vel_fl,
128  ci.context.dt); // "-" because \tau<0 makes robot moves forwards.
129  co.fr_torque = -m_PID[1].compute(vel_fr - act_vel_fr, ci.context.dt);
130 }
131 
133  const rapidxml::xml_node<char>& node)
134 {
135  std::map<std::string, TParamEntry> params;
136  params["KP"] = TParamEntry("%lf", &KP);
137  params["KI"] = TParamEntry("%lf", &KI);
138  params["KD"] = TParamEntry("%lf", &KD);
139  params["max_torque"] = TParamEntry("%lf", &max_torque);
140 
141  // Initial speed.
142  params["V"] = TParamEntry("%lf", &this->setpoint_lin_speed);
143  params["W"] = TParamEntry("%lf_deg", &this->setpoint_ang_speed);
144 
145  parse_xmlnode_children_as_param(node, params);
146 }
147 
149  const TeleopInput& in, TeleopOutput& out)
150 {
152 
153  switch (in.keycode)
154  {
155  case 'W':
156  case 'w':
157  setpoint_lin_speed += 0.1;
158  break;
159 
160  case 'S':
161  case 's':
162  setpoint_lin_speed -= 0.1;
163  break;
164 
165  case 'A':
166  case 'a':
167  setpoint_ang_speed += 1.0 * M_PI / 180.0;
168  break;
169 
170  case 'D':
171  case 'd':
172  setpoint_ang_speed -= 1.0 * M_PI / 180.0;
173  break;
174 
175  case ' ':
176  setpoint_lin_speed = .0;
177  setpoint_ang_speed = .0;
178  break;
179  };
180  out.append_gui_lines += "[Controller=" + string(class_name()) +
181  "] Teleop keys: w/s=incr/decr lin speed. "
182  "a/d=left/right steering. spacebar=stop.\n";
183  out.append_gui_lines += mrpt::format(
184  "setpoint: v=%.03f w=%.03f deg/s\n", setpoint_lin_speed,
185  setpoint_ang_speed * 180.0 / M_PI);
186 }
double max_torque
Maximum abs. value torque (for clamp) [Nm].
virtual void control_step(const DynamicsAckermann::TControllerInput &ci, DynamicsAckermann::TControllerOutput &co)
double setpoint_ang_speed
desired velocities (m/s) and (rad/s)
double dt
timestep
Definition: basic_types.h:57
virtual void teleop_interface(const TeleopInput &in, TeleopOutput &out) override
void parse_xmlnode_children_as_param(const rapidxml::xml_node< char > &xml_node, const std::map< std::string, TParamEntry > &params, const char *function_name_context="")
Definition: xml_utils.cpp:196
double max_out
For clamping (0=no clamp)
double compute(double err, double dt)
double steer_ang
Equivalent ackerman steering angle.


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 19:36:40