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

Add toleranced Cartesian waypoints to solver #354

Merged
merged 18 commits into from
Jan 14, 2024
Merged

Add toleranced Cartesian waypoints to solver #354

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
f54438f
Add toleranced Cartesian waypoints to solver
marrts Nov 3, 2023
285d5fc
Clang formatting
marrts Nov 3, 2023
6547ff7
Remove jac tolerances and add error function
marrts Nov 6, 2023
81b45c8
Clang formatting
marrts Nov 6, 2023
c8a7f3c
use jacobians and populate error_function
marrts Nov 7, 2023
c7d01c1
Working in tests
marrts Nov 7, 2023
a1ecbe2
Clean up applyTolerances function
marrts Nov 8, 2023
38b7c85
Fix missing option in OptStatus to string
marrts Nov 16, 2023
fe32194
Implement Jacobian calculations
marrts Nov 28, 2023
e709842
Clang formatting
marrts Nov 28, 2023
d0aa5e1
Add dfdx to dynamic cart pose contraints
marrts Dec 7, 2023
1abdd28
Rename error_function as class member and add reduced error calc
marrts Dec 7, 2023
e049ece
Check for active target in CartPoseErrCalc
marrts Dec 7, 2023
e26ce1b
Clang formatting
marrts Dec 7, 2023
674559d
PR feedback cleanup
marrts Dec 8, 2023
ff83f5c
Update convergence string conversion to match enum
marrts Dec 12, 2023
885e93b
Remove comments about not being used, because it is used now
marrts Jan 12, 2024
566b795
Fix failing test from different rotational error calc methods
marrts Jan 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 27 additions & 21 deletions trajopt/include/trajopt/kinematic_terms.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>

#include <trajopt_common/utils.hpp>
#include <tesseract_environment/environment.h>
#include <tesseract_environment/utils.h>
#include <tesseract_kinematics/core/joint_group.h>
Expand All @@ -15,6 +16,10 @@ TRAJOPT_IGNORE_WARNINGS_POP

namespace trajopt
{
const double DEFAULT_EPSILON = 1e-5;

using ErrorFunctionType = std::function<Eigen::VectorXd(const Eigen::Isometry3d&, const Eigen::Isometry3d&)>;

/**
* @brief Used to calculate the error for CartPoseTermInfo
* This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc
Expand All @@ -38,6 +43,10 @@ struct DynamicCartPoseErrCalculator : public TrajOptVectorOfVector
/** @brief A offset transform to be applied to target_frame_ location */
Eigen::Isometry3d target_frame_offset_;

/** @brief Error function for calculating the error in the position given the source and target positions
* this defaults to tesseract_common::calcTransformError if unset*/
ErrorFunctionType error_function{ nullptr };

/**
* @brief This is a vector of indices to be returned Default: {0, 1, 2, 3, 4, 5}
*
Expand All @@ -52,16 +61,9 @@ struct DynamicCartPoseErrCalculator : public TrajOptVectorOfVector
std::string target_frame,
const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(),
const Eigen::Isometry3d& target_frame_offset = Eigen::Isometry3d::Identity(),
Eigen::VectorXi indices = Eigen::Matrix<int, 1, 6>(std::vector<int>({ 0, 1, 2, 3, 4, 5 }).data()))
: manip_(std::move(manip))
, source_frame_(std::move(source_frame))
, target_frame_(std::move(target_frame))
, source_frame_offset_(source_frame_offset)
, target_frame_offset_(target_frame_offset)
, indices_(std::move(indices))
{
assert(indices_.size() <= 6);
}
Eigen::VectorXi indices = Eigen::Matrix<int, 1, 6>(std::vector<int>({ 0, 1, 2, 3, 4, 5 }).data()),
Eigen::VectorXd lower_tolerance = {},
Eigen::VectorXd upper_tolerance = {});

void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const Eigen::VectorXd& dof_vals) override;

Expand Down Expand Up @@ -96,6 +98,9 @@ struct DynamicCartPoseJacCalculator : sco::MatrixOfVector
*/
Eigen::VectorXi indices_;

/** @brief perturbation amount for calculating Jacobian */
double epsilon_;

DynamicCartPoseJacCalculator(
tesseract_kinematics::JointGroup::ConstPtr manip,
std::string source_frame,
Expand All @@ -109,6 +114,7 @@ struct DynamicCartPoseJacCalculator : sco::MatrixOfVector
, target_frame_(std::move(target_frame))
, target_frame_offset_(target_frame_offset)
, indices_(std::move(indices))
, epsilon_(DEFAULT_EPSILON)
{
assert(indices_.size() <= 6);
}
Expand Down Expand Up @@ -140,6 +146,10 @@ struct CartPoseErrCalculator : public TrajOptVectorOfVector
/** @brief indicates which link is active */
bool is_target_active_{ true };

/** @brief Error function for calculating the error in the position given the source and target positions
* this defaults to tesseract_common::calcTransformError if unset*/
ErrorFunctionType error_function_{ nullptr };

/**
* @brief This is a vector of indices to be returned Default: {0, 1, 2, 3, 4, 5}
*
Expand All @@ -154,17 +164,9 @@ struct CartPoseErrCalculator : public TrajOptVectorOfVector
std::string target_frame,
const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(),
const Eigen::Isometry3d& target_frame_offset = Eigen::Isometry3d::Identity(),
Eigen::VectorXi indices = Eigen::Matrix<int, 1, 6>(std::vector<int>({ 0, 1, 2, 3, 4, 5 }).data()))
: manip_(std::move(manip))
, source_frame_(std::move(source_frame))
, source_frame_offset_(source_frame_offset)
, target_frame_(std::move(target_frame))
, target_frame_offset_(target_frame_offset)
, indices_(std::move(indices))
{
is_target_active_ = manip_->isActiveLinkName(target_frame_);
assert(indices_.size() <= 6);
}
Eigen::VectorXi indices = Eigen::Matrix<int, 1, 6>(std::vector<int>({ 0, 1, 2, 3, 4, 5 }).data()),
Eigen::VectorXd lower_tolerance = {},
Eigen::VectorXd upper_tolerance = {});

void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const Eigen::VectorXd& dof_vals) override;

Expand Down Expand Up @@ -201,6 +203,9 @@ struct CartPoseJacCalculator : sco::MatrixOfVector
*/
Eigen::VectorXi indices_;

/** @brief perturbation amount for calculating Jacobian */
double epsilon_;

CartPoseJacCalculator(
tesseract_kinematics::JointGroup::ConstPtr manip,
std::string source_frame,
Expand All @@ -214,6 +219,7 @@ struct CartPoseJacCalculator : sco::MatrixOfVector
, target_frame_(std::move(target_frame))
, target_frame_offset_(target_frame_offset)
, indices_(std::move(indices))
, epsilon_(DEFAULT_EPSILON)
{
is_target_active_ = manip_->isActiveLinkName(target_frame_);
assert(indices_.size() <= 6);
Expand Down
20 changes: 20 additions & 0 deletions trajopt/include/trajopt/problem_description.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -295,6 +295,16 @@ struct DynamicCartPoseTermInfo : public TermInfo
Eigen::Isometry3d source_frame_offset;
/** @brief A Static transform to be applied to target_ location */
Eigen::Isometry3d target_frame_offset;
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
Eigen::VectorXd lower_tolerance;
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
Eigen::VectorXd upper_tolerance;

/** @brief Error function for calculating the error in the position given the source and target positions
* this defaults to tesseract_common::calcTransformError if unset*/
std::function<Eigen::VectorXd(const Eigen::Isometry3d&, const Eigen::Isometry3d&)> error_function = nullptr;

DynamicCartPoseTermInfo();

Expand Down Expand Up @@ -325,6 +335,16 @@ struct CartPoseTermInfo : public TermInfo
Eigen::Isometry3d source_frame_offset;
/** @brief A Static transform to be applied to target_ location */
Eigen::Isometry3d target_frame_offset;
/** @brief Distance below waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle()) */
Eigen::VectorXd lower_tolerance;
/** @brief Distance above waypoint that is allowed. Should be size = 6. First 3 elements are dx, dy, dz. The last 3
* elements are angle axis error allowed (Eigen::AngleAxisd.axis() * Eigen::AngleAxisd.angle())*/
Eigen::VectorXd upper_tolerance;

/** @brief Error function for calculating the error in the position given the source and target positions
* this defaults to tesseract_common::calcTransformError if unset*/
std::function<Eigen::VectorXd(const Eigen::Isometry3d&, const Eigen::Isometry3d&)> error_function = nullptr;

CartPoseTermInfo();

Expand Down
Loading
Loading