Skip to content

Commit

Permalink
Clang tidy
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpowelson
mpowelson committed Dec 30, 2020
1 parent 535286b commit ed312b7
Show file tree
Hide file tree
Showing 8 changed files with 58 additions and 31 deletions.
12 changes: 6 additions & 6 deletions ...nning/tesseract_motion_planners/include/tesseract_motion_planners/trajopt/trajopt_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,12 +37,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
namespace tesseract_planning
{
trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
int index,
std::string working_frame,
Eigen::Isometry3d tcp,
const Eigen::VectorXd& coeffs,
std::string link,
trajopt::TermType type);
int index,
std::string working_frame,
Eigen::Isometry3d tcp,
const Eigen::VectorXd& coeffs,
std::string link,
trajopt::TermType type);

trajopt::TermInfo::Ptr createDynamicCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
int index,
Expand Down
10 changes: 10 additions & 0 deletions ..._planners/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,12 @@ class TrajOptIfoptPlanProfile
using Ptr = std::shared_ptr<TrajOptIfoptPlanProfile>;
using ConstPtr = std::shared_ptr<const TrajOptIfoptPlanProfile>;

TrajOptIfoptPlanProfile() = default;
virtual ~TrajOptIfoptPlanProfile() = default;
TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = default;
TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = default;
TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = default;
TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = default;

virtual void apply(TrajOptIfoptProblem& problem,
const CartesianWaypoint& cartesian_waypoint,
Expand All @@ -74,7 +79,12 @@ class TrajOptIfoptCompositeProfile
using Ptr = std::shared_ptr<TrajOptIfoptCompositeProfile>;
using ConstPtr = std::shared_ptr<const TrajOptIfoptCompositeProfile>;

TrajOptIfoptCompositeProfile() = default;
virtual ~TrajOptIfoptCompositeProfile() = default;
TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = default;
TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = default;
TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = default;
TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = default;

virtual void apply(TrajOptIfoptProblem& problem,
int start_index,
Expand Down
8 changes: 4 additions & 4 deletions ...n_planners/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_motion_planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,10 @@ class TrajOptIfoptMotionPlanner : public MotionPlanner
TrajOptIfoptMotionPlanner();

~TrajOptIfoptMotionPlanner() override = default;
TrajOptIfoptMotionPlanner(const TrajOptIfoptMotionPlanner&) = default;
TrajOptIfoptMotionPlanner& operator=(const TrajOptIfoptMotionPlanner&) = default;
TrajOptIfoptMotionPlanner(TrajOptIfoptMotionPlanner&&) = default;
TrajOptIfoptMotionPlanner& operator=(TrajOptIfoptMotionPlanner&&) = default;
TrajOptIfoptMotionPlanner(const TrajOptIfoptMotionPlanner&) = delete;
TrajOptIfoptMotionPlanner& operator=(const TrajOptIfoptMotionPlanner&) = delete;
TrajOptIfoptMotionPlanner(TrajOptIfoptMotionPlanner&&) = delete;
TrajOptIfoptMotionPlanner& operator=(TrajOptIfoptMotionPlanner&&) = delete;

const std::string& getName() const override;

Expand Down
12 changes: 6 additions & 6 deletions tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,12 @@
namespace tesseract_planning
{
trajopt::TermInfo::Ptr createCartesianWaypointTermInfo(const Eigen::Isometry3d& c_wp,
int index,
std::string working_frame,
Eigen::Isometry3d tcp,
const Eigen::VectorXd& coeffs,
std::string link,
trajopt::TermType type)
int index,
std::string working_frame,
Eigen::Isometry3d tcp,
const Eigen::VectorXd& coeffs,
std::string link,
trajopt::TermType type)
{
auto pose_info = std::make_shared<trajopt::CartPoseTermInfo>();
pose_info->term_type = type;
Expand Down
41 changes: 29 additions & 12 deletions ...seract_motion_planners/src/trajopt_ifopt/problem_generators/default_problem_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,7 +150,8 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
auto interpolate_cnt = static_cast<int>(seed_composite->size());

std::string profile = getProfileString(plan_instruction->getProfile(), name, request.plan_profile_remapping);
TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile<TrajOptIfoptPlanProfile>(profile, plan_profiles, std::make_shared<TrajOptIfoptDefaultPlanProfile>());
TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile<TrajOptIfoptPlanProfile>(
profile, plan_profiles, std::make_shared<TrajOptIfoptDefaultPlanProfile>());
if (!cur_plan_profile)
throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile");

Expand Down Expand Up @@ -195,12 +196,21 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
}
else if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
{
const JointWaypoint& cur_position = *plan_instruction->getWaypoint().cast_const<JointWaypoint>();
if (isStateWaypoint(plan_instruction->getWaypoint()))
; // TODO: Handle this ideally halfway efficiently
const JointWaypoint* cur_position;
std::shared_ptr<const JointWaypoint> temp;
if (isJointWaypoint(plan_instruction->getWaypoint()))
{
cur_position = plan_instruction->getWaypoint().cast_const<JointWaypoint>();
}
else
{
const StateWaypoint* state_waypoint = plan_instruction->getWaypoint().cast_const<StateWaypoint>();
temp = std::make_shared<JointWaypoint>(state_waypoint->joint_names, state_waypoint->position);
cur_position = temp.get();
}

Eigen::Isometry3d cur_pose = Eigen::Isometry3d::Identity();
if (!kin->calcFwdKin(cur_pose, cur_position))
if (!kin->calcFwdKin(cur_pose, *cur_position))
throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: failed to solve forward kinematics!");

cur_pose = env->getCurrentState()->link_transforms.at(kin->getBaseLinkName()) * cur_pose * tcp;
Expand Down Expand Up @@ -234,7 +244,7 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
}

// Add final point with waypoint
cur_plan_profile->apply(*problem, cur_position, *plan_instruction, composite_mi, active_links, index);
cur_plan_profile->apply(*problem, *cur_position, *plan_instruction, composite_mi, active_links, index);

++index;
}
Expand All @@ -247,11 +257,18 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
{
if (isJointWaypoint(plan_instruction->getWaypoint()) || isStateWaypoint(plan_instruction->getWaypoint()))
{
// const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction->getWaypoint()); // This
// doesn't keep tolerances or other metadata
const JointWaypoint& cur_position = *plan_instruction->getWaypoint().cast_const<JointWaypoint>();
if (isStateWaypoint(plan_instruction->getWaypoint()))
; // TODO: Handle this ideally halfway efficiently
const JointWaypoint* cur_position;
std::shared_ptr<const JointWaypoint> temp;
if (isJointWaypoint(plan_instruction->getWaypoint()))
{
cur_position = plan_instruction->getWaypoint().cast_const<JointWaypoint>();
}
else
{
const StateWaypoint* state_waypoint = plan_instruction->getWaypoint().cast_const<StateWaypoint>();
temp = std::make_shared<JointWaypoint>(state_waypoint->joint_names, state_waypoint->position);
cur_position = temp.get();
}

// Increment index to account for intermediate points in seed
for (std::size_t s = 0; s < seed_composite->size() - 1; ++s)
Expand All @@ -260,7 +277,7 @@ DefaultTrajoptIfoptProblemGenerator(const std::string& name,
}

/** @todo Should check that the joint names match the order of the manipulator */
cur_plan_profile->apply(*problem, cur_position, *plan_instruction, manip_info, active_links, i);
cur_plan_profile->apply(*problem, *cur_position, *plan_instruction, manip_info, active_links, i);

// Add to fixed indices
fixed_steps.push_back(i);
Expand Down
3 changes: 2 additions & 1 deletion ...act_motion_planners/src/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,8 @@ void TrajOptIfoptDefaultCompositeProfile::apply(TrajOptIfoptProblem& problem,
const std::vector<std::string>& /*active_links*/,
const std::vector<int>& /*fixed_indices*/) const
{
const std::vector<trajopt::JointPosition::ConstPtr> vars(&problem.vars[static_cast<std::size_t>(start_index)], &problem.vars[static_cast<std::size_t>(end_index)]);
const std::vector<trajopt::JointPosition::ConstPtr> vars(&problem.vars[static_cast<std::size_t>(start_index)],
&problem.vars[static_cast<std::size_t>(end_index)]);

if (collision_constraint_config->type != tesseract_collision::CollisionEvaluatorType::NONE)
addCollisionConstraint(problem.nlp, vars, problem.environment, manip_info, collision_constraint_config);
Expand Down
1 change: 0 additions & 1 deletion ...t_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_collision_config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,5 +35,4 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP

namespace tesseract_planning
{

} // namespace tesseract_planning
2 changes: 1 addition & 1 deletion ...act_planning/tesseract_motion_planners/src/trajopt_ifopt/trajopt_ifopt_motion_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ std::string TrajOptIfoptMotionPlannerStatusCategory::message(int code) const
}

TrajOptIfoptMotionPlanner::TrajOptIfoptMotionPlanner()
: status_category_(std::make_shared<const TrajOptIfoptMotionPlannerStatusCategory>(name_))
: status_category_(std::make_shared<const TrajOptIfoptMotionPlannerStatusCategory>(name_))
{
}

Expand Down

0 comments on commit ed312b7

Please sign in to comment.