Skip to content

Commit

Permalink
Update to use transform error diff function for numerical jacobian
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Feb 10, 2024
1 parent ac685dc commit b921e47
Show file tree
Hide file tree
Showing 6 changed files with 274 additions and 146 deletions.
32 changes: 13 additions & 19 deletions trajopt/include/trajopt/kinematic_terms.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@ namespace trajopt
const double DEFAULT_EPSILON = 1e-5;

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

/**
* @brief Used to calculate the error for CartPoseTermInfo
Expand Down Expand Up @@ -61,9 +63,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()),
Eigen::VectorXd lower_tolerance = {},
Eigen::VectorXd upper_tolerance = {});
const Eigen::VectorXi& indices = Eigen::Matrix<int, 1, 6>(std::vector<int>({ 0, 1, 2, 3, 4, 5 }).data()),
const Eigen::VectorXd& lower_tolerance = {},
const Eigen::VectorXd& upper_tolerance = {});

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

Expand Down Expand Up @@ -107,7 +109,7 @@ struct DynamicCartPoseJacCalculator : sco::MatrixOfVector
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()))
const 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)
Expand Down Expand Up @@ -164,9 +166,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()),
Eigen::VectorXd lower_tolerance = {},
Eigen::VectorXd upper_tolerance = {});
const Eigen::VectorXi& indices = Eigen::Matrix<int, 1, 6>(std::vector<int>({ 0, 1, 2, 3, 4, 5 }).data()),
const Eigen::VectorXd& lower_tolerance = {},
const Eigen::VectorXd& upper_tolerance = {});

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

Expand Down Expand Up @@ -195,6 +197,9 @@ struct CartPoseJacCalculator : sco::MatrixOfVector
/** @brief indicates which link is active */
bool is_target_active_{ true };

/** @brief The error function to calculate the error difference used for jacobian calculations */
ErrorDiffFunctionType error_diff_function_;

/**
* @brief This is a vector of indices to be returned Default: {0, 1, 2, 3, 4, 5}
*
Expand All @@ -212,18 +217,7 @@ struct CartPoseJacCalculator : sco::MatrixOfVector
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))
, epsilon_(DEFAULT_EPSILON)
{
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::MatrixXd operator()(const Eigen::VectorXd& dof_vals) const override;
};
Expand Down
188 changes: 121 additions & 67 deletions trajopt/src/kinematic_terms.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,15 +72,15 @@ DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator(tesseract_kinematics:
std::string target_frame,
const Eigen::Isometry3d& source_frame_offset,
const Eigen::Isometry3d& target_frame_offset,
Eigen::VectorXi indices,
Eigen::VectorXd lower_tolerance,
Eigen::VectorXd upper_tolerance)
const Eigen::VectorXi& indices,
const Eigen::VectorXd& lower_tolerance,
const Eigen::VectorXd& upper_tolerance)
: 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))
, indices_(indices)
{
assert(indices_.size() <= 6);

Expand All @@ -96,16 +96,16 @@ DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator(tesseract_kinematics:
if ((lower_tolerance.size() == 0 && upper_tolerance.size() == 0) ||
tesseract_common::almostEqualRelativeAndAbs(lower_tolerance, upper_tolerance))
{
error_function = [this](const Eigen::Isometry3d& source_tf, const Eigen::Isometry3d& target_tf) -> Eigen::VectorXd {
return tesseract_common::calcTransformError(source_tf, target_tf);
error_function = [this](const Eigen::Isometry3d& target_tf, const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd {
return tesseract_common::calcTransformError(target_tf, source_tf);
};
}
else
{
error_function = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& source_tf,
const Eigen::Isometry3d& target_tf) -> Eigen::VectorXd {
error_function = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd {
// Calculate the error using tesseract_common::calcTransformError or equivalent
Eigen::VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf);
VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
Expand Down Expand Up @@ -152,28 +152,26 @@ void DynamicCartPoseErrCalculator::Plot(const tesseract_visualization::Visualiza
MatrixXd DynamicCartPoseJacCalculator::operator()(const VectorXd& dof_vals) const
{
// Duplicated from calcForwardNumJac in trajopt_sco/src/num_diff.cpp, but with ignoring tolerances
auto calc_error = [this](const VectorXd& vals) -> VectorXd {
tesseract_common::TransformMap state = manip_->calcFwdKin(vals);
Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
Isometry3d target_tf = state[target_frame_] * target_frame_offset_;
VectorXd err;
err = tesseract_common::calcTransformErrorJac(target_tf, source_tf);

VectorXd reduced_err(indices_.size());
for (int i = 0; i < indices_.size(); ++i)
reduced_err[i] = err[indices_[i]];

return reduced_err;
};
tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals);
Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

VectorXd err = calc_error(dof_vals);
Eigen::MatrixXd jac0(err.size(), dof_vals.size());
Eigen::MatrixXd jac0(indices_.size(), dof_vals.size());
Eigen::VectorXd dof_vals_pert = dof_vals;
for (int i = 0; i < dof_vals.size(); ++i)
{
dof_vals_pert(i) = dof_vals(i) + epsilon_;
VectorXd err_pert = calc_error(dof_vals_pert);
jac0.col(i) = (err_pert - err) / epsilon_;
tesseract_common::TransformMap state_pert = manip_->calcFwdKin(dof_vals_pert);
Isometry3d source_tf_pert = state_pert[source_frame_] * source_frame_offset_;
Isometry3d target_tf_pert = state_pert[target_frame_] * target_frame_offset_;
VectorXd error_diff =
tesseract_common::calcJacobianTransformErrorDiff(target_tf, target_tf_pert, source_tf, source_tf_pert);

VectorXd reduced_error_diff(indices_.size());
for (int i = 0; i < indices_.size(); ++i)
reduced_error_diff[i] = error_diff[indices_[i]];

jac0.col(i) = reduced_error_diff / epsilon_;
dof_vals_pert(i) = dof_vals(i);
}

Expand All @@ -185,15 +183,15 @@ CartPoseErrCalculator::CartPoseErrCalculator(tesseract_kinematics::JointGroup::C
std::string target_frame,
const Eigen::Isometry3d& source_frame_offset,
const Eigen::Isometry3d& target_frame_offset,
Eigen::VectorXi indices,
Eigen::VectorXd lower_tolerance,
Eigen::VectorXd upper_tolerance)
const Eigen::VectorXi& indices,
const VectorXd& lower_tolerance,
const VectorXd& upper_tolerance)
: 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))
, indices_(indices)
{
assert(indices_.size() <= 6);
is_target_active_ = manip_->isActiveLinkName(target_frame_);
Expand All @@ -210,21 +208,45 @@ CartPoseErrCalculator::CartPoseErrCalculator(tesseract_kinematics::JointGroup::C
if ((lower_tolerance.size() == 0 && upper_tolerance.size() == 0) ||
tesseract_common::almostEqualRelativeAndAbs(lower_tolerance, upper_tolerance))
{
error_function_ = [this](const Eigen::Isometry3d& source_tf,
const Eigen::Isometry3d& target_tf) -> Eigen::VectorXd {
return tesseract_common::calcTransformError(source_tf, target_tf);
};
if (is_target_active_)
{
error_function_ = [this](const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd {
return tesseract_common::calcTransformError(source_tf, target_tf);
};
}
else
{
error_function_ = [this](const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd {
return tesseract_common::calcTransformError(target_tf, source_tf);
};
}
}
else
{
error_function_ = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& source_tf,
const Eigen::Isometry3d& target_tf) -> Eigen::VectorXd {
// Calculate the error using tesseract_common::calcTransformError or equivalent
Eigen::VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
};
if (is_target_active_)
{
error_function_ = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd {
// Calculate the error using tesseract_common::calcTransformError or equivalent
VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
};
}
else
{
error_function_ = [lower_tolerance, upper_tolerance](const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> Eigen::VectorXd {
// Calculate the error using tesseract_common::calcTransformError or equivalent
VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
};
}
}
}

Expand All @@ -234,11 +256,7 @@ VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const
Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

VectorXd err;
if (is_target_active_)
err = error_function_(source_tf, target_tf);
else
err = error_function_(target_tf, source_tf);
VectorXd err = error_function_(target_tf, source_tf);

VectorXd reduced_err(indices_.size());
for (int i = 0; i < indices_.size(); ++i)
Expand Down Expand Up @@ -267,34 +285,70 @@ void CartPoseErrCalculator::Plot(const tesseract_visualization::Visualization::P
plotter->plotMarker(m3);
}

MatrixXd CartPoseJacCalculator::operator()(const VectorXd& dof_vals) const
CartPoseJacCalculator::CartPoseJacCalculator(tesseract_kinematics::JointGroup::ConstPtr manip,
std::string source_frame,
std::string target_frame,
const Eigen::Isometry3d& source_frame_offset,
const Eigen::Isometry3d& target_frame_offset,
Eigen::VectorXi indices)
: 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))
, epsilon_(DEFAULT_EPSILON)
{
// Duplicated from calcForwardNumJac in trajopt_sco/src/num_diff.cpp, but with ignoring tolerances
auto calc_error = [this](const VectorXd& vals) -> VectorXd {
tesseract_common::TransformMap state = manip_->calcFwdKin(vals);
Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
Isometry3d target_tf = state[target_frame_] * target_frame_offset_;
VectorXd err;
if (is_target_active_)
err = tesseract_common::calcTransformErrorJac(source_tf, target_tf);
else
err = tesseract_common::calcTransformErrorJac(target_tf, source_tf);
is_target_active_ = manip_->isActiveLinkName(target_frame_);
assert(indices_.size() <= 6);

VectorXd reduced_err(indices_.size());
for (int i = 0; i < indices_.size(); ++i)
reduced_err[i] = err[indices_[i]];
if (is_target_active_)
{
error_diff_function_ = [this](const VectorXd& vals,
const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> VectorXd {
tesseract_common::TransformMap perturbed_state = manip_->calcFwdKin(vals);
Isometry3d perturbed_target_tf = perturbed_state[target_frame_] * target_frame_offset_;
VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(source_tf, target_tf, perturbed_target_tf);

VectorXd reduced_error_diff(indices_.size());
for (int i = 0; i < indices_.size(); ++i)
reduced_error_diff[i] = error_diff[indices_[i]];

return reduced_error_diff;
};
}
else
{
error_diff_function_ = [this](const VectorXd& vals,
const Eigen::Isometry3d& target_tf,
const Eigen::Isometry3d& source_tf) -> VectorXd {
tesseract_common::TransformMap perturbed_state = manip_->calcFwdKin(vals);
Isometry3d perturbed_source_tf = perturbed_state[source_frame_] * source_frame_offset_;
VectorXd error_diff = tesseract_common::calcJacobianTransformErrorDiff(target_tf, source_tf, perturbed_source_tf);

VectorXd reduced_error_diff(indices_.size());
for (int i = 0; i < indices_.size(); ++i)
reduced_error_diff[i] = error_diff[indices_[i]];

return reduced_error_diff;
};
}
}

return reduced_err;
};
MatrixXd CartPoseJacCalculator::operator()(const VectorXd& dof_vals) const
{
tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals);
Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

VectorXd err = calc_error(dof_vals);
Eigen::MatrixXd jac0(err.size(), dof_vals.size());
Eigen::MatrixXd jac0(indices_.size(), dof_vals.size());
Eigen::VectorXd dof_vals_pert = dof_vals;
for (int i = 0; i < dof_vals.size(); ++i)
{
dof_vals_pert(i) = dof_vals(i) + epsilon_;
VectorXd err_pert = calc_error(dof_vals_pert);
jac0.col(i) = (err_pert - err) / epsilon_;
VectorXd error_diff = error_diff_function_(dof_vals_pert, target_tf, source_tf);
jac0.col(i) = error_diff / epsilon_;
dof_vals_pert(i) = dof_vals(i);
}

Expand Down
7 changes: 6 additions & 1 deletion trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,8 @@ class CartLineConstraint : public ifopt::ConstraintSet

using Ptr = std::shared_ptr<CartLineConstraint>;
using ConstPtr = std::shared_ptr<const CartLineConstraint>;
using ErrorDiffFunctionType =
std::function<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::Isometry3d&, const Eigen::Isometry3d&)>;

CartLineConstraint(CartLineInfo info,
JointPosition::ConstPtr position_var,
Expand Down Expand Up @@ -190,6 +192,9 @@ class CartLineConstraint : public ifopt::ConstraintSet

/** @brief The cartesian line information used when calculating error */
CartLineInfo info_;

/** @brief The error function to calculate the error difference used for jacobian calculations */
ErrorDiffFunctionType error_diff_function_{ nullptr };
};
}; // namespace trajopt_ifopt
} // namespace trajopt_ifopt
#endif
11 changes: 10 additions & 1 deletion trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,9 @@ class CartPosConstraint : public ifopt::ConstraintSet

using Ptr = std::shared_ptr<CartPosConstraint>;
using ConstPtr = std::shared_ptr<const CartPosConstraint>;
using ErrorFunctionType = std::function<Eigen::VectorXd(const Eigen::Isometry3d&, const Eigen::Isometry3d&)>;
using ErrorDiffFunctionType =
std::function<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::Isometry3d&, const Eigen::Isometry3d&)>;

CartPosConstraint(CartPosInfo info, JointPosition::ConstPtr position_var, const std::string& name = "CartPos");

Expand Down Expand Up @@ -183,6 +186,12 @@ class CartPosConstraint : public ifopt::ConstraintSet

/** @brief The kinematic information used when calculating error */
CartPosInfo info_;

/** @brief Error function for calculating the error in the position given the source and target positions */
ErrorFunctionType error_function_{ nullptr };

/** @brief The error function to calculate the error difference used for jacobian calculations */
ErrorDiffFunctionType error_diff_function_{ nullptr };
};
}; // namespace trajopt_ifopt
} // namespace trajopt_ifopt
#endif
Loading

0 comments on commit b921e47

Please sign in to comment.