Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add command queue to servo to account for latency #2594

Merged
merged 58 commits into from
Jan 3, 2024
Merged

Add command queue to servo to account for latency #2594

merged 58 commits into from
Jan 3, 2024

Commits on Dec 17, 2023

  1. add command queue to servo to account for latency

    @pac48
    pac48 committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    8d48c76 View commit details
    Browse the repository at this point in the history

  2. run pre-commit

    @pac48
    pac48 committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    2babc8d View commit details
    Browse the repository at this point in the history

  3. fix unsigned compare

    @pac48
    pac48 committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    65d1a59 View commit details
    Browse the repository at this point in the history

  4. Update moveit_ros/moveit_servo/config/servo_parameters.yaml 

    Fix wording

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
    @pac48 @sea-bass
    pac48 and sea-bass committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    6324d4c View commit details
    Browse the repository at this point in the history

  5. Update moveit_ros/moveit_servo/src/servo.cpp 

    Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
    @pac48 @sea-bass
    pac48 and sea-bass committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    04f6f76 View commit details
    Browse the repository at this point in the history

  6. Update moveit_ros/moveit_servo/src/servo.cpp 

    Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
    @pac48 @sea-bass
    pac48 and sea-bass committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    0cc8001 View commit details
    Browse the repository at this point in the history

  7. add comments and change variable names

    @pac48
    pac48 committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    b83de4c View commit details
    Browse the repository at this point in the history

  8. add checks to determine what state information should be published 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    2d325c0 View commit details
    Browse the repository at this point in the history

  9. change latency parameter name 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 17, 2023
    Configuration menu
    Copy the full SHA
    55c76bc View commit details
    Browse the repository at this point in the history

Commits on Dec 18, 2023

  1. factor command queue out of servo instance 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    0d5e75d View commit details
    Browse the repository at this point in the history

  2. update demos 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    242161c View commit details
    Browse the repository at this point in the history

  3. needs clean up but working well

    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    8bbdfa0 View commit details
    Browse the repository at this point in the history

  4. deal with duplicate timestamps for sim

    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    b6e30ac View commit details
    Browse the repository at this point in the history

  5. add acceleration limiting smoothing

    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    9bb7ca3 View commit details
    Browse the repository at this point in the history

  6. add timeout check in filter 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    eb080f5 View commit details
    Browse the repository at this point in the history

  7. factor out robot state from servo call 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    06c7429 View commit details
    Browse the repository at this point in the history

  8. update comments in smoothing pluin 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    536ffd5 View commit details
    Browse the repository at this point in the history

  9. fix tests 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    6a18b44 View commit details
    Browse the repository at this point in the history

  10. change velocity calculation to make interpolation not overshoot 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    9a82923 View commit details
    Browse the repository at this point in the history

  11. Update moveit_ros/moveit_servo/config/panda_simulated_config.yaml 

    Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
    @pac48 @sea-bass
    pac48 and sea-bass committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    ddf171d View commit details
    Browse the repository at this point in the history

  12. Update moveit_ros/moveit_servo/config/servo_parameters.yaml 

    Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
    @pac48 @sea-bass
    pac48 and sea-bass committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    6d1c46e View commit details
    Browse the repository at this point in the history

  13. Update moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp 

    Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
    @pac48 @sea-bass
    pac48 and sea-bass committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    207ce40 View commit details
    Browse the repository at this point in the history

  14. fix time calculation 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    8982c7b View commit details
    Browse the repository at this point in the history

  15. add check to ensure time interval is positive 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    69399a1 View commit details
    Browse the repository at this point in the history

  16. simplify demos 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    853d53b View commit details
    Browse the repository at this point in the history

  17. wait for first robot state before starting servo loop 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    37fadd5 View commit details
    Browse the repository at this point in the history

  18. add comments to acceleration filter 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 18, 2023
    Configuration menu
    Copy the full SHA
    e36eeb0 View commit details
    Browse the repository at this point in the history

Commits on Dec 20, 2023

  1. fix wait time units

    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    b7ac97f View commit details
    Browse the repository at this point in the history

  2. fix logic bug in smoothHalt and remove stopping point from trajectory… 

    … message. Still some overshoot.

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    6b38b6b View commit details
    Browse the repository at this point in the history

  3. add acceleration limit to servo 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    3d17cfa View commit details
    Browse the repository at this point in the history

  4. remove acceleration filter 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    2f50898 View commit details
    Browse the repository at this point in the history

  5. Merge branch 'main' into pr-servo-command-queue

    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    c4e94f4 View commit details
    Browse the repository at this point in the history

  6. remove other filter files from moveit_core 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    c10e0e8 View commit details
    Browse the repository at this point in the history

  7. add doc string and basic clean up

    @pac48
    pac48 committed Dec 20, 2023
    Configuration menu
    Copy the full SHA
    b02099f View commit details
    Browse the repository at this point in the history

Commits on Dec 21, 2023

  1. refactor getRobotState to utils and add a test 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 21, 2023
    Configuration menu
    Copy the full SHA
    9ab22d0 View commit details
    Browse the repository at this point in the history

  2. make some things const and fix comments 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 21, 2023
    Configuration menu
    Copy the full SHA
    d41ac16 View commit details
    Browse the repository at this point in the history

  3. use joint_limts params to get robot acceleration limits 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 21, 2023
    Configuration menu
    Copy the full SHA
    c5140d1 View commit details
    Browse the repository at this point in the history

  4. update demo config and set velocities in demos 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 21, 2023
    Configuration menu
    Copy the full SHA
    cc03bc6 View commit details
    Browse the repository at this point in the history

Commits on Dec 23, 2023

  1. fix acceleration calculation 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 23, 2023
    Configuration menu
    Copy the full SHA
    99d6abf View commit details
    Browse the repository at this point in the history

  2. apply collision_velocity_scale_ in smooth hault, add comments, and re… 

    …name variables

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 23, 2023
    Configuration menu
    Copy the full SHA
    dabbf36 View commit details
    Browse the repository at this point in the history

  3. use bounds on scaling factors in [0... 1] 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 23, 2023
    Configuration menu
    Copy the full SHA
    254c70b View commit details
    Browse the repository at this point in the history

  4. remove joint_acceleration parameter 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 23, 2023
    Configuration menu
    Copy the full SHA
    4091c8b View commit details
    Browse the repository at this point in the history

  5. add test for jointLimitAccelerationScaling 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 23, 2023
    Configuration menu
    Copy the full SHA
    2e266b8 View commit details
    Browse the repository at this point in the history

Commits on Dec 26, 2023

  1. refactor velocity and acceleration scaling into common function 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 26, 2023
    Configuration menu
    Copy the full SHA
    4b1aab2 View commit details
    Browse the repository at this point in the history

  2. general clean up, add comments, fix parameters, set timestamp in upda… 

    …teSlidingWindow, etc.

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 26, 2023
    Configuration menu
    Copy the full SHA
    d9365ce View commit details
    Browse the repository at this point in the history

Commits on Dec 27, 2023

  1. Update moveit_ros/moveit_servo/config/panda_simulated_config.yaml 

    Co-authored-by: AndyZe <andyz@utexas.edu>
    @pac48 @AndyZe
    pac48 and AndyZe committed Dec 27, 2023
    Configuration menu
    Copy the full SHA
    cff0b14 View commit details
    Browse the repository at this point in the history

  2. Update moveit_ros/moveit_servo/src/servo.cpp 

    Co-authored-by: AndyZe <andyz@utexas.edu>
    @pac48 @AndyZe
    pac48 and AndyZe committed Dec 27, 2023
    Configuration menu
    Copy the full SHA
    f873e0c View commit details
    Browse the repository at this point in the history

  3. remove override_acceleration_scaling_factor 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 27, 2023
    Configuration menu
    Copy the full SHA
    09d92f9 View commit details
    Browse the repository at this point in the history

  4. fix variable name 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 27, 2023
    Configuration menu
    Copy the full SHA
    7181671 View commit details
    Browse the repository at this point in the history

  5. enable use_smoothing in demos 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 27, 2023
    Configuration menu
    Copy the full SHA
    d523204 View commit details
    Browse the repository at this point in the history

Commits on Dec 28, 2023

  1. Update moveit_ros/moveit_servo/config/panda_simulated_config.yaml 

    Co-authored-by: AndyZe <andyz@utexas.edu>
    @pac48 @AndyZe
    pac48 and AndyZe committed Dec 28, 2023
    Configuration menu
    Copy the full SHA
    7f54978 View commit details
    Browse the repository at this point in the history

  2. add current state to command queue if it is empty. This is needed sin… 

    …ce the time stamp is set in the updateSlidingWindow function now.

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 28, 2023
    Configuration menu
    Copy the full SHA
    72d4f92 View commit details
    Browse the repository at this point in the history

  3. remove acceleration smoothing 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 28, 2023
    Configuration menu
    Copy the full SHA
    37e590f View commit details
    Browse the repository at this point in the history

  4. revert jointLimitVelocityScalingFactor refactor 

    Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 28, 2023
    Configuration menu
    Copy the full SHA
    89b094f View commit details
    Browse the repository at this point in the history

Commits on Dec 29, 2023

  1. 1) fix spelling 2) add comments 3) make sure rolling_window always ha… 

    …s current state if no command generated 4) fix smooth hault: stop command was not generated if smoothing disabled 5) call resetSmoothing when there are no commands

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
    @pac48
    pac48 committed Dec 29, 2023
    Configuration menu
    Copy the full SHA
    1acf7e4 View commit details
    Browse the repository at this point in the history

  2. Merge branch 'main' into pr-servo-command-queue

    @pac48
    pac48 committed Dec 29, 2023
    Configuration menu
    Copy the full SHA
    8e4d205 View commit details
    Browse the repository at this point in the history

Commits on Jan 3, 2024

  1. Merge branch 'main' into pr-servo-command-queue

    @sea-bass
    sea-bass committed Jan 3, 2024
    Configuration menu
    Copy the full SHA
    3217e03 View commit details
    Browse the repository at this point in the history

  2. Merge branch 'main' into pr-servo-command-queue

    @sea-bass
    sea-bass committed Jan 3, 2024
    Configuration menu
    Copy the full SHA
    279925f View commit details
    Browse the repository at this point in the history