A version of TOTG computeTimeStamps() that accounts for torque limits #3412

AndyZe · 2023-04-26T15:56:57Z

Description

This adds a new version of TOTG computeTimeStamps() which takes robot dynamics into consideration. The robot inertia model is pulled from the URDF.

Here's the algorithm in a nutshell:

  // 1. Call computeTimeStamps() to time-parameterize the trajectory with given vel/accel limits.
  // 2. Run forward dynamics to check if torque limits are violated at any waypoint.
  // 3. If a torque limit was violated, decrease the acceleration limit for that joint and go back to Step 1.

This will probably be interesting to @mechwiz @stephanie-eng @sjahr

Sorry for the large line count. Most of that comes from syncing with MoveIt2 (the 2nd commit). You could ignore that one when reviewing, if you want -- focus mostly on computeTimeStampsWithTorqueLimits()

codecov · 2023-04-26T20:22:07Z

Codecov Report

Patch coverage: 69.28% and project coverage change: -0.01 ⚠️

Comparison is base (51cff82) 61.72% compared to head (65240b9) 61.70%.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3412      +/-   ##
==========================================
- Coverage   61.72%   61.70%   -0.01%     
==========================================
  Files         382      385       +3     
  Lines       33788    34095     +307     
==========================================
+ Hits        20851    21036     +185     
- Misses      12937    13059     +122

Impacted Files	Coverage Δ
...del/include/moveit/robot_model/joint_model_group.h	`93.55% <ø> (+0.15%)`	⬆️
...ry_processing/time_optimal_trajectory_generation.h	`100.00% <ø> (ø)`
...ng/src/iterative_torque_limit_parameterization.cpp	`63.94% <63.94%> (ø)`
moveit_core/robot_model/src/joint_model_group.cpp	`62.01% <66.67%> (+0.15%)`	⬆️
...cessing/src/time_optimal_trajectory_generation.cpp	`88.28% <72.83%> (-1.62%)`	⬇️
...ocessing/iterative_torque_limit_parameterization.h	`100.00% <100.00%> (ø)`

... and 9 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.

...jectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

...jectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

...ry_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h

rhaschke

Dear @AndyZe, I have a few important pose-merge comments. Please address them.

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

rhaschke · 2023-05-10T15:38:55Z

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

+  return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
+}
+
+bool TimeOptimalTrajectoryGeneration::computeTimeStamps(


This method has strong overlap with the other computeTimeStamps method.
Better factor out common code into a separate helper function!

Completely agree with you but won't address now. It's not really related to this "torque-limited" feature.

BTW I'm no longer at PickNik. I'm working on ag robots with AppliedLogix now.

You seem to like programming by copy-and-paste! I'm trying to cleanup the redundancy and discovered some more issues in this PR, namely you are incorrectly indexing the joints. Could you please open your PR #3426 for additional commits?

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

rhaschke · 2023-05-10T15:43:45Z

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

+
+double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor) const
+{
+  double scaling_factor = std::clamp(requested_scaling_factor, -1.0, 1.0);


Why do you allow for negative scaling factors?

I believe you're correct. Done.

AndyZe · 2023-05-10T16:03:35Z

Thanks Robert, I will do this in the next few days 👍

rhaschke · 2023-05-11T09:31:28Z

moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h

+  /** \brief Computes the indices of joint variables given a vector of joint names to look up */
+  bool computeJointVariableIndices(const std::vector<std::string>& joint_names,
+                                   std::vector<size_t>& joint_bijection) const;
+


This seems to be an exact copy of computeIKIndexBijection - except throwing a different error message!
Increasing code redundancy should be strongly avoided!

rhaschke · 2023-05-11T10:21:41Z

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

+  // Get the velocity and acceleration limits for all active joints
+  const moveit::core::RobotModel& rmodel = group->getParentModel();
+  const std::vector<std::string>& vars = group->getVariableNames();
+  std::vector<size_t> active_joint_indices;
+  if (!group->computeJointVariableIndices(group->getActiveJointModelNames(), active_joint_indices))


Hi @AndyZe,
In contrast to the previous code, you switched from all variables to active variables when looking up max velocities and accelerations. However, the TOTG algorithm still considers all variables, doesn't it?
I expect this to segfault due to mismatching vector lengths for variables and limits! Did you test this with robot models comprising mimic joints?

rhaschke · 2023-07-31T15:03:16Z

@AndyZe, we get complaints from users on the TOTG behavior since the latest release: #3450 (comment)

AndyZe requested a review from mlautman as a code owner April 26, 2023 15:56

AndyZe marked this pull request as draft April 26, 2023 15:57

AndyZe force-pushed the andyz/dynamics_with_totg branch 3 times, most recently from 6ec5915 to de3d8eb Compare April 26, 2023 18:56

AndyZe force-pushed the andyz/dynamics_with_totg branch 16 times, most recently from 5710ebd to 9e8aebb Compare May 2, 2023 18:54

AndyZe marked this pull request as ready for review May 2, 2023 19:37

AndyZe requested a review from tylerjw as a code owner May 2, 2023 19:37

AndyZe force-pushed the andyz/dynamics_with_totg branch from 22b62a7 to 3ca562c Compare May 2, 2023 19:41

AndyZe requested a review from henningkayser May 2, 2023 20:07

AndyZe and others added 4 commits May 2, 2023 15:19

A version of TOTG computeTimeStamps() that accounts for torque limits

40b95ca

Sync with MoveIt2

42b7f28

Iterate on computeTimeStampsWithTorqueLimits()

83fa40f

Add a test

ca1b2a8

Remove debug output

f66d9cf

AndyZe force-pushed the andyz/dynamics_with_totg branch from bb3c1db to f66d9cf Compare May 2, 2023 20:20

AndyZe added 2 commits May 2, 2023 15:56

Revert to the old MoveIt1 behavior of default vel/accel limits

7e32bf6

clang-tidy

0d7eae4

henningkayser reviewed May 3, 2023

View reviewed changes

Clean up verifyScalingFactor()

7c96d80

AndyZe force-pushed the andyz/dynamics_with_totg branch from 2603a8d to 7c96d80 Compare May 4, 2023 13:54

Formatting; auto-timestep

0a9f4e3

AndyZe force-pushed the andyz/dynamics_with_totg branch from f383054 to fe9ce8d Compare May 4, 2023 17:02

Move computeTimeStampsWithTorqueLimits to new file

488b175

AndyZe force-pushed the andyz/dynamics_with_totg branch from fe9ce8d to 488b175 Compare May 4, 2023 17:07

AndyZe requested a review from henningkayser May 4, 2023 17:56

Make external_link_wrenches into an argument

65240b9

AndyZe force-pushed the andyz/dynamics_with_totg branch from 9b2cc65 to 65240b9 Compare May 5, 2023 14:47

henningkayser reviewed May 10, 2023

View reviewed changes

...ry_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h Show resolved Hide resolved

henningkayser approved these changes May 10, 2023

View reviewed changes

AndyZe merged commit 4aeccc7 into moveit:master May 10, 2023
7 checks passed

AndyZe deleted the andyz/dynamics_with_totg branch May 10, 2023 14:55

rhaschke reviewed May 10, 2023

View reviewed changes

AndyZe mentioned this pull request May 10, 2023

Minor cleanup and debugging in TOTG #3426

Closed

rhaschke reviewed May 11, 2023

View reviewed changes

rhaschke mentioned this pull request May 11, 2023

Fixup TOTG #3427

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

A version of TOTG computeTimeStamps() that accounts for torque limits #3412

A version of TOTG computeTimeStamps() that accounts for torque limits #3412

AndyZe commented Apr 26, 2023 •

edited

codecov bot commented Apr 26, 2023 •

edited

rhaschke left a comment

rhaschke May 10, 2023

AndyZe May 10, 2023

rhaschke May 11, 2023

rhaschke May 10, 2023

AndyZe May 10, 2023

AndyZe commented May 10, 2023

rhaschke May 11, 2023

rhaschke May 11, 2023

rhaschke commented Jul 31, 2023

A version of TOTG computeTimeStamps() that accounts for torque limits #3412

A version of TOTG computeTimeStamps() that accounts for torque limits #3412

Conversation

AndyZe commented Apr 26, 2023 • edited

Description

codecov bot commented Apr 26, 2023 • edited

Codecov Report

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke May 10, 2023

Choose a reason for hiding this comment

AndyZe May 10, 2023

Choose a reason for hiding this comment

rhaschke May 11, 2023

Choose a reason for hiding this comment

rhaschke May 10, 2023

Choose a reason for hiding this comment

AndyZe May 10, 2023

Choose a reason for hiding this comment

AndyZe commented May 10, 2023

rhaschke May 11, 2023

Choose a reason for hiding this comment

rhaschke May 11, 2023

Choose a reason for hiding this comment

rhaschke commented Jul 31, 2023

AndyZe commented Apr 26, 2023 •

edited

codecov bot commented Apr 26, 2023 •

edited