Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-20 14:38:18 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'TrajPlan' of https://github.com/Wetmelon/ODrive into traptraj

Browse Source
This commit is contained in:
Oskar Weigl
2018-09-21 21:51:01 -07:00
parent 7cd292bae3 8392f69b1c
commit 05e87cfc5b
11 changed files with 431 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+4
View File
@@ -3,6 +3,10 @@ Please add a note of your changes below this heading if you make a Pull Request.
# Unreleased
## Added
* Trapezoidal Trajectory Planner
# Releases
## [0.4.4] - 2018-09-18
### Fixed
Firmware/MotorControl/axis.cpp
+5 -2
View File
@@ -11,18 +11,21 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor)
Motor& motor,
TrapezoidalTrajectory& trap)
: hw_config_(hw_config),
config_(config),
encoder_(encoder),
sensorless_estimator_(sensorless_estimator),
controller_(controller),
motor_(motor)
motor_(motor),
trap_(trap)
{
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
}
static void step_cb_wrapper(void* ctx) {
Firmware/MotorControl/axis.hpp
+5 -2
View File
@@ -65,7 +65,8 @@ public:
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor);
Motor& motor,
TrapezoidalTrajectory& trap);
void setup();
void start_thread();
@@ -150,6 +151,7 @@ public:
SensorlessEstimator& sensorless_estimator_;
Controller& controller_;
Motor& motor_;
TrapezoidalTrajectory& trap_;
osThreadId thread_id_;
volatile bool thread_id_valid_ = false;
@@ -188,7 +190,8 @@ public:
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
make_protocol_object("encoder", encoder_.make_protocol_definitions()),
make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions())
make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
make_protocol_object("trap_traj", trap_.make_protocol_definitions())
);
}
};
Firmware/MotorControl/controller.cpp
+32 -2
View File
@@ -44,6 +44,19 @@ void Controller::set_current_setpoint(float current_setpoint) {
#endif
}
void Controller::move_to_pos(float goal_point) {
planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_,
vel_setpoint_, axis_->trap_.config_.vel_limit,
axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit);
config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL;
TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f);
pos_setpoint_ = myTraj.Y;
vel_setpoint_ = myTraj.Yd;
current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A;
planned_move_timer_ = axis_->loop_counter_ * current_meas_period;
}
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) {
@@ -82,7 +95,24 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
// Only runs if anticogging_.calib_anticogging is true; non-blocking
anticogging_calibration(pos_estimate, vel_estimate);
float anticogging_pos = pos_estimate;
// Controlled Move
if (config_.control_mode >= CTRL_MODE_PLANNED_MOVE_CONTROL) {
float time_now = axis_->loop_counter_ * current_meas_period;
if ((time_now - planned_move_timer_) > planned_move_end_time_) {
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
vel_setpoint_ = 0.0f;
current_setpoint_ = 0.0f;
} else {
TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_);
pos_setpoint_ = myTraj.Y;
vel_setpoint_ = myTraj.Yd;
current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A;
}
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
}
// Position control
// TODO Decide if we want to use encoder or pll position here
float vel_des = vel_setpoint_;
@@ -103,7 +133,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
// We get the current position and apply a current feed-forward
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(static_cast<int>(pos_estimate), axis_->encoder_.config_.cpr)];
Iq += anticogging_.cogging_map[mod(static_cast<int>(anticogging_pos), axis_->encoder_.config_.cpr)];
}
float v_err = vel_des - vel_estimate;
Firmware/MotorControl/controller.hpp
+15 -4
View File
@@ -11,7 +11,8 @@ typedef enum {
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3
CTRL_MODE_POSITION_CONTROL = 3,
CTRL_MODE_PLANNED_MOVE_CONTROL = 4
} Motor_control_mode_t;
struct ControllerConfig_t {
@@ -31,6 +32,9 @@ public:
void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward);
void set_vel_setpoint(float vel_setpoint, float current_feed_forward);
void set_current_setpoint(float current_setpoint);
// Trajectory-Planned control
void move_to_pos(float goal_point);
// TODO: make this more similar to other calibration loops
void start_anticogging_calibration();
@@ -71,6 +75,9 @@ public:
float vel_integrator_current_ = 0.0f; // [A]
float current_setpoint_ = 0.0f; // [A]
float planned_move_timer_ = 0.0f;
float planned_move_end_time_ = 0.0f;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
@@ -86,11 +93,15 @@ public:
make_protocol_property("vel_limit", &config_.vel_limit)
),
make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint,
"pos_setpoint", "vel_feed_forward", "current_feed_forward"),
"pos_setpoint",
"vel_feed_forward",
"current_feed_forward"),
make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
"vel_setpoint", "current_feed_forward"),
"vel_setpoint",
"current_feed_forward"),
make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
"current_setpoint"),
"current_setpoint"),
make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"),
make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
);
}
Firmware/MotorControl/main.cpp
+7 -1
View File
@@ -14,6 +14,7 @@ SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
ControllerConfig_t controller_configs[AXIS_COUNT];
MotorConfig_t motor_configs[AXIS_COUNT];
AxisConfig_t axis_configs[AXIS_COUNT];
TrapTrajConfig_t trap_configs[AXIS_COUNT];
bool user_config_loaded_;
SystemStats_t system_stats_ = { 0 };
@@ -26,6 +27,7 @@ typedef Config<
SensorlessEstimator::Config_t[AXIS_COUNT],
ControllerConfig_t[AXIS_COUNT],
MotorConfig_t[AXIS_COUNT],
TrapTrajConfig_t[AXIS_COUNT],
AxisConfig_t[AXIS_COUNT]> ConfigFormat;
void save_configuration(void) {
@@ -35,6 +37,7 @@ void save_configuration(void) {
&sensorless_configs,
&controller_configs,
&motor_configs,
&trap_configs,
&axis_configs)) {
//printf("saving configuration failed\r\n"); osDelay(5);
} else {
@@ -51,6 +54,7 @@ void load_configuration(void) {
&sensorless_configs,
&controller_configs,
&motor_configs,
&trap_configs,
&axis_configs)) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
@@ -59,6 +63,7 @@ void load_configuration(void) {
sensorless_configs[i] = SensorlessEstimator::Config_t();
controller_configs[i] = ControllerConfig_t();
motor_configs[i] = MotorConfig_t();
trap_configs[i] = TrapTrajConfig_t();
axis_configs[i] = AxisConfig_t();
}
} else {
@@ -162,8 +167,9 @@ int odrive_main(void) {
Motor *motor = new Motor(hw_configs[i].motor_config,
hw_configs[i].gate_driver_config,
motor_configs[i]);
TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor);
*encoder, *sensorless_estimator, *controller, *motor, *trap);
}
// Start ADC for temperature measurements and user measurements
Firmware/MotorControl/odrive_main.h
+2
View File
@@ -109,9 +109,11 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <trapTraj.hpp>
#include <axis.hpp>
#include <communication/communication.h>
#endif // __cplusplus
Firmware/MotorControl/trapTraj.cpp
+102
View File
@@ -0,0 +1,102 @@
#include <math.h>
#include "odrive_main.h"
// Standard sign function, implemented to match the Python impelmentation
template <typename T>
int sign(T val) {
if (val == T(0))
return T(0);
else
return (std::signbit(val)) ? -1 : 1;
}
TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t &config) : config_(config) {}
float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi,
float Vi, float Vmax,
float Amax, float Dmax) {
float dx_stop = (Vi * Vi) / (Dmax * 2.0f);
float dX = Xf - Xi;
int s = sign(dX);
float Ar = s * Amax; // Maximum Acceleration (signed)
float Dr = -1.0f * s * Dmax; // Maximum Deceleration (signed)
float Vr = s * Vmax; // Maximum Velocity (signed)
float Ta;
float Tv;
float Td;
// Checking for overshoot on "minimum stop"
if (fabs(dX) <= dx_stop) {
Ta = 0;
Tv = 0;
Vr = Vi;
Dr = -1.0f * sign(Vi) * Dmax;
Td = fabs(Vi) / Dmax;
} else {
// Handle the case where initial velocity > Max velocity
if ((s * Vi) > (s * Vr)) {
Ar = -1.0f * s * Amax;
}
Ta = (Vr - Vi) / Ar; // Acceleration time
Td = (-Vr) / Dr; // Deceleration time
// Peak Velocity handling
float dXmin = Ta * (Vr + Vi) / 2.0f + Td * Vr / 2.0f;
// Short move handling
if (fabs(dX) < fabs(dXmin)) {
Vr = s * sqrt((-((Vi * Vi) / Ar) - 2.0f * dX) / (1.0f / Dr - 1.0f / Ar));
Ta = std::max(0.0f, (Vr - Vi) / Ar);
Tv = 0;
Td = std::max(0.0f, -Vr / Dr);
} else {
Tv = (dX - dXmin) / Vr;
}
}
// Populate object's values
Xf_ = Xf;
Xi_ = Xi;
Vi_ = Vi;
Ar_ = Ar;
Dr_ = Dr;
Vr_ = Vr;
Ta_ = Ta;
Tv_ = Tv;
Td_ = Td;
yAccel_ = (Ar * Ta * Ta) / 2.0f + (Vi * Ta) + Xi;
Tav_ = Ta + Tv;
return Ta + Tv + Td;
}
TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) {
TrapTrajStep_t trajStep;
if (t < 0.0f) { // Initial Conditions
trajStep.Y = Xi_;
trajStep.Yd = Vi_;
trajStep.Ydd = Ar_;
} else if (t < Ta_) { // Accelerating
trajStep.Y = (Ar_ * (t * t) / 2.0f) + (Vi_ * t) + Xi_;
trajStep.Yd = (Ar_ * t) + Vi_;
trajStep.Ydd = Ar_;
} else if (t < Ta_ + Tv_) { // Coasting
trajStep.Y = yAccel_ + (Vr_ * (t - Ta_));
trajStep.Yd = Vr_;
trajStep.Ydd = 0;
} else if (t < Ta_ + Tv_ + Td_) { // Deceleration
float Tdc = t - Tav_;
trajStep.Y = yAccel_ + (Vr_ * (t - Ta_)) + Dr_ * (Tdc * Tdc) / 2.0f;
trajStep.Yd = Vr_ + Dr_ * Tdc;
trajStep.Ydd = Dr_;
}
return trajStep;
}
Firmware/MotorControl/trapTraj.hpp
+55
View File
@@ -0,0 +1,55 @@
#ifndef _TRAP_TRAJ_H
#define _TRAP_TRAJ_H
struct TrapTrajConfig_t {
float vel_limit = 20000.0f;
float accel_limit = 5000.0f;
float decel_limit = 5000.0f;
float cpss_to_A = 0.0f;
};
struct TrapTrajStep_t {
float Y;
float Yd;
float Ydd;
};
class TrapezoidalTrajectory {
public:
Axis *axis_ = nullptr; // set by Axis constructor
TrapTrajConfig_t &config_;
TrapezoidalTrajectory(TrapTrajConfig_t &config);
float planTrapezoidal(float Xf, float Xi,
float Vi, float Vmax,
float Amax, float Dmax);
TrapTrajStep_t evalTrapTraj(float t);
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_object("config",
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("accel_limit", &config_.accel_limit),
make_protocol_property("decel_limit", &config_.decel_limit)));
}
private:
float yAccel_;
float Xi_;
float Xf_;
float Vi_;
float Ar_;
float Dr_;
float Vr_;
float Ta_;
float Tv_;
float Td_;
float Tav_;
};
#endif
Firmware/Tupfile.lua
+1
View File
@@ -155,6 +155,7 @@ build{
'MotorControl/encoder.cpp',
'MotorControl/controller.cpp',
'MotorControl/sensorless_estimator.cpp',
'MotorControl/trapTraj.cpp',
'MotorControl/main.cpp',
'communication/communication.cpp',
'communication/ascii_protocol.cpp',
tools/Motion Planning/FIR_Planner.py
+203
View File
@@ -0,0 +1,203 @@
# Copyright (c) 2018 Paul Guénette
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
import numpy as np
import math
import matplotlib.pyplot as plt
import random
# Symbol Description
# Ta, Tv and Td Duration of the stages of the AL profile
# q0f , v0f and a0f Initial conditions of the jerk-limited trajectory
# q0 and v0 Adapted initial conditions for the AL profile
# qe Position set-point
# s Direction (sign) of the trajectory
# vmax, amax, dmax and jmax Kinematic bounds
# vr, ar and dr Reached values of velocity and acceleration
# Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time)
def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax):
dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance
dX = Xf - Xi # Distance to travel
s = np.sign(dX) # Sign of travel direction
Ar = s*Amax # Maximum Acceleration (signed)
Dr = -s*Dmax # Maximum Deceleration (signed)
Vr = s*Vmax # Maximum Velocity (signed)
if abs(dX) <= dX_stop: # Check for an overshoot condition (decelerate only)
Ta = 0
Tv = 0
Vr = Vi
Dr = -np.sign(Vi)*Dmax
Td = abs(Vi) / Dmax
print("Overshoot Move:")
print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop))
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(
Xf, Xi, Vi, Vmax, Amax))
print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td))
print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr))
print()
else:
# Correct initial acceleration direction if needed
if s*Vi > s*Vr:
Ar = -s*Amax
Ta = (Vr - Vi)/Ar # Acceleration Time
Td = -Vr/Dr # Deceleration Time
# Peak velocity handling
dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0
# Short move handling
if abs(dX) < abs(dXmin):
print("Short Move:")
print("dX: {:.3f}\tdXmin: {:.3f}".format(dX, dXmin))
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(
Xf, Xi, Vi, Vmax, Amax))
print("Ta: {:.3f}\tTd: {:.3f}\tVr: {:.3f}".format(Ta, Td, Vr))
print()
Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar))
Ta = max(0, (Vr - Vi)/Ar)
Tv = 0
Td = max(0, -Vr/Dr)
else:
Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v
# We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory
# Create the time series and preallocate the position, velocity, and acceleration arrays
t_traj = np.linspace(0, Ta+Tv+Td, 10000)
y = [None]*len(t_traj)
yd = [None]*len(t_traj)
ydd = [None]*len(t_traj)
# We only know acceleration (Ar and Dr), so we integrate to create
# the velocity and position curves
y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi
Tav = Ta + Tv
for i in range(len(t_traj)):
t = t_traj[i]
if(t < 0): # Initial conditions
y[i] = Xi
yd[i] = Vi
ydd[i] = Ar
elif(t < Ta): # Acceleration
y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi
yd[i] = (Ar * t) + Vi
ydd[i] = Ar
elif(t < Ta+Tv): # Coasting
y[i] = y_Accel + (Vr * (t - Ta))
yd[i] = Vr
ydd[i] = 0
elif(t <= Ta+Tv+Td): # Deceleration
Tdc = t - Tav
y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((Tdc)*(Tdc))/2
yd[i] = Vr + Dr*(Tdc)
ydd[i] = Dr
return (y, yd, ydd, t_traj)
numRows = 2
numCols = 2
fig, axes = plt.subplots(numRows, numCols)
random.seed()
for x in range(numRows*numCols):
# Vmax = random.uniform(0.1, 20)
# Amax = random.uniform(0.1, 4)
# Dmax = Amax
# Xf = random.uniform(-100.0, 100.0)
# Xi = random.uniform(-100.0, 100.0)
# Vi = random.uniform(-Vmax*2, Vmax*2)
Vmax = 100000.0
Amax = 100000.0
Dmax = Amax
Xf = 0
Xi = 1000000
Vi = 0
(Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax)
if(abs(Xf-Xi) <= Vi**2 / (2*Dmax)):
print("Overshoot: ", Xf)
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(
Xf, Xi, Vi, Vmax, Amax))
print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format(
Y[-1], Yd[-1], Ydd[-1]))
print()
(Y2, Yd2, Ydd2, t2) = FIR_trapPlan(Xf, Y[-1], Yd[-1], Vmax, Amax, Dmax)
Y.extend(Y2)
Yd.extend(Yd2)
Ydd.extend(Ydd2)
t2 = t2 + t[-1]
t = np.append(t, t2, axis=0)
if(abs(Xf-Y[-1]) > 0.0001):
print("Bad Final Position")
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(
Xf, Xi, Vi, Vmax, Amax))
print()
# plt.figure()
# plt.subplot(2,1,1)
# plt.plot(t, Y)
# plt.plot(t, Yd)
# plt.plot(t[-1], Xf, 'b*')
# plt.plot(t[-1], 0, 'r*')
# plt.subplot(2,1,2)
# plt.plot(t, Ydd)
# plt.show()
elif(abs(Yd[-1]) > 0.0001):
print("Bad Final Velocity")
print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(
Xf, Xi, Vi, Vmax, Amax))
print()
# plt.figure()
# plt.plot(t, Y)
# plt.plot(t, Yd)
# # plt.plot(t, Ydd)
# plt.show()
ax1 = axes[int(x/numCols), x % numCols]
ax1.plot(t, Y)
ax1.plot(t, Yd)
ax1.plot(t[-1], Xf, 'b*')
ax1.plot(t[-1], 0, 'r*')
# ax2 = ax1.twinx()
# ax2.plot(t, Ydd, color='tab:green')
# ax2.tick_params(axis='y', labelcolor='tab:green')
dX = abs(Xf - Y[-1])
dV = abs(0 - Yd[-1])
axes[int(x/numCols), x % numCols].set_title(
'Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV))
plt.show()