search

pantor / ruckig

Motion Generation for Robots and Machines. Real-time. Jerk-constrained. Time-optimal.
https://ruckig.com
MIT License
730 stars 165 forks source link

no smooth transition between trajectory segments despite using DurationDiscretization.Discrete #95

Closed mqnc closed 2 years ago

mqnc commented 2 years ago

While I was working on this bug report, I discovered that ruckig has an offline trajectory mode which solves all my problems in life. Still, I think this is a bug and tracking it down will make the world a better place.

I want to piece together several trajectories but there were always jumps in the transition points. I attached an example. Sorry the code is so redundant but this was the easiest way to generate the problem from my cpp source.

I noticed that it's always just one parameter that has a problem. I suspect that it's the critical one that determines the duration and you just forgot to slow this one down to the next biggest multiple of dt.

from copy import copy
import matplotlib.pyplot as plt
import numpy as np
from ruckig import Ruckig, InputParameter, OutputParameter, Result
import ruckig

def sample(
        startPosition,
        startVelocity,
        startAcceleration,
        targetPosition,
        targetVelocity,
        targetAcceleration,
        maxVelocity,
        maxAcceleration,
        maxJerk
):
    otg = Ruckig(6, 0.016)
    inp = InputParameter(6)
    out = OutputParameter(6)

    inp.current_position = startPosition
    inp.current_velocity = startVelocity
    inp.current_acceleration = startAcceleration
    inp.target_position = targetPosition
    inp.target_velocity = targetVelocity
    inp.target_acceleration = targetAcceleration
    inp.max_velocity = maxVelocity
    inp.max_acceleration = maxAcceleration
    inp.max_jerk = maxJerk

    inp.synchronization = ruckig.Synchronization.Time
    inp.duration_discretization = ruckig.DurationDiscretization.Discrete

    first_output = None
    traj = [startPosition]
    res = Result.Working
    while res == Result.Working:
        res = otg.update(inp, out)
        out.pass_to_input(inp)
        traj.append(out.new_position)
        if not first_output:
            first_output = copy(out)

    return traj

curve = [[-0.1, -1.2, 2, -0.5, -4.71238898038469, -6.2]]

curve.extend(sample(
    startPosition=[-0.1, -1.2, 2, -0.5, -4.71238898038469, -6.2],
    startVelocity=[0, 0, 0, 0, 0, 0],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[1.9679526741215, -1.79337540810325, 1.60165402119475, -
                    0.742383751689976, -4.42451465459919, -4.63004355951186],
    targetVelocity=[1.4216077453218, -0.373745261217006, -0.436991883620502, -
                    0.162165540016975, 0.194020004947316, 1.48964658466271],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.9998, 1.9998, 1.9998, 1.9998, 1.9998, 1.9998],
    maxAcceleration=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999],
    maxJerk=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999]
)[1:])

curve.extend(sample(
    startPosition=[1.9679526741215, -1.79337540810325, 1.60165402119475, -
                   0.742383751689976, -4.42451465459919, -4.63004355951186],
    startVelocity=[1.4216077453218, -0.373745261217006, -0.436991883620502, -
                   0.162165540016975, 0.194020004947316, 1.48964658466271],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[3.20696349927192, -2.08539432042343, 1.0597792047688, -
                    0.879318138009455, -4.2592428371458, -2.92672678259001],
    targetVelocity=[0.168259905193136, 0.0943548076762705, -
                    0.756673465817308, -0.0389809012756666, 0.053669544440146, 1.9998],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.9998, 1.9998, 1.9998, 1.9998, 1.9998, 1.9998],
    maxAcceleration=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999],
    maxJerk=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999]
)[1:])

curve.extend(sample(
    startPosition=[3.20696349927192, -2.08539432042343, 1.0597792047688, -
                   0.879318138009455, -4.2592428371458, -2.92672678259001],
    startVelocity=[0.168259905193136, 0.0943548076762705, -
                   0.756673465817308, -0.0389809012756666, 0.053669544440146, 1.9998],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[1.45326481801535, -1.31742729867851, 0.019058231384651, -
                    0.739446876167513, -4.41053543358465, -0.657551568169395],
    targetVelocity=[-1.2738654730322, 0.552247753120335, -0.746013018925092,
                    0.0861790508936229, -0.0912516905522356, 1.64275856771295],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.9998, 1.9998, 1.9998, 1.9998, 1.9998, 1.9998],
    maxAcceleration=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999],
    maxJerk=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999]
)[1:])

curve.extend(sample(
    startPosition=[1.45326481801535, -1.31742729867851, 0.019058231384651, -
                   0.739446876167513, -4.41053543358465, -0.657551568169395],
    startVelocity=[-1.2738654730322, 0.552247753120335, -0.746013018925092,
                   0.0861790508936229, -0.0912516905522356, 1.64275856771295],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[-0.347408286286064, -0.544807171803238, -
                    1.02121623310491, -0.639709764838941, -4.51282721276351, 1.65663416188154],
    targetVelocity=[-0.131147264269121, 0.0474046878466293, -0.0794483917298527, -
                    0.0342416475861322, 0.043019682952083, 0.231353015772312],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.9998, 1.9998, 1.9998, 1.9998, 1.9998, 1.9998],
    maxAcceleration=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999],
    maxJerk=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999]
)[1:])

curve.extend(sample(
    startPosition=[-0.347408286286064, -0.544807171803238, -
                   1.02121623310491, -0.639709764838941, -4.51282721276351, 1.65663416188154],
    startVelocity=[-0.131147264269121, 0.0474046878466293, -0.0794483917298527, -
                   0.0342416475861322, 0.043019682952083, 0.231353015772312],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[-1.22558666866157, -0.467723029280449, -
                    1.65303434911015, -1.99401835626587, -2.85676592942331, 4.90815758566065],
    targetVelocity=[-0.0302929179039969, 0.00122893787795177, -
                    0.036558151261666, -0.0906247865274197, 0.233332981936197, 0.163309169738774],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.9998, 1.9998, 1.9998, 1.9998, 1.9998, 1.9998],
    maxAcceleration=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999],
    maxJerk=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999]
)[1:])

curve.extend(sample(
    startPosition=[-1.22558666866157, -0.467723029280449, -
                   1.65303434911015, -1.99401835626587, -2.85676592942331, 4.90815758566065],
    startVelocity=[-0.0302929179039969, 0.00122893787795177, -
                   0.036558151261666, -0.0906247865274197, 0.233332981936197, 0.163309169738774],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[-0.695659037205651, -0.540991535969191, -
                    1.5479518907819, -1.99815965046869, -0.559781546793431, 3.90287656149631],
    targetVelocity=[0.18436164148335, -0.0262808807303888, 0.0205449108473776, -
                    0.00528270363536876, 0.710840601527282, -0.28020145530023],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[0.7992, 0.7992, 0.7992, 0.7992, 0.7992, 0.7992],
    maxAcceleration=[3.996, 3.996, 3.996, 3.996, 3.996, 3.996],
    maxJerk=[3.996, 3.996, 3.996, 3.996, 3.996, 3.996]
)[1:])

curve.extend(sample(
    startPosition=[-0.695659037205651, -0.540991535969191, -
                   1.5479518907819, -1.99815965046869, -0.559781546793431, 3.90287656149631],
    startVelocity=[0.18436164148335, -0.0262808807303888, 0.0205449108473776, -
                   0.00528270363536876, 0.710840601527282, -0.28020145530023],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[-0.0874527817812409, -0.628561776808307, -
                    1.49779199890686, -2.01981404720537, 1.68814742972457, 3.05499779185952],
    targetVelocity=[0.354347411744906, -0.0547171036838394,
                    0.0119193699908037, -0.0178981162799585, 1.19928006, -0.404346458254636],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.9998, 1.9998, 1.9998, 1.9998, 1.9998, 1.9998],
    maxAcceleration=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999],
    maxJerk=[9.999, 9.999, 9.999, 9.999, 9.999, 9.999]
)[1:])

curve.extend(sample(
    startPosition=[-0.0874527817812409, -0.628561776808307, -
                   1.49779199890686, -2.01981404720537, 1.68814742972457, 3.05499779185952],
    startVelocity=[0.354347411744906, -0.0547171036838394,
                   0.0119193699908037, -0.0178981162799585, 1.19928006, -0.404346458254636],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[0.640183128398815, -0.750769855691266, -
                    1.51940658643282, -2.07063582344055, 3.85679424946687, 2.46344067814976],
    targetVelocity=[0.171744687193063, -0.0584947649324608, -0.0205105546487691, -
                    0.0271773718891913, 0.274610306393505, 0.0745633868515529],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.1994, 1.1994, 1.1994, 1.1994, 1.1994, 1.1994],
    maxAcceleration=[5.997, 5.997, 5.997, 5.997, 5.997, 5.997],
    maxJerk=[5.997, 5.997, 5.997, 5.997, 5.997, 5.997]
)[1:])

curve.extend(sample(
    startPosition=[0.640183128398815, -0.750769855691266, -
                   1.51940658643282, -2.07063582344055, 3.85679424946687, 2.46344067814976],
    startVelocity=[0.171744687193063, -0.0584947649324608, -0.0205105546487691, -
                   0.0271773718891913, 0.274610306393505, 0.0745633868515529],
    startAcceleration=[0, 0, 0, 0, 0, 0],
    targetPosition=[3.04159265358979, -1.94159265358979, -
                    2, -2.64159265358979, 4.71238898038469, 6.2],
    targetVelocity=[0, 0, 0, 0, 0, 0],
    targetAcceleration=[0, 0, 0, 0, 0, 0],
    maxVelocity=[1.1994, 1.1994, 1.1994, 1.1994, 1.1994, 1.1994],
    maxAcceleration=[5.997, 5.997, 5.997, 5.997, 5.997, 5.997],
    maxJerk=[5.997, 5.997, 5.997, 5.997, 5.997, 5.997]
)[1:])

plt.plot(np.diff(curve, axis=0))
plt.show()
mqnc commented 2 years ago

image

pantor commented 2 years ago

Thanks for spotting this! It is now fixed with the latest commit in the master branch.

There was indeed a bug with the time synchronization of the limiting DoF in case of discrete durations.