Skip to content

Commit

Permalink
Update based on changes in trajopt
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Oct 10, 2023
1 parent 6d0e676 commit 6083378
Show file tree
Hide file tree
Showing 4 changed files with 13 additions and 13 deletions.
4 changes: 2 additions & 2 deletions tesseract_examples/src/online_planning_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -199,12 +199,12 @@ bool OnlinePlanningExample::setupProblem(const std::vector<Eigen::VectorXd>& ini
// Add a collision cost for all steps
double margin_coeff = 10;
double margin = 0.1;
auto collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
auto collision_config = std::make_shared<trajopt_common::TrajOptCollisionConfig>(margin, margin_coeff);
collision_config->contact_request.type = tesseract_collision::ContactTestType::ALL;
collision_config->type = CollisionEvaluatorType::DISCRETE;
collision_config->collision_margin_buffer = 0.10;

auto collision_cache = std::make_shared<trajopt_ifopt::CollisionCache>(steps_);
auto collision_cache = std::make_shared<trajopt_common::CollisionCache>(steps_);
if (use_continuous_)
{
std::array<bool, 2> position_vars_fixed{ true, false };
Expand Down
8 changes: 4 additions & 4 deletions ...tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,12 +48,12 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);

/** @brief Configuration info for collisions that are modeled as costs */
trajopt_ifopt::TrajOptCollisionConfig::Ptr collision_cost_config{
std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>()
trajopt_common::TrajOptCollisionConfig::Ptr collision_cost_config{
std::make_shared<trajopt_common::TrajOptCollisionConfig>()
};
/** @brief Configuration info for collisions that are modeled as constraints */
trajopt_ifopt::TrajOptCollisionConfig::Ptr collision_constraint_config{
std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>()
trajopt_common::TrajOptCollisionConfig::Ptr collision_constraint_config{
std::make_shared<trajopt_common::TrajOptCollisionConfig>()
};
/** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
bool smooth_velocities = true;
Expand Down
6 changes: 3 additions & 3 deletions ...nners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,22 +84,22 @@ std::vector<ifopt::ConstraintSet::Ptr>
createCollisionConstraints(const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
const tesseract_environment::Environment::ConstPtr& env,
const tesseract_common::ManipulatorInfo& manip_info,
const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
const std::vector<int>& fixed_indices,
bool fixed_sparsity = true);

bool addCollisionConstraint(trajopt_sqp::QPProblem& nlp,
const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
const tesseract_environment::Environment::ConstPtr& env,
const tesseract_common::ManipulatorInfo& manip_info,
const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
const std::vector<int>& fixed_indices);

bool addCollisionCost(trajopt_sqp::QPProblem& nlp,
const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
const tesseract_environment::Environment::ConstPtr& env,
const tesseract_common::ManipulatorInfo& manip_info,
const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
const std::vector<int>& fixed_indices);

ifopt::ConstraintSet::Ptr createJointVelocityConstraint(const Eigen::Ref<const Eigen::VectorXd>& target,
Expand Down
8 changes: 4 additions & 4 deletions tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ std::vector<ifopt::ConstraintSet::Ptr>
createCollisionConstraints(const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
const tesseract_environment::Environment::ConstPtr& env,
const tesseract_common::ManipulatorInfo& manip_info,
const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
const std::vector<int>& fixed_indices,
bool fixed_sparsity)
{
Expand All @@ -190,7 +190,7 @@ createCollisionConstraints(const std::vector<trajopt_ifopt::JointPosition::Const
return constraints;

// Add a collision cost for all steps
auto collision_cache = std::make_shared<trajopt_ifopt::CollisionCache>(vars.size());
auto collision_cache = std::make_shared<trajopt_common::CollisionCache>(vars.size());
std::unordered_map<std::string, tesseract_kinematics::JointGroup::ConstPtr> manipulators;
if (config->type == tesseract_collision::CollisionEvaluatorType::DISCRETE)
{
Expand Down Expand Up @@ -322,7 +322,7 @@ bool addCollisionConstraint(trajopt_sqp::QPProblem& nlp,
const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
const tesseract_environment::Environment::ConstPtr& env,
const tesseract_common::ManipulatorInfo& manip_info,
const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
const std::vector<int>& fixed_indices)
{
auto constraints = createCollisionConstraints(vars, env, manip_info, config, fixed_indices);
Expand All @@ -335,7 +335,7 @@ bool addCollisionCost(trajopt_sqp::QPProblem& nlp,
const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
const tesseract_environment::Environment::ConstPtr& env,
const tesseract_common::ManipulatorInfo& manip_info,
const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
const std::vector<int>& fixed_indices)
{
// Coefficients are applied within the constraint
Expand Down

0 comments on commit 6083378

Please sign in to comment.