Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-02-05 22:52:02 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

rewrite cpp trajectory module to match python

Browse Source
This commit is contained in:
Oskar Weigl
2018-09-23 22:00:00 -07:00
parent 0796a5d5ac
commit 4b6943e401
5 changed files with 102 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Firmware/LICENSE
View File

@@ -1,6 +1,6 @@
The MIT License (MIT)
Copyright (c) 2016 Oskar Weigl (madcowswe)
Copyright (c) 2016-2018 Oskar Weigl
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal

144
Firmware/MotorControl/trapTraj.cpp
View File

@@ -1,101 +1,93 @@
#include <math.h>
#include "odrive_main.h"
#include "utils.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;
// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {
return (std::signbit(val)) ? -1.0f : 1.0f;
}
TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t &config) : config_(config) {}
// Symbol Description
// Ta, Tv and Td Duration of the stages of the AL profile
// Xi and Vi Adapted initial conditions for the AL profile
// Xf Position set-point
// s Direction (sign) of the trajectory
// Vmax, Amax, Dmax and jmax Kinematic bounds
// Ar, Dr and Vr Reached values of acceleration and velocity
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);
TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t& config) : config_(config) {}
float Ar = s * Amax; // Maximum Acceleration (signed)
float Dr = -1.0f * s * Dmax; // Maximum Deceleration (signed)
float Vr = s * Vmax; // Maximum Velocity (signed)
bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
float Vmax, float Amax, float Dmax) {
float dX = Xf - Xi; // Distance to travel
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
Ar_ = s * Amax; // Maximum Acceleration (signed)
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
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;
}
// If we start with a speed faster than cruising, then we need to decel instead of accel
// aka "double deceleration move" in the paper
if ((s * Vi) > (s * Vr_)) {
Ar_ = -s * Amax;
}
// Populate object's values
// Time to accel/decel to/from Vr (cruise speed)
Ta_ = (Vr_ - Vi) / Ar_;
Td_ = -Vr_ / Dr_;
Xf_ = Xf;
// Integral of velocity ramps over the full accel and decel times to get
// minimum displacement required to reach cuising speed
float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
// Are we displacing enough to reach cruising speed?
if (s*dX < s*dXmin) {
// Short move (triangle profile)
Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
Td_ = std::max(0.0f, -Vr_ / Dr_);
Tv_ = 0.0f;
} else {
// Long move (trapezoidal profile)
Tv_ = (dX - dXmin) / Vr_;
}
// Fill in the rest of the values used at evaluation-time
Tf_ = Ta_ + Tv_ + Td_;
Xi_ = Xi;
Xf_ = Xf;
Vi_ = Vi;
yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
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;
return true;
}
TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) {
TrapTrajStep_t trajStep;
if (t < 0.0f) { // Initial Conditions
trajStep.Y = Xi_;
trajStep.Yd = Vi_;
trajStep.Ydd = Ar_;
if (t < 0.0f) { // Initial Condition
trajStep.Y = Xi_;
trajStep.Yd = Vi_;
trajStep.Ydd = 0.0f;
} else if (t < Ta_) { // Accelerating
trajStep.Y = (Ar_ * (t * t) / 2.0f) + (Vi_ * t) + Xi_;
trajStep.Yd = (Ar_ * t) + Vi_;
trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
trajStep.Yd = Vi_ + Ar_*t;
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.Y = yAccel_ + Vr_*(t - Ta_);
trajStep.Yd = Vr_;
trajStep.Ydd = 0.0f;
} else if (t < Tf_) { // Deceleration
float td = t - Tf_;
trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td);
trajStep.Yd = Dr_*td;
trajStep.Ydd = Dr_;
} else if (t >= Tf_) { // Final Condition
trajStep.Y = Xf_;
trajStep.Yd = 0.0f;
trajStep.Ydd = 0.0f;
} else {
// TODO: report error here
}
return trajStep;

35
Firmware/MotorControl/trapTraj.hpp
View File

@@ -2,10 +2,10 @@
#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;
float vel_limit = 20000.0f; // [count/s]
float accel_limit = 5000.0f; // [count/s^2]
float decel_limit = 5000.0f; // [count/s^2]
float cpss_to_A = 0.0f; // [A/(count/s^2)]
};
struct TrapTrajStep_t {
@@ -16,15 +16,9 @@ struct TrapTrajStep_t {
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);
TrapezoidalTrajectory(TrapTrajConfig_t& config);
bool planTrapezoidal(float Xf, float Xi, float Vi,
float Vmax, float Amax, float Dmax);
TrapTrajStep_t evalTrapTraj(float t);
auto make_protocol_definitions() {
@@ -32,24 +26,29 @@ class TrapezoidalTrajectory {
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)));
make_protocol_property("decel_limit", &config_.decel_limit),
make_protocol_property("cpss_to_A", &config_.cpss_to_A)
)
);
}
private:
float yAccel_;
Axis* axis_ = nullptr; // set by Axis constructor
TrapTrajConfig_t& config_;
float Xi_;
float Xf_;
float Vi_;
float Ar_;
float Dr_;
float Vr_;
float Dr_;
float Ta_;
float Tv_;
float Td_;
float Tav_;
float Tf_;
float yAccel_;
};
#endif

2
Firmware/MotorControl/utils.h
View File

@@ -63,6 +63,8 @@ extern "C" {
#define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y))
#define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y))
#define SQ(x) ((x) * (x))
static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
static const float sqrt3_by_2 = 0.86602540378f;

28
tools/motion_planning/PlanTrap.py
View File

@@ -28,15 +28,13 @@ 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)
# Symbol Description
# Ta, Tv and Td Duration of the stages of the AL profile
# Xi and Vi Adapted initial conditions for the AL profile
# Xf Position set-point
# s Direction (sign) of the trajectory
# Vmax, Amax, Dmax and jmax Kinematic bounds
# Ar, Dr and Vr Reached values of acceleration and velocity
# Test scales:
pos_range = 10000.0
@@ -57,18 +55,18 @@ def PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax):
# If we start with a speed faster than cruising, then we need to decel instead of accel
# aka "double deceleration move" in the paper
if s*Vi > s*Vr:
Ar = -s*Amax
print("Handbrake!")
Ar = -s*Amax
# Time to accel/decel to/from Vr (cruise speed)
Ta = (Vr - Vi)/Ar
Ta = (Vr-Vi)/Ar
Td = -Vr/Dr
# Integrate velocity ramps over the full accel and decel times to get
# Integral of velocity ramps over the full accel and decel times to get
# minimum displacement required to reach cuising speed
dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0
dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0
# Did we displace enough to reach cruising speed?
# Are we displacing enough to reach cruising speed?
if s*dX < s*dXmin:
print("Short Move:")
# From paper:
@@ -126,6 +124,8 @@ def EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf):
y[i] = Xf
yd[i] = 0
ydd[i] = 0
else:
raise ValueError("t = {} is outside of considered range".format(t))
dy = np.diff(y)
dy_max = np.max(np.abs(dy))