Skip to content

Commit

Permalink
Improve the semantics of non-zero target velocities
Browse files Browse the repository at this point in the history 
When acceleration or velocity limits were enabled, the previous
semantics were:

 "be at velocity v_f when you reach x_f"

These are challenging semantics to work with, especially if the
initial velocity is non-zero.  For instance, if you were traveling at
+20Hz at x=10, and the new target was v=20Hz, x=9.9999, the resulting
trajectory would have to come to a stop, accelerate backwards, then
ramp up speed in order to reach 20Hz again while passing through
9.9999.

The new semantics are:

 "match the trajectory defined by x=x_f + t * v_f"

This provides a more natural interpretation of the arguments, is
more useful in practice, and requires fewer ISR cycles.
  • Loading branch information
@jpieper
jpieper committed Jun 15, 2023
1 parent b7f3908 commit 36095ff
Show file tree
Hide file tree
Showing 5 changed files with 89 additions and 92 deletions.
32 changes: 13 additions & 19 deletions docs/reference.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1641,25 +1641,19 @@ limits are as follows:
at the given velocity indefinitely, or until the command stop
position is reached.

- *Only velocity limit set*: In this case, until the desired position
is reached, the control velocity will be set to either the positive
or negative velocity limit. Once the desired position has been
reached, then the velocity will continue indefinitely at the command
velocity.

- *Both set*: In this case, neither the command position nor the
command velocity are immediately applied. Instead, the control
velocity is advanced by the configured acceleration each timestep
while being limited to the maximum configured velocity in order to
achieve the desired position and velocity. Once the desired
position and velocity have been achieved, then that final velocity
is continued indefinitely. Note that this may require "back
tracking" if the current and final velocities do not permit an
approach in a single pass.

- *Only acceleration limit set*: This behaves like the "both set"
case, except that the control velocity is allowed to increase or
decrease arbitrarily.
- *Either set*: If x_c is the command position, v_c is the command
velocity, and t is the time from receipt of the command, the
semantics can be described as: "match the trajectory defined by x =
x_c + v_c * t".

If the acceleration limit is set, the above is effected by
commanding accelerations of either [-accel_limit, 0, accel_limit].
If an acceleration limit is not set, then the velocity will change
instantaneously.

If the velocity limit is set, then the intermediate velocities will
obey "-velocity_limit < velocity < +velocity_limit". If it is not
set, then the velocities may grow to arbitrary magnitude.

NOTE: This is limited internally to be no more than
`servo.max_velocity`.
Expand Down
9 changes: 9 additions & 0 deletions fw/bldc_servo.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -385,6 +385,15 @@ class BldcServo::Impl {
}
}

// If we have a velocity command and velocity_limit, ensure that
// the command does not violate the limit.
if (!std::isnan(next->velocity_limit) &&
!std::isnan(next->velocity)) {
next->velocity = Limit(next->velocity,
-next->velocity_limit,
next->velocity_limit);
}

// Transform any position and stop_position command into the
// relative raw space.
const auto delta = static_cast<int64_t>(
Expand Down
65 changes: 23 additions & 42 deletions fw/bldc_servo_position.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,43 +100,15 @@ class BldcServoPosition {
return std::copysign(a, -v0);
}

if (vf != 0.0f) {
// A moving target. First check to see if we are moving
// towards it in the right direction and can accelerate.
if (v0 * vf > 0.0f && // in the right direction
v0 * dx > 0.0f) { // towards it
// How much distance do we need to change to the target
// velocity?
const float vel_change_distance =
std::abs(v0 * dv) / a - dv * dv / (2.0f * a);
if (std::abs(dx) > vel_change_distance) {
if (std::isnan(data->velocity_limit) ||
std::abs(v0) < data->velocity_limit) {
// We accelerate slightly faster so we complete in a
// numerically stable way.
return std::copysign(1.001f * a, dx);
} else {
return 0.0f;
}
} else {
return std::copysign(a, -v0);
}
}
}
// Perform all operations in the target reference frame,
// i.e. we'll transform our frame so that the target velocity is
// 0.

if (vf != 0.0f) {
// We have a non-zero velocity target that we have to
// backtrack for. Aim for the point necessary to get
// there from a stop.
const float dist = vf * vf / (2.0f * a);
dx += std::copysign(dist, -vf);
}
const auto v_frame = v0 - vf;

// Assume we have a stationary target.
if ((v0 * dx) >= 0.0f) {
// The target is stationary and we are moving towards it
// or stationary and there is sufficient distance.
const float decel_distance = (v0 * v0) / (2.0f * a);
if ((v_frame * dx) >= 0.0f && dx != 0.0f) {
// The target is stationary and we are moving towards it.
const float decel_distance = (v_frame * v_frame) / (2.0f * a);
if (std::abs(dx) >= decel_distance) {
if (std::isnan(data->velocity_limit) ||
std::abs(v0) < data->velocity_limit) {
Expand All @@ -145,12 +117,12 @@ class BldcServoPosition {
return 0.0f;
}
} else {
return std::copysign(a, -v0);
return std::copysign(a, -v_frame);
}
}

// We are moving away. Try to fix that.
return std::copysign(a, -v0);
return std::copysign(a, -v_frame);
}

static void DoVelocityAndAccelLimits(
Expand Down Expand Up @@ -293,11 +265,20 @@ class BldcServoPosition {
// scale at a 40kHz switching frequency. 1.2 million RPM should
// be enough for anybody?
const float step = velocity_command / rate_hz;
status->control_position_raw =
(*status->control_position_raw +
(static_cast<int64_t>(
static_cast<int32_t>((static_cast<float>(1ll << 32) * step))) <<
16));
const int64_t int64_step =
(static_cast<int64_t>(
static_cast<int32_t>((static_cast<float>(1ll << 32) * step))) <<
16);
status->control_position_raw = *status->control_position_raw + int64_step;

if (data->position_relative_raw && !std::isnan(velocity)) {
const float tstep = velocity / rate_hz;
const int64_t tint64_step =
(static_cast<int64_t>(
static_cast<int32_t>((static_cast<float>(1ll << 32) * tstep))) <<
16);
data->position_relative_raw = *data->position_relative_raw + tint64_step;
}

if (std::isfinite(config->max_position_slip)) {
const int64_t current_position = position->position_relative_raw;
Expand Down
63 changes: 37 additions & 26 deletions fw/test/bldc_servo_position_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,9 +69,9 @@ struct Context {
}

float Call() {
if (!std::isnan(data.position)) {
if (!std::isnan(data.position) && !data.position_relative_raw) {
data.position_relative_raw = MotorPosition::FloatToInt(data.position);
} else {
} else if (std::isnan(data.position)) {
data.position_relative_raw.reset();
}
return BldcServoPosition::UpdateCommand(
Expand Down Expand Up @@ -350,16 +350,16 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
{ 10.0, 1.0, 5.0, 0.0, NaN, 1.0, 40, 5.000, 5.000 },
{ 10.0,-1.0, 5.0, 0.0, NaN, 1.0, 40, 5.000, 5.000 },

{ 0.0, 0.0, 5.0, 0.5, NaN, 1.0, 40, 5.000, 5.000 },
{ 0.0, 0.0, 5.0, 0.5, NaN, 1.0, 40, 10.000, 10.000 },
{ 0.0, 0.0, 5.0, 0.0, NaN, 2.0, 40, 2.500, 2.500 },
{ 4.0, 0.0, 5.0, 0.0, NaN, 1.0, 40, 1.000, 1.000 },

/////////////////////////////////
// No velocity limit.
{ 0.0, 0.0, 5.0, 0.0, 1.0, NaN, 40, 0.000, 4.514 },
{ 0.0, 1.0, 5.0, 0.0, 1.0, NaN, 40, 0.000, 3.730 },
{ 0.0, 1.0, 5.0, 1.5, 1.0, NaN, 40, 0.000, 2.647 },
{ 0.0, -1.0, -5.0,-1.5, 1.0, NaN, 40, 0.000, 2.647 },
{ 0.0, 1.0, 5.0, 1.5, 1.0, NaN, 40, 0.000, 5.025 },
{ 0.0, -1.0, -5.0,-1.5, 1.0, NaN, 40, 0.000, 5.025 },
{ 5.0, 0.0, 0.0, 0.0, 1.0, NaN, 40, 0.000, 4.514 },
{ 5.0, 0.0, 0.0, 0.0, 2.0, NaN, 40, 0.000, 3.205 },

Expand All @@ -377,27 +377,28 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
{ 0.3, 4.0, 3.0, 0.0, 2.0, 0.7, 40, 1.504, 4.207 },

// non-zero final velocity
{ 0.0, 0.0, 3.0, 0.5, 1.0, 0.5, 40, 6.751, 6.250 },
{ 0.0, 0.0, 3.0, 0.3, 1.0, 0.5, 40, 5.592, 6.290 },
{ 0.0, 0.0, 3.0, 0.5, 1.0, 0.8, 40, 10.119, 11.217 },
{ 0.0, 0.0, 3.0, 0.3, 1.0, 0.8, 40, 5.592, 6.912 },

// A command velocity that exceeds the limit.
{ 0.0, 0.0, 3.0, 1.0, 1.0, 0.5, 40, 6.751, 6.250 },
{ 0.0, 0.0, -3.0, -1.0, 1.0, 0.5, 40, 6.751, 6.250 },
// A command velocity that exceeds the limit. Note, this will
// never complete as it is not possible to catch up.
{ 0.0, 0.0, 3.0, 1.0, 1.0, 0.5, 40, 21.501, NaN },
{ 0.0, 0.0, -3.0, -1.0, 1.0, 0.5, 40, 21.501, NaN },

// non-zero that requires looping back
{ 0.0, 0.0, 0.0, 0.5, 1.0, 0.5, 40, 1.001, 1.207 },
{-0.03, 0.5, 0.0, 0.3, 1.0, 0.5, 40, 0.000975, 1.548 },
// non-zero targets
{ 0.0, 0.0, 0.0, 0.5, 1.0, 0.6, 40, 1.152, 1.850 },
{-0.03, 0.5, 0.0, 0.3, 1.0, 0.6, 40, 0.000975, 0.2474 },
// The same as the previous, but shifted to be near the wraparound
// point and at a lower PWM rate to maximize numerical problems.
{3275.97, 0.5, 3276.0, 0.3, 1.0, 0.5, 40, 0.000975, 1.548 },
{32765.97, 0.5, 32766.0, 0.3, 1.0, 0.5, 40, 0.000975, 1.550 },
{3275.97, 0.5, 3276.0, 0.3, 1.0, 0.5, 15, 0.000933, 1.548 },
{32765.97, 0.5, 32766.0, 0.3, 1.0, 0.5, 15, 0.000933, 1.550 },
{3275.97, 0.5, 3276.0, 0.3, 1.0, 0.6, 40, 0.000975, 0.2474 },
{32765.97, 0.5, 32766.0, 0.3, 1.0, 0.6, 40, 0.000975, 0.2441 },
{3275.97, 0.5, 3276.0, 0.3, 1.0, 0.6, 15, 0.000933, 0.2474 },
{32765.97, 0.5, 32766.0, 0.3, 1.0, 0.6, 15, 0.000933, 0.2441 },

// Actually wrap around.
{32767.98, 0.5, -32767.99, 0.3, 1.0, 0.5, 15, 0.000933, 1.550 },
// // Actually wrap around.
{32767.98, 0.5, -32767.99, 0.3, 1.0, 0.6, 15, 0.000933, 0.2441 },

{ 0.0, 0.0, 0.0, -0.5, 1.0, 0.5, 40, 1.001, 1.207 },
{ 0.0, 0.0, 0.0, -0.5, 1.0, 0.6, 40, 1.152, 1.850 },

};

Expand Down Expand Up @@ -452,7 +453,8 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
const int64_t extra_count = extra_time * ctx.rate_hz;

const int64_t max_count =
(2.0 + test_case.expected_total_duration) * ctx.rate_hz;
(2.0 + (std::isnan(test_case.expected_total_duration) ?
20.0 : test_case.expected_total_duration)) * ctx.rate_hz;

for (int64_t i = 0; i < max_count; i++) {
ctx.Call();
Expand Down Expand Up @@ -516,7 +518,8 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
if (std::isfinite(test_case.xf)) {
BOOST_TEST(ctx.from_raw(
ctx.status.control_position_raw.value()) ==
test_case.xf);
test_case.xf +
test_case.vf * test_case.expected_total_duration);
}
total_duration = current_duration;
}
Expand All @@ -527,16 +530,24 @@ BOOST_AUTO_TEST_CASE(AccelVelocityLimits, * boost::unit_test::tolerance(1e-3)) {
old_pos = this_pos;
}

if (std::isfinite(test_case.xf)) {
if (std::isfinite(test_case.xf) &&
std::isfinite(test_case.expected_total_duration)) {
const double expected_final =
test_case.xf + expected_vf * extra_time;
test_case.xf +
test_case.expected_total_duration * test_case.vf +
expected_vf * extra_time;
BOOST_TEST(ctx.from_raw(
ctx.status.control_position_raw.value()) == expected_final);
}
BOOST_TEST(ctx.status.control_velocity.value() == expected_vf);
BOOST_TEST(ctx.status.trajectory_done == true);
BOOST_TEST(ctx.status.trajectory_done ==
std::isfinite(test_case.expected_total_duration));

BOOST_TEST(total_duration == test_case.expected_total_duration);
if (std::isfinite(test_case.expected_total_duration)) {
BOOST_TEST(total_duration == test_case.expected_total_duration);
} else {
BOOST_TEST(total_duration == 0.0);
}
BOOST_TEST(coast_duration == test_case.expected_coast_duration);
}
}
Expand Down
12 changes: 7 additions & 5 deletions utils/dynamometer_drive.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,7 +221,7 @@ class ServoStatsReader {
// Here we verify that the final and total timer are always valid.
if (result.final_timer == 0 ||
result.total_timer == 0 ||
result.final_timer > 3850 ||
result.final_timer > 3750 ||
result.total_timer < 5000) {
throw mjlib::base::system_error::einval(
fmt::format("Invalid timer final={} total={}",
Expand Down Expand Up @@ -2184,7 +2184,7 @@ class Application {
// have out of band knowledge that this results in the worst
// possible loop timing.
co_await dut_->Command(
fmt::format("d pos 3.0 -0.5 {} v0.5 a0.2", options_.max_torque_Nm));
fmt::format("d pos 3.0 -0.5 {} v0.7 a0.2", options_.max_torque_Nm));

const double step_s = 0.1;
const int loops = 14 / step_s;
Expand All @@ -2202,13 +2202,15 @@ class Application {
const double fixture_velocity =
options_.transducer_scale *
fixture_->servo_stats().velocity;
const double target_pos = 3.0 - time_s * 0.5;

fixture_position_history.insert(
std::make_pair(time_s, fixture_position));
fixture_velocity_history.insert(
std::make_pair(time_s, fixture_velocity));

if (std::abs(fixture_position - 3.0) < 0.2 &&
if (std::abs(fixture_velocity + 0.5) < 0.2 &&
std::abs(fixture_position - target_pos) < 0.2 &&
done_time == 0.0) {
done_time = time_s;
}
Expand Down Expand Up @@ -2254,9 +2256,9 @@ class Application {
}
}

if (std::abs(done_time - 7.2) > 1.0) {
if (std::abs(done_time - 4.8) > 1.0) {
throw mjlib::base::system_error::einval(
fmt::format("Took wrong amount of time {} != 7.2", done_time));
fmt::format("Took wrong amount of time {} != 4.8", done_time));
}

co_await dut_->Command("d stop");
Expand Down

0 comments on commit 36095ff

Please sign in to comment.