Add toleranced Cartesian waypoints to solver #354

marrts · 2023-11-03T23:20:32Z

This is to support tolerances on Cartesian waypoints so that you can set constraints that don't have to match perfectly

trajopt/include/trajopt/kinematic_terms.hpp

marip8 · 2023-11-04T00:14:11Z

trajopt/src/kinematic_terms.cpp

+Eigen::VectorXd calculateExceededTolerance(const Eigen::VectorXd& lower_tolerance,
+                                           const Eigen::VectorXd& upper_tolerance,
+                                           const Eigen::VectorXd& error)
+{
+  // Initialize the resultant vector
+  Eigen::VectorXd resultant(error.size());
+
+  // Iterate through each element
+  for (int i = 0; i < error.size(); ++i)
+  {
+    // If error is within tolerances, set resultant to 0
+    if (error(i) >= lower_tolerance(i) && error(i) <= upper_tolerance(i))
+    {
+      resultant(i) = 0.0;
+    }
+    // If error is below lower tolerance, set resultant to error - lower_tolerance
+    else if (error(i) < lower_tolerance(i))
+    {
+      resultant(i) = error(i) - lower_tolerance(i);
+    }
+    // If error is above upper tolerance, set resultant to error - upper_tolerance
+    else if (error(i) > upper_tolerance(i))
+    {
+      resultant(i) = error(i) - upper_tolerance(i);
+    }
+  }
+
+  return resultant;
+}
+


I think this already exists in Trajopt IFOPT in a vectorized, potentially faster way here

That comparison you referenced is doing something slightly different. I reimplemented this though in the latest commit.

Levi-Armstrong · 2023-11-05T14:23:56Z

In order for this to not have stability issues this cannot use the the jacobian utility to provide the jacobain because the error function is not continuous. When within the tolerance this will produce an all zero jacobian which is not correct so this will need to have its own jacobian method. This need to more or less use the error function without tolerance to calculate the jacobian.

marrts · 2023-11-05T14:50:34Z

In order for this to not have stability issues this cannot use the the jacobian utility to provide the jacobain because the error function is not continuous. When within the tolerance this will produce an all zero jacobian which is not correct so this will need to have its own jacobian method. This need to more or less use the error function without tolerance to calculate the jacobian.

This is why in my tesseract_planning PR I made it so that we can simultaneously have both costs and constraints, so the cost will continue to drive it to zero while the constraint will keep it inside the bounds. However, if there's a better solution I'd definitely be open to making the changes for that.

Levi-Armstrong · 2023-11-06T15:41:48Z

I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function signature should be the same as calcTransformError -> VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

marrts · 2023-11-06T15:45:33Z

I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function have the signature of calcTransformError -> VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

Does this have anything to do with the Jacobian issue, or is the purpose of this to save on computation in the cases where there is no tolerance applied?

Levi-Armstrong

Also need to figure out how to handle the jacobian issue when toleranced.

trajopt/src/problem_description.cpp

Levi-Armstrong · 2023-11-06T15:48:45Z

Does this have anything to do with the Jacobian issue, or is the purpose of this to save on computation in the cases where there is no tolerance applied?

No, just computation and address the issue @marip8 pointed out.

marrts · 2023-11-06T21:53:24Z

I would update this so the class holds a std::function which should be used to calculate the error. Then based on construction if toleranced we point it to one function and if not tolerance it points to another function. The function signature should be the same as calcTransformError -> VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

I added an error function option, but I'm not sure what we want to put the default values of the tolerances at now. The error function defaults to tesseract_common::calcTransformError if none is given.

marrts · 2023-11-06T22:01:52Z

but I'm not sure what we want to put the default values of the tolerances at now.

Just realized I don't actually use the tolerances anywhere any more. Should the tolerances just be completely dropped from these structs/classes and put the burden on a higher level function calling them to pass in an error function that captures the tolerances to calculate the error?

Levi-Armstrong · 2023-11-06T22:58:17Z

Sorry for the confusion. I would not pass the error function into the constructor. I would keep the tolerance as input to the constructor and have the constructor evaluate the tolerance to determine which function to assign to the internal member variable.

marrts · 2023-11-28T23:19:25Z

This has been working for me now and I think it addresses everything. It has been very useful in increasing motion planning success.

trajopt/src/kinematic_terms.cpp

trajopt/include/trajopt/kinematic_terms.hpp

trajopt/src/kinematic_terms.cpp

trajopt/include/trajopt/kinematic_terms.hpp

trajopt/src/kinematic_terms.cpp

trajopt_sco/include/trajopt_sco/optimizers.hpp

marrts · 2023-12-12T16:18:28Z

The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of calcTransformErrorJac. Fundamentally the "numerical" and "analytical" solution are doing the same thing, except "analytical" now uses calcTransformErrorJac and numerical still uses calcTransformError as a basis for the calculation.

Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56

Levi-Armstrong · 2023-12-14T15:55:37Z

The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of calcTransformErrorJac. Fundamentally the "numerical" and "analytical" solution are doing the same thing, except "analytical" now uses calcTransformErrorJac and numerical still uses calcTransformError as a basis for the calculation.

Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56

@marrts I will take a look at this and push a fix.

rjoomen · 2023-12-20T10:35:41Z

trajopt/src/problem_description.cpp

+                                                     target_frame_offset,
+                                                     indices,
+                                                     lower_tolerance,
+                                                     upper_tolerance);

    // This is currently not being used. There is an intermittent bug that needs to be tracked down it is not used.


Is this issue solved now? Otherwise please add @todo, so it can be found more easily.

I would say this resolves it, so I'm removing those comments.

rjoomen · 2023-12-20T10:35:51Z

trajopt/src/problem_description.cpp

+                                                     target_frame_offset,
+                                                     indices,
+                                                     lower_tolerance,
+                                                     upper_tolerance);

    // This is currently not being used. There is an intermittent bug that needs to be tracked down it is not used.


Is this issue solved now? Otherwise please add @todo, so it can be found more easily.

rjoomen · 2023-12-20T10:41:03Z

trajopt/src/problem_description.cpp

+                                                            target_frame_offset,
+                                                            indices,
+                                                            lower_tolerance,
+                                                            upper_tolerance);

    // This is currently not being used. There is an intermittent bug that needs to be tracked down it is not used.


Is this issue solved now? Otherwise please add @todo, so it can be found more easily.

rjoomen · 2023-12-20T10:49:59Z

Could you implement this for trajopt_ifopt as well?

marrts · 2024-01-12T15:05:16Z

The test that is failing is comparing the numerical to the analytical Jacobian solutions, which would be expected to be different now because of the addition of calcTransformErrorJac. Fundamentally the "numerical" and "analytical" solution are doing the same thing, except "analytical" now uses calcTransformErrorJac and numerical still uses calcTransformError as a basis for the calculation.
Failing comparison: https://github.com/marrts/trajopt_ros/blob/9b7676069257930aca365c23adc142579a6d3e8d/trajopt/test/kinematic_costs_unit.cpp#L56

@marrts I will take a look at this and push a fix.

This test is failing because the rotational error set to [-pi,pi] for the numerical and [0,2pi] for the analytical. The test produces these error vectors for the initial error.

err_numerical:  -1.19071 -0.0193624  -0.417557  -0.910973  -0.810396   -2.52978
err_analytical:  -1.19071 -0.0193624  -0.417557    1.12722    1.00277    3.13031

The Jacobian results are:

Error:   Numerical:
  0.0193634  -0.189402   0.199863  0.0145331  0.0711069 -0.0978927          0
 -0.189711    0.37213   0.287726 -0.0535873   0.130371   0.122383          0
         0   0.168799 -0.0644466   -0.33596   0.101585 -0.0885404          0
  0.628394  -0.259181  -0.148265    0.46934   -1.30136   0.384817   0.316577
 -0.256931    1.32699   0.362308   -1.27522  -0.361558   -0.60032   -1.19339
  0.856022   0.134472   -1.03897  -0.181689   0.104203    -0.8853   0.628396
Error:   Analytical:
  0.0193634  -0.189402   0.199863  0.0145331  0.0711069 -0.0978927          0
 -0.189711    0.37213   0.287726 -0.0535873   0.130371   0.122383          0
         0   0.168799 -0.0644466   -0.33596   0.101585 -0.0885404          0
 -0.123759   0.625531  -0.454852   -0.85613    1.29629   -1.09011  -0.156293
  0.899549   -1.37083   -1.01615    1.33296   0.168067   0.196658    1.68613
  0.756401   0.680107  -0.486995  -0.539909   -1.00088  -0.609487  -0.123762

The first three rows, corresponding to position, match and the last three, corresponding to rotation are different. This is the expected behavior based on the initial error vectors we have.

If you rotate the target frame by 180 degrees around z this goes away and the test passes:

Eigen::Isometry3d target_frame_offset = Eigen::Isometry3d::Identity() * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ());

err_numerical:  -1.19071 -0.0193624  -0.417557  -0.591328   0.664716   0.344705
err_analytical:  -1.19071 -0.0193624  -0.417557  -0.591328   0.664716   0.344705

I see a few options for resolving this:

Simply apply my change and the test passes
Do two tests, one with my change and one without and on the one without just check the first three rows of the matrix match
Remove this test altogether because it actually isn't necessarily expected that they match
Change the numerical implementation to somehow switch to an error that always has a positive angle

marrts · 2024-01-12T15:18:35Z

Could you implement this for trajopt_ifopt as well?

I haven't used trajopt_ifopt yet, and it's not obvious to me what changes would need to be made. Perhaps we get this one merged we make an issue to implement this in trajopt_ifopt?

I do see that there is a bounds_ object in the CartPosConstraint class, does this allow tolerance by default?

trajopt/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h

Lines 175 to 176 in 0733775

    
           /** @brief Bounds on the positions of each joint */ 
        
           std::vector<ifopt::Bounds> bounds_;

Levi-Armstrong · 2024-01-12T16:33:05Z

@marrts I would just apply your change to make the unit test pass.

marrts mentioned this pull request Nov 3, 2023

Add toleranced waypoints to TrajOpt Solver tesseract-robotics/tesseract_planning#403

Merged

marip8 reviewed Nov 4, 2023

View reviewed changes

trajopt/include/trajopt/kinematic_terms.hpp Outdated Show resolved Hide resolved

marip8 reviewed Nov 4, 2023

View reviewed changes

Levi-Armstrong requested changes Nov 6, 2023

View reviewed changes

trajopt/src/problem_description.cpp Outdated Show resolved Hide resolved

trajopt/src/problem_description.cpp Outdated Show resolved Hide resolved

trajopt/src/problem_description.cpp Outdated Show resolved Hide resolved

marrts force-pushed the toleranced-waypoints branch from 5f623af to 9a0155c Compare November 29, 2023 22:39

marrts force-pushed the toleranced-waypoints branch from 9a0155c to add5389 Compare December 7, 2023 17:09

marrts requested a review from Levi-Armstrong December 7, 2023 21:26

marrts commented Dec 7, 2023

View reviewed changes

trajopt/src/kinematic_terms.cpp Show resolved Hide resolved

Levi-Armstrong requested changes Dec 8, 2023

View reviewed changes

trajopt/include/trajopt/kinematic_terms.hpp Outdated Show resolved Hide resolved

trajopt/src/kinematic_terms.cpp Outdated Show resolved Hide resolved

trajopt/include/trajopt/kinematic_terms.hpp Outdated Show resolved Hide resolved

trajopt/src/kinematic_terms.cpp Show resolved Hide resolved

Levi-Armstrong reviewed Dec 11, 2023

View reviewed changes

trajopt_sco/include/trajopt_sco/optimizers.hpp Outdated Show resolved Hide resolved

rjoomen reviewed Dec 20, 2023

View reviewed changes

marrts added 2 commits January 12, 2024 10:59

Add toleranced Cartesian waypoints to solver

f54438f

Clang formatting

285d5fc

marrts added 16 commits January 12, 2024 10:59

Remove jac tolerances and add error function

6547ff7

Clang formatting

81b45c8

use jacobians and populate error_function

c8a7f3c

Working in tests

c7d01c1

Clean up applyTolerances function

a1ecbe2

Fix missing option in OptStatus to string

38b7c85

Implement Jacobian calculations

fe32194

Clang formatting

e709842

Add dfdx to dynamic cart pose contraints

d0aa5e1

Rename error_function as class member and add reduced error calc

1abdd28

Check for active target in CartPoseErrCalc

e049ece

Clang formatting

e26ce1b

PR feedback cleanup

674559d

Update convergence string conversion to match enum

ff83f5c

Remove comments about not being used, because it is used now

885e93b

Fix failing test from different rotational error calc methods

566b795

marrts force-pushed the toleranced-waypoints branch from 9b76760 to 566b795 Compare January 12, 2024 17:00

marrts requested a review from Levi-Armstrong January 12, 2024 21:43

Levi-Armstrong approved these changes Jan 14, 2024

View reviewed changes

Levi-Armstrong merged commit 988c079 into tesseract-robotics:master Jan 14, 2024
7 of 8 checks passed

rjoomen mentioned this pull request Jan 17, 2024

Add toleranced Cartesian waypoints to trajopt_ifopt #381

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add toleranced Cartesian waypoints to solver #354

Add toleranced Cartesian waypoints to solver #354

marrts commented Nov 3, 2023

marip8 Nov 4, 2023

marrts Nov 7, 2023

Levi-Armstrong commented Nov 5, 2023

marrts commented Nov 5, 2023 •

edited

Loading

Levi-Armstrong commented Nov 6, 2023 •

edited

Loading

marrts commented Nov 6, 2023

Levi-Armstrong left a comment

Levi-Armstrong commented Nov 6, 2023

marrts commented Nov 6, 2023

marrts commented Nov 6, 2023

Levi-Armstrong commented Nov 6, 2023

marrts commented Nov 28, 2023

marrts commented Dec 12, 2023 •

edited

Loading

Levi-Armstrong commented Dec 14, 2023

rjoomen Dec 20, 2023

marrts Jan 12, 2024

rjoomen Dec 20, 2023

rjoomen Dec 20, 2023

rjoomen commented Dec 20, 2023

marrts commented Jan 12, 2024

marrts commented Jan 12, 2024

Levi-Armstrong commented Jan 12, 2024

Add toleranced Cartesian waypoints to solver #354

Add toleranced Cartesian waypoints to solver #354

Conversation

marrts commented Nov 3, 2023

marip8 Nov 4, 2023

Choose a reason for hiding this comment

marrts Nov 7, 2023

Choose a reason for hiding this comment

Levi-Armstrong commented Nov 5, 2023

marrts commented Nov 5, 2023 • edited Loading

Levi-Armstrong commented Nov 6, 2023 • edited Loading

marrts commented Nov 6, 2023

Levi-Armstrong left a comment

Choose a reason for hiding this comment

Levi-Armstrong commented Nov 6, 2023

marrts commented Nov 6, 2023

marrts commented Nov 6, 2023

Levi-Armstrong commented Nov 6, 2023

marrts commented Nov 28, 2023

marrts commented Dec 12, 2023 • edited Loading

Levi-Armstrong commented Dec 14, 2023

rjoomen Dec 20, 2023

Choose a reason for hiding this comment

marrts Jan 12, 2024

Choose a reason for hiding this comment

rjoomen Dec 20, 2023

Choose a reason for hiding this comment

rjoomen Dec 20, 2023

Choose a reason for hiding this comment

rjoomen commented Dec 20, 2023

marrts commented Jan 12, 2024

marrts commented Jan 12, 2024

Levi-Armstrong commented Jan 12, 2024

marrts commented Nov 5, 2023 •

edited

Loading

Levi-Armstrong commented Nov 6, 2023 •

edited

Loading

marrts commented Dec 12, 2023 •

edited

Loading