[Bug] Stop.NONE can have unexpected kinematics in curve() due to dynamics constraint

[laurensvalk]

Describe the bug
Stop.NONE makes consecutive motor movements continuous. In case of curve(), it makes both the rotary and forward motion continuous.

This means that when you exit a curve into a straight, the angular velocity is maintained until it decelerates to zero, and vice versa.

This is intentional to reduce slip, but it means the straight() command does not produce a straight line (although it does recover to a straight path after absorbing the excess angular velocity).

To reproduce
This was reported by @afarago

from pybricks.hubs import EssentialHub
from pybricks.pupdevices import Motor, ColorSensor, ColorLightMatrix
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

hub = EssentialHub()

bal_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
jobb_motor = Motor(Port.B)
robot_bazis = DriveBase(bal_motor, jobb_motor, wheel_diameter=43, axle_track=86)
robot_bazis.settings(straight_speed=800, straight_acceleration=200, turn_acceleration=400)

for x in range(4):
    robot_bazis.straight(100, Stop.NONE)
    robot_bazis.curve(43, 90, Stop.NONE)
robot_bazis.straight(100)

Expected behavior
Kinematically correct curves and straights.

An easy solution would be to continue only one of angular or forward velocity. This allows the user to get kinematically correct paths, perhaps at the price of some slip due to the instantaneous discontinuity in speed reference. This could be done by accepting a tuple then=(Stop.NONE, Stop.HOLD) or variations thereof, which can be passed to pbio as is.

The proper solution might be to produce a spline or bezier that has the correct endpoints equivalent to the kinematic curve, but is allowed to deviate from the curve in between to accommodate for excess angular velocity. This is out of scope for now 🤓

[laurensvalk]

I can reproduce it with

try:
    from pybricks.pupdevices import Motor
except ImportError:
    from pybricks.ev3devices import Motor
from pybricks.tools import wait
from pybricks.parameters import Port, Direction, Stop
from pybricks.robotics import DriveBase
from pybricks import version

print(version)

# Initialize default "Driving Base" with medium motors and wheels.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
drive_base = DriveBase(left_motor, right_motor, wheel_diameter=43, axle_track=86)
drive_base.settings(straight_speed=600, straight_acceleration=200, turn_acceleration=400)

# Allocate logs for motors and controller signals.
DURATION = 20000
DIV = 4
left_motor.log.start(DURATION, DIV)
right_motor.log.start(DURATION, DIV)
drive_base.distance_control.log.start(DURATION, DIV)
drive_base.heading_control.log.start(DURATION, DIV)

# Drive straight forward and back again.
for x in range(4):
    drive_base.straight(100, Stop.NONE)
    drive_base.curve(43, 90, Stop.NONE)

# Wait so we can also log hold capability, then turn off the motor completely.
wait(100)
drive_base.stop()

# Transfer data logs.
print("Transferring data...")
left_motor.log.save("servo_left.txt")
right_motor.log.save("servo_right.txt")
drive_base.distance_control.log.save("control_distance.txt")
drive_base.heading_control.log.save("control_heading.txt")
print("Done")

[laurensvalk]

This is a tricky issue.

A partial fix committed above solves the main bug: with multiple synchronized trajectories we can't time-shift a trajectory (which we sometimes do for better performance in tight loops). This commit disables that optimization for drivebases.

But as I was digging into this, I realized there is another subtle more conceptual issue. Between calling any two commands there is always a bit of time. During that time, a drive base with Stop.NONE will have moved a degree or two. Since turn/straight commands are all incremental, this causes a few degree of error to add up. A run_target equivalent for Drivebases would solve this, but that is out of scope for now. EDIT: But users can easily make this, so this is probably good enough.

We could do the same trick as we use for Smart Coast/Brake, but that still leaves the robot having to get rid of its excess speed left over from the previous maneuver as discussed above.

So maybe we should leave it at the fix proposed here, and revisit proper splines/beziers or absolute turns for a future API.

[laurensvalk]

A run_target equivalent for Drivebases would solve this, but that is out of scope for now. EDIT: But users can easily make this, so this is probably good enough.

With two patches underway, this script drives a smooth, proper square, which would be even better with gyro included:

from pybricks.hubs import EssentialHub
from pybricks.pupdevices import Motor, ColorSensor, ColorLightMatrix
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

hub = EssentialHub()

bal_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
jobb_motor = Motor(Port.B)
robot_bazis = DriveBase(bal_motor, jobb_motor, wheel_diameter=43, axle_track=87.4)
print(robot_bazis.heading_control.scale)
robot_bazis.settings(straight_speed=500, straight_acceleration=200, turn_acceleration=400)
for i in range(1, 5):
    robot_bazis.straight(100, Stop.NONE)
    robot_bazis.curve(43, i * 90 - robot_bazis.angle(), Stop.NONE)
robot_bazis.straight(100)

[afarago]

Many thanks!
Awesome use of current angle, I was not aware of it before.

I have retried the same with an InventorHub and standard medium motors -- unfortunately anything above straight_speed=100 results in a wacky and unpredictable movement.

from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor, ColorSensor, ColorLightMatrix
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

hub = InventorHub()

left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
drive_base = DriveBase(
    left_motor,
    right_motor,
    wheel_diameter=56,
    axle_track=114)

print(drive_base.heading_control.scale, drive_base.settings())
# drive_base.settings(straight_speed=500, straight_acceleration=200, turn_acceleration=400)
drive_base.settings(straight_speed=300, straight_acceleration=100)
for i in range(1, 5):
    drive_base.straight(200, Stop.NONE)
    # print(drive_base.angle(), i * 90 - drive_base.angle())
    drive_base.curve(43, (i * 90 - drive_base.angle()), Stop.NONE)
drive_base.straight(200)

[laurensvalk]

Which firmware is that on? The fixes introduced here are not in the main release yet. To find out, run a script with:

from pybricks import version
print(version)

To try the fixes introduced in this thread:

• Go here: https://github.com/pybricks/pybricks-micropython/actions/runs/4466696811
• Scroll down and download the firmware file for your hub
• In Pybricks Code, start installing the firmware as usual
• On the hub selection tab, click Advanced and follow the instructions to select the zip file you just downloaded

[afarago]

I have tried with two different hubs/versions with same results.

('primehub', '3.3.0b2', 'v3.3.0b2 on 2023-03-08')
('primehub', '3.3.0b2', 'v3.3.0b2-50-g461803d4 on 2023-03-16')

[laurensvalk]

Yes, and this issue was fixed on March 20 :)

So you could try it with the file linked above if you like.

[afarago]

You are right, sorry for my ignorace.
Testing with ('primehub', '3.3.0b2', 'v3.3.0b2-65-gc0a09a13 on 2023-03-20') yields to much better results, still introduces two "oddities":

1. oddity: after1. - oddity: after curve there is a "hard stop" (video1)
2. oddity: after turn there is a hard stop (video2)
3. awesome: after curve (with right axle_track) turn is perfectly timed (video3)
4. oddity: should not it be (video3) the same as in video1/version1?

from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor, ColorSensor, ColorLightMatrix
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks import version
print(version)

hub = InventorHub()

left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)
drive_base = DriveBase(
    left_motor,
    right_motor,
    wheel_diameter=56,
    axle_track=114)

print(drive_base.heading_control.scale, drive_base.settings())
drive_base.settings(straight_speed=500, straight_acceleration=200, turn_acceleration=400)
for i in range(1, 5):
    drive_base.straight(200, Stop.NONE)
    # drive_base.curve(43, i * 90 - drive_base.angle(), Stop.NONE)  # video 1
    # drive_base.turn(i * 90 - drive_base.angle(), Stop.NONE)       # video 2
    drive_base.curve(56, i * 90 - drive_base.angle(), Stop.NONE)    # video 3
drive_base.straight(200, Stop.HOLD)
wait(5000)

version1 - curve with smaller axle track then drivebase
https://user-images.githubusercontent.com/4489389/226733575-a61093c2-c458-459b-b682-326b72975aed.mp4

version2 - turn
https://user-images.githubusercontent.com/4489389/226733652-cd243204-1566-44db-b7fc-833888e117c3.mp4

version3 - curve with same axle track as drivebase
https://user-images.githubusercontent.com/4489389/226733716-2ece4723-bfea-4e19-aa1a-64d8b4fdc4dd.mp4

Probably this might even be a neverending kinematics journey, so from my perspective code v3 seems to bring the expected results perfectly.

[laurensvalk]

One thing I've noticed is that your acceleration values are very low.

A limitation of Stop.NONE right now is that it works best if the target speed is actually reachable before movement completes. With a higher acceleration (or a longer move), it is usually reachable.

Can you run the program with higher accelerations or just using the defaults?

[laurensvalk]

Version 1:

[laurensvalk]

Version 2

[laurensvalk]

Version 3

[laurensvalk]

After reviewing your three cases in some detail with the help of the graphs above, I think this is the expected behavior.

Let's start with version 2, where the code is:

for i in range(1, 5):
    drive_base.straight(200, Stop.NONE)
    drive_base.turn(i * 90 - drive_base.angle(), Stop.NONE)

This is asking the robot to go straight and not stop at the end, and then immediately do an in-place turn.

An in-place turn means driving forward by 0 mm. But it has all this excess speed coming in to this move, so this manifests as an abrupt stop.

If you are curious, you can see this happen in the control_distance graph of this experiment: The target speed does indeed drop to 0 during the curve, as intended.

Now for version 3

With a large enough radius, more distance is driven throughout the curve. This means enough time is available to obtain and maintain the desired turn and drive speed.

Now for version 1

This is somewhere in between the two cases; you can still see the drive speed in the curve having to go to almost zero to achieve the small-radius turn. This curve is so small that one wheel still has to go backwards to achieve it.

---

All-in-all, the robot drives the expected result in all cases, and comes out in a nice square. It's basically just that Stop.NONE is really only beneficial for moves where there is time and space to not stop.

Does that make sense?

Perhaps the analogy is driving on the road. You can keep driving fast along a slowly-curved highway, but you probably want to slow down and possibly stop at a small junction on a city street.

[afarago]

This does make sense, I think the issue could be closed based on the changes - if you agree.
Many thanks for the improvements!