Implemented spline interpolation in joint space #103

urmahp · 2022-06-09T07:35:05Z

No description provided.

Use a constant for stopj time and make sure the robot actually stops when trying to stop the servoj thread servoj internally uses a step time that takes the current velocity scaling into account. Therefore, a servoj command might block longer than one control cycle. If we join the thread this operation will effectively wait until that servoj command is finished, while the robot is still moving. By explicitly killing and calling stopj we make sure the robot actually stops

This enables the possibility to have both spline interpolation and movej points in on trajectory

codecov · 2023-01-20T07:04:32Z

Codecov Report

Patch coverage: 20.75% and project coverage change: -1.67 ⚠️

Comparison is base (8dbe2a1) 78.36% compared to head (588e4c2) 76.69%.

❗ Current head 588e4c2 differs from pull request most recent head dafbdb8. Consider uploading reports for the commit dafbdb8 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #103      +/-   ##
==========================================
- Coverage   78.36%   76.69%   -1.67%     
==========================================
  Files         100       95       -5     
  Lines        4409     3987     -422     
  Branches      449      394      -55     
==========================================
- Hits         3455     3058     -397     
- Misses        690      706      +16     
+ Partials      264      223      -41

Impacted Files	Coverage Δ
...lude/ur_client_library/control/reverse_interface.h	`100.00% <ø> (ø)`
...lient_library/control/trajectory_point_interface.h	`100.00% <ø> (ø)`
include/ur_client_library/ur/ur_driver.h	`33.33% <ø> (-26.67%)`	⬇️
src/ur/ur_driver.cpp	`51.11% <0.00%> (+2.13%)`	⬆️
src/control/trajectory_point_interface.cpp	`40.00% <8.10%> (-31.80%)`	⬇️
tests/test_trajectory_point_interface.cpp	`95.53% <100.00%> (+0.25%)`	⬆️

... and 10 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Do you have feedback about the report comment? Let us know in this issue.

resources/external_control.urscript

urrsk

It needs to use the target values to avoid unnecessary noise in interpolation of the spline

resources/external_control.urscript

Use target values for the starting point of the spline to avoid unnecessary noise

The num_retries wouldn't be reset within each function, but it will now so the correct number of retries is performed in each function, before throwing an exception.

* Added tests for the comm classes This add tests for * bin parser * package serializer * TCP socket * stream * producer * pipeline Fixed check size in bin parser, so that it works with starndard data types Added recconection time variable to TCP socket, this will control the time in between connection attempts if the server is unavailable. Co-authored-by: Felix Exner <[email protected]>

The file in question got removed from master, so it got replaced with a permalink.

- Optimized the code - Added unittest for the trajectory point - Added an example for spline

urrsk · 2023-06-28T21:09:31Z

The rebase to master was not so easy, so I think we should verify the latest features like freedrive and tool contact before merging

fmauch · 2023-06-28T21:38:08Z

Thank you for the updates. I'll try to have a look into this tomorrow.

fmauch

First of all: Thank you both @urmahp and @urrsk for the tremendous work put into this. I've raised a couple of discussion points and it looks like we have to make the tests a bit more stable, but apart from that it seems very good.

I've tested this with UniversalRobots/Universal_Robots_ROS_Driver#543 and it seems to be working fine.

fmauch · 2023-06-30T05:35:37Z

examples/spline_example.cpp

+
+void SendTrajectory(const std::vector<vector6d_t>& p_p, const std::vector<vector6d_t>& p_v,
+                    const std::vector<vector6d_t>& p_a, const std::vector<double>& time, bool use_spline_interpolation_)
+{


There could be an assertion here that p_p has the same size as time, as this is a hard requirement.

examples/spline_example.cpp

fmauch · 2023-06-30T05:39:08Z

examples/spline_example.cpp

+  // Power it off
+  if (!g_my_dashboard->commandPowerOff())
+  {
+    URCL_LOG_ERROR("Could not send Power off command");
+    return 1;
+  }
+
+  // Power it on
+  if (!g_my_dashboard->commandPowerOn())
+  {
+    URCL_LOG_ERROR("Could not send Power on command");
+    return 1;
+  }


What exactly is the motivation behind this? I found it rather annoying while testing with a real robot. If we reduce this to only switching the robot an and releasing the brakes, it would have the same effect that we know the robot will be running afterwards without the additional switch off / switch on thing.

fmauch · 2023-06-30T05:42:24Z

examples/spline_example.cpp

+  // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main
+  // loop.
+
+  g_my_driver->startRTDECommunication();


With having this line so far away from the main loop it is probably not really deterministic whether we will be able to read once the second data package is sent.

As the current joint positions are used in assembling the trajectory we can't simply move it down. One approach could be to start RTDE communication in a separate thread.

include/ur_client_library/ur/ur_driver.h

include/ur_client_library/control/trajectory_point_interface.h

fmauch · 2023-06-30T06:08:52Z

src/control/trajectory_point_interface.cpp

+    return false;
+  }
+
+  control::TrajectorySplineType spline_type = control::TrajectorySplineType::SPLINE_QUINTIC;


I think this can be cleaned up a bit in such a way that we initialize the spline_type as SPLINE_QUADRATIC and change it to a higher order in the if clauses when velocity / acceleration information is given.

fmauch · 2023-06-30T06:35:44Z

examples/spline_example.cpp

+  }
+
+  const unsigned number_of_points = 5;
+  const double s_pos[number_of_points] = { 4.15364583e-03, 4.15364583e-03, -1.74088542e-02, 1.44817708e-02,


There seems to be an error in unit conversion. The motions done by the robot are in fact a lot larger than this:

0: 0.0331154 1: 0.0331154 2: 0.0115529 3: 0.0434435 4: 0.0434435

Edit: Nevermind. This seems to work correct. It's just that the position / velocity combination inside the test is a bit unintuitive. And in fact, it seems to move the robot into its joint limits inside the tests. I would suggest using a simpler motion instead.

Additionally, I would suggest using a fixed motion and no relative motion to the starting state as this will have a much more predictable output.

…eal-time requirements

Co-authored-by: Felix Exner (fexner) <[email protected]>

fmauch · 2023-07-06T05:03:28Z

Closing this in favor of merged #151

urmahp mentioned this pull request Jun 9, 2022

Implemented spline interpolation in joint space UniversalRobots/Universal_Robots_ROS_Driver#543

Merged

fmauch mentioned this pull request Dec 12, 2022

Added trajectory port and prefixed gpios UniversalRobots/Universal_Robots_ROS2_Description#47

Merged

fmauch and others added 3 commits January 9, 2023 10:48

Implemented spline interpolation in joint space in URScript

cec0847

Added spline interpolation to the trajectoryThread

ff3cf76

This enables the possibility to have both spline interpolation and movej points in on trajectory

urmahp force-pushed the spline_interpolation_v2 branch from 688fa0b to ff3cf76 Compare January 9, 2023 14:56

Fix the test

dce10ba

Fix formatting

72f4fab

urrsk reviewed Jun 7, 2023

View reviewed changes

resources/external_control.urscript Outdated Show resolved Hide resolved

urrsk reviewed Jun 7, 2023

View reviewed changes

resources/external_control.urscript Outdated Show resolved Hide resolved

resources/external_control.urscript Outdated Show resolved Hide resolved

urrsk and others added 6 commits June 7, 2023 13:04

Use target values for spline interpolation

588e4c2

Use target values for the starting point of the spline to avoid unnecessary noise

Changed num_retries from static to an unsigned int (UniversalRobots#136)

b19dbc7

The num_retries wouldn't be reset within each function, but it will now so the correct number of retries is performed in each function, before throwing an exception.

Update link to ros_industrial_cmake_boilerplate

146db88

The file in question got removed from master, so it got replaced with a permalink.

Optimized the spline interpolation code

e919b46

- Optimized the code - Added unittest for the trajectory point - Added an example for spline

Merge branch 'master' into spline_interpolation_v2

a84d86c

Fix formatting

8a5f6d8

urrsk force-pushed the spline_interpolation_v2 branch from 1eac493 to 8a5f6d8 Compare June 28, 2023 21:16

urrsk added 3 commits June 29, 2023 18:16

Merge fix

b5214f8

More merge fix

0992492

Revert a bit to see if this have impact on the CI

45792e7

urrsk force-pushed the spline_interpolation_v2 branch from 05491bc to 45792e7 Compare June 29, 2023 20:56

fmauch reviewed Jun 30, 2023

View reviewed changes

urrsk and others added 3 commits June 30, 2023 09:20

Made the spline test motion absolute and change the order

02ed066

Try increasing the keepalive_counter in order to decrease examples' r…

6625547

…eal-time requirements

Add more printsouts for the full_driver example

fe56323

urrsk and others added 5 commits June 30, 2023 11:21

more print outs

3806f9e

Revert some printouts and make it compile again

ddaf62d

Apply suggestions from code review

438f0d6

Co-authored-by: Felix Exner (fexner) <[email protected]>

rsk1

df3f81a

rsk2

dafbdb8

urrsk mentioned this pull request Jun 30, 2023

Spline merge #151

Merged

fmauch closed this Jul 6, 2023

urrsk assigned urmahp Aug 4, 2023

urmahp deleted the spline_interpolation_v2 branch August 29, 2023 06:54

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implemented spline interpolation in joint space #103

Implemented spline interpolation in joint space #103

urmahp commented Jun 9, 2022

codecov bot commented Jan 20, 2023 •

edited

Loading

urrsk left a comment

urrsk commented Jun 28, 2023

fmauch commented Jun 28, 2023

fmauch left a comment

fmauch Jun 30, 2023

fmauch Jun 30, 2023

fmauch Jun 30, 2023

fmauch Jun 30, 2023

fmauch Jun 30, 2023 •

edited

Loading

fmauch commented Jul 6, 2023

Implemented spline interpolation in joint space #103

Implemented spline interpolation in joint space #103

Conversation

urmahp commented Jun 9, 2022

codecov bot commented Jan 20, 2023 • edited Loading

Codecov Report

urrsk left a comment

Choose a reason for hiding this comment

urrsk commented Jun 28, 2023

fmauch commented Jun 28, 2023

fmauch left a comment

Choose a reason for hiding this comment

fmauch Jun 30, 2023

Choose a reason for hiding this comment

fmauch Jun 30, 2023

Choose a reason for hiding this comment

fmauch Jun 30, 2023

Choose a reason for hiding this comment

fmauch Jun 30, 2023

Choose a reason for hiding this comment

fmauch Jun 30, 2023 • edited Loading

Choose a reason for hiding this comment

fmauch commented Jul 6, 2023

codecov bot commented Jan 20, 2023 •

edited

Loading

fmauch Jun 30, 2023 •

edited

Loading