Skip to content

Commit

Permalink
Trajopt clang-tidy clean-up v3
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen committed Jul 16, 2024
1 parent 8109f58 commit 1aff0c1
Show file tree
Hide file tree
Showing 96 changed files with 1,200 additions and 1,163 deletions.
8 changes: 4 additions & 4 deletions trajopt/include/trajopt/problem_description.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,25 +35,25 @@ enum class TermType : char

inline TermType operator|(TermType lhs, TermType rhs)
{
using T = std::underlying_type<TermType>::type;
using T = std::underlying_type_t<TermType>;
return TermType(static_cast<T>(lhs) | static_cast<T>(rhs));
}

inline TermType operator&(TermType lhs, TermType rhs)
{
using T = std::underlying_type<TermType>::type;
using T = std::underlying_type_t<TermType>;
return TermType(static_cast<T>(lhs) & static_cast<T>(rhs));
}

inline TermType operator^(TermType lhs, TermType rhs)
{
using T = std::underlying_type<TermType>::type;
using T = std::underlying_type_t<TermType>;
return TermType(static_cast<T>(lhs) ^ static_cast<T>(rhs));
}

inline TermType operator~(TermType rhs)
{
using T = std::underlying_type<TermType>::type;
using T = std::underlying_type_t<TermType>;
return TermType(~static_cast<T>(rhs));
}

Expand Down
4 changes: 2 additions & 2 deletions trajopt/include/trajopt/typedefs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ class TrajOptCostFromErrFunc : public sco::CostFromErrFunc, public Plotter
// If error function has a inherited from TrajOptVectorOfVector, call its Plot function
if (auto* plt = dynamic_cast<TrajOptVectorOfVector*>(f_.get()))
{
Eigen::VectorXd dof_vals = sco::getVec(x, vars_);
const Eigen::VectorXd dof_vals = sco::getVec(x, vars_);
plt->Plot(plotter, dof_vals);
}
}
Expand Down Expand Up @@ -120,7 +120,7 @@ class TrajOptConstraintFromErrFunc : public sco::ConstraintFromErrFunc, public P
// If error function has a inherited from TrajOptVectorOfVector, call its Plot function
if (auto* plt = dynamic_cast<TrajOptVectorOfVector*>(f_.get()))
{
Eigen::VectorXd dof_vals = sco::getVec(x, vars_);
const Eigen::VectorXd dof_vals = sco::getVec(x, vars_);
plt->Plot(plotter, dof_vals);
}
}
Expand Down
120 changes: 64 additions & 56 deletions trajopt/src/collision_terms.cpp
View file

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions trajopt/src/file_write_callback.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void WriteFile(const std::shared_ptr<std::ofstream>& file,
}

// Calc cartesian pose
tesseract_common::TransformMap state = manip->calcFwdKin(joint_angles);
const tesseract_common::TransformMap state = manip->calcFwdKin(joint_angles);

for (const auto& it : state)
{
Expand Down Expand Up @@ -113,7 +113,7 @@ std::function<void(sco::OptProb*, sco::OptResults&)> WriteCallback(std::shared_p
}

// Write cartesian pose labels
std::vector<std::string> pose_str = std::vector<std::string>{ "x", "y", "z", "q_w", "q_x", "q_y", "q_z" };
const std::vector<std::string> pose_str = std::vector<std::string>{ "x", "y", "z", "q_w", "q_x", "q_y", "q_z" };
for (const auto& i : pose_str)
{
*file << ',' << i;
Expand Down
86 changes: 43 additions & 43 deletions trajopt/src/kinematic_terms.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,7 +110,7 @@ DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator(
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);
const VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
Expand All @@ -121,8 +121,8 @@ DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator(
VectorXd DynamicCartPoseErrCalculator::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_;
const Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
const Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

VectorXd err = error_function(target_tf, source_tf);

Expand Down Expand Up @@ -176,17 +176,17 @@ MatrixXd DynamicCartPoseJacCalculator::operator()(const VectorXd& dof_vals) cons
{
// Duplicated from calcForwardNumJac in trajopt_sco/src/num_diff.cpp, but with ignoring tolerances
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_;
const Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
const Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

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_;
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_;
const Isometry3d source_tf_pert = state_pert[source_frame_] * source_frame_offset_;
const 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);

Expand Down Expand Up @@ -254,7 +254,7 @@ CartPoseErrCalculator::CartPoseErrCalculator(
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);
const VectorXd err = tesseract_common::calcTransformError(source_tf, target_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
Expand All @@ -265,7 +265,7 @@ CartPoseErrCalculator::CartPoseErrCalculator(
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);
const VectorXd err = tesseract_common::calcTransformError(target_tf, source_tf);

// Apply tolerances
return applyTolerances(err, lower_tolerance, upper_tolerance);
Expand All @@ -277,8 +277,8 @@ CartPoseErrCalculator::CartPoseErrCalculator(
VectorXd CartPoseErrCalculator::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_;
const Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
const Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

VectorXd err = error_function_(target_tf, source_tf);

Expand Down Expand Up @@ -334,7 +334,7 @@ CartPoseJacCalculator::CartPoseJacCalculator(
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_;
const 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());
Expand All @@ -350,7 +350,7 @@ CartPoseJacCalculator::CartPoseJacCalculator(
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_;
const 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());
Expand All @@ -365,15 +365,15 @@ CartPoseJacCalculator::CartPoseJacCalculator(
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_;
const Isometry3d source_tf = state[source_frame_] * source_frame_offset_;
const Isometry3d target_tf = state[target_frame_] * target_frame_offset_;

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 error_diff = error_diff_function_(dof_vals_pert, target_tf, source_tf);
const 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 Expand Up @@ -444,7 +444,7 @@ Eigen::VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) cons
assert(var_vals.rows() % 2 == 0);
// Top half of the vector are the joint values. The bottom half are the 1/dt values
auto half = static_cast<int>(var_vals.rows() / 2);
int num_vels = half - 1;
const int num_vels = half - 1;
// (x1-x0)*(1/dt)
VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() *
var_vals.segment(half + 1, num_vels).array();
Expand Down Expand Up @@ -489,7 +489,7 @@ VectorXd JointAccErrCalculator::operator()(const VectorXd& var_vals) const
{
assert(var_vals.rows() % 2 == 0);
auto half = static_cast<int>(var_vals.rows() / 2);
int num_acc = half - 2;
const int num_acc = half - 2;
VectorXd vels = vel_calc(var_vals);

// v1-v0
Expand All @@ -505,18 +505,18 @@ VectorXd JointAccErrCalculator::operator()(const VectorXd& var_vals) const
MatrixXd JointAccJacCalculator::operator()(const VectorXd& var_vals) const
{
auto num_vals = static_cast<int>(var_vals.rows());
int half = num_vals / 2;
const int half = num_vals / 2;
MatrixXd jac = MatrixXd::Zero(half - 2, num_vals);

VectorXd vels = vel_calc(var_vals);
MatrixXd vel_jac = vel_jac_calc(var_vals);
for (int i = 0; i < jac.rows(); i++)
{
int dt_1_index = i + half + 1;
int dt_2_index = dt_1_index + 1;
double dt_1 = var_vals(dt_1_index);
double dt_2 = var_vals(dt_2_index);
double total_dt = dt_1 + dt_2;
const int dt_1_index = i + half + 1;
const int dt_2_index = dt_1_index + 1;
const double dt_1 = var_vals(dt_1_index);
const double dt_2 = var_vals(dt_2_index);
const double total_dt = dt_1 + dt_2;

jac(i, i) = 2.0 * (vel_jac(i + 1, i) - vel_jac(i, i)) / total_dt;
jac(i, i + 1) = 2.0 * (vel_jac(i + 1, i + 1) - vel_jac(i, i + 1)) / total_dt;
Expand All @@ -536,7 +536,7 @@ VectorXd JointJerkErrCalculator::operator()(const VectorXd& var_vals) const
{
assert(var_vals.rows() % 2 == 0);
auto half = static_cast<int>(var_vals.rows() / 2);
int num_jerk = half - 3;
const int num_jerk = half - 3;
VectorXd acc = acc_calc(var_vals);

VectorXd acc_diff = acc.segment(1, num_jerk) - acc.segment(0, num_jerk);
Expand All @@ -552,21 +552,21 @@ VectorXd JointJerkErrCalculator::operator()(const VectorXd& var_vals) const
MatrixXd JointJerkJacCalculator::operator()(const VectorXd& var_vals) const
{
auto num_vals = static_cast<int>(var_vals.rows());
int half = num_vals / 2;
const int half = num_vals / 2;
MatrixXd jac = MatrixXd::Zero(half - 3, num_vals);

VectorXd acc = acc_calc(var_vals);
MatrixXd acc_jac = acc_jac_calc(var_vals);

for (int i = 0; i < jac.rows(); i++)
{
int dt_1_index = i + half + 1;
int dt_2_index = dt_1_index + 1;
int dt_3_index = dt_2_index + 1;
double dt_1 = var_vals(dt_1_index);
double dt_2 = var_vals(dt_2_index);
double dt_3 = var_vals(dt_3_index);
double total_dt = dt_1 + dt_2 + dt_3;
const int dt_1_index = i + half + 1;
const int dt_2_index = dt_1_index + 1;
const int dt_3_index = dt_2_index + 1;
const double dt_1 = var_vals(dt_1_index);
const double dt_2 = var_vals(dt_2_index);
const double dt_3 = var_vals(dt_3_index);
const double total_dt = dt_1 + dt_2 + dt_3;

jac(i, i) = 3.0 * (acc_jac(i + 1, i) - acc_jac(i, i)) / total_dt;
jac(i, i + 1) = 3.0 * (acc_jac(i + 1, i + 1) - acc_jac(i, i + 1)) / total_dt;
Expand Down Expand Up @@ -601,12 +601,12 @@ MatrixXd TimeCostJacCalculator::operator()(const VectorXd& var_vals) const
VectorXd AvoidSingularityErrCalculator::operator()(const VectorXd& var_vals) const
{
// Calculate the SVD of the jacobian at this joint state
MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_);
Eigen::JacobiSVD<MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
const MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_);
const Eigen::JacobiSVD<MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);

// Get the U and V vectors for the smallest singular value
double smallest_sv = svd.singularValues().tail(1)(0);
double cost = 1.0 / (smallest_sv + lambda_);
const double smallest_sv = svd.singularValues().tail(1)(0);
const double cost = 1.0 / (smallest_sv + lambda_);

const static double smallest_allowable_sv = 0.1;
const double threshold = 1.0 / (smallest_allowable_sv + lambda_);
Expand All @@ -623,10 +623,10 @@ MatrixXd AvoidSingularityJacCalculator::jacobianPartialDerivative(const VectorXd
{
// Calculate the jacobian for the given joint perturbed by some epsilon
Eigen::VectorXd joints(state);
double eps = eps_;
const double eps = eps_;
joints(jntIdx) += eps;

MatrixXd jacobian_increment = fwd_kin_->calcJacobian(joints, link_name_);
const MatrixXd jacobian_increment = fwd_kin_->calcJacobian(joints, link_name_);
return (jacobian_increment - jacobian) / eps;
}

Expand All @@ -636,13 +636,13 @@ MatrixXd AvoidSingularityJacCalculator::operator()(const VectorXd& var_vals) con
cost_jacobian.resize(1, var_vals.size());

// Calculate the SVD of the jacobian at this joint state
MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_);
Eigen::JacobiSVD<MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
const MatrixXd jacobian = fwd_kin_->calcJacobian(var_vals, link_name_);
const Eigen::JacobiSVD<MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);

// Get the U and V vectors for the smallest singular value
double smallest_sv = svd.singularValues().tail(1)(0);
const double smallest_sv = svd.singularValues().tail(1)(0);
VectorXd ui = svd.matrixU().rightCols(1);
VectorXd vi = svd.matrixV().rightCols(1);
const VectorXd vi = svd.matrixV().rightCols(1);

// Calculate the jacobian partial derivative for each joint, perturbing it slightly
for (Eigen::Index jntIdx = 0; jntIdx < var_vals.size(); ++jntIdx)
Expand Down
Loading

0 comments on commit 1aff0c1

Please sign in to comment.