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.

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

Add toleranced Cartesian waypoints to solver #354

Merged
merged 18 commits into from
Jan 14, 2024

Conversation

marrts
Copy link
Contributor

@marrts marrts commented Nov 3, 2023

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

Comment on lines 32 to 69
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;
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@Levi-Armstrong
Copy link
Contributor

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
Copy link
Contributor Author

marrts commented Nov 5, 2023

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
Copy link
Contributor

Levi-Armstrong commented Nov 6, 2023

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
Copy link
Contributor Author

marrts commented Nov 6, 2023

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?

Copy link
Contributor

@Levi-Armstrong Levi-Armstrong left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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
@Levi-Armstrong
Copy link
Contributor

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
Copy link
Contributor Author

marrts commented Nov 6, 2023

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
Copy link
Contributor Author

marrts commented Nov 6, 2023

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
Copy link
Contributor

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
Copy link
Contributor Author

marrts commented Nov 28, 2023

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

@marrts marrts force-pushed the toleranced-waypoints branch from 5f623af to 9a0155c Compare November 29, 2023 22:39
@marrts marrts force-pushed the toleranced-waypoints branch from 9a0155c to add5389 Compare December 7, 2023 17:09
@marrts marrts requested a review from Levi-Armstrong December 7, 2023 21:26
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
@marrts
Copy link
Contributor Author

marrts commented Dec 12, 2023

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
Copy link
Contributor

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.

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@rjoomen
Copy link
Contributor

rjoomen commented Dec 20, 2023

Could you implement this for trajopt_ifopt as well?

@marrts
Copy link
Contributor Author

marrts commented Jan 12, 2024

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:

  1. Simply apply my change and the test passes
  2. 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
  3. Remove this test altogether because it actually isn't necessarily expected that they match
  4. Change the numerical implementation to somehow switch to an error that always has a positive angle

@marrts
Copy link
Contributor Author

marrts commented Jan 12, 2024

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?

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

@Levi-Armstrong
Copy link
Contributor

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

@marrts marrts force-pushed the toleranced-waypoints branch from 9b76760 to 566b795 Compare January 12, 2024 17:00
@marrts marrts requested a review from Levi-Armstrong January 12, 2024 21:43
@Levi-Armstrong Levi-Armstrong merged commit 988c079 into tesseract-robotics:master Jan 14, 2024
7 of 8 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants