Skip to content

Commit c791e74

Browse files
rjoomenrjoomen
authored
Adding Trajopt_Ifopt option to all examples (#389)
1 parent 981dd03 commit c791e74

24 files changed

+581
-131
lines changed

‎.clang-format‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false
88
AllowShortFunctionsOnASingleLine: true
99
AllowShortIfStatementsOnASingleLine: false
1010
AllowShortLoopsOnASingleLine: false
11-
AllowShortLoopsOnASingleLine: false
1211
AlwaysBreakBeforeMultilineStrings: false
1312
AlwaysBreakTemplateDeclarations: true
1413
BinPackArguments: false

‎.github/workflows/unstable.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ on:
1818

1919
jobs:
2020
ci:
21-
name: ${{ matrix.distro }}
21+
name: ${{ matrix.distro }}-unstable
2222
runs-on: ubuntu-latest
2323
strategy:
2424
fail-fast: false

‎tesseract_examples/include/tesseract_examples/car_seat_example.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,6 @@
3030
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3131
#include <string>
3232
#include <vector>
33-
#include <memory>
3433
#include <Eigen/Geometry>
3534
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3635

@@ -44,7 +43,9 @@ class CarSeatExample : public Example
4443
{
4544
public:
4645
CarSeatExample(tesseract_environment::Environment::Ptr env,
47-
tesseract_visualization::Visualization::Ptr plotter = nullptr);
46+
tesseract_visualization::Visualization::Ptr plotter = nullptr,
47+
bool ifopt = false,
48+
bool debug = false);
4849
~CarSeatExample() override = default;
4950
CarSeatExample(const CarSeatExample&) = default;
5051
CarSeatExample& operator=(const CarSeatExample&) = default;
@@ -54,6 +55,8 @@ class CarSeatExample : public Example
5455
bool run() override final;
5556

5657
private:
58+
bool ifopt_;
59+
bool debug_;
5760
std::unordered_map<std::string, std::unordered_map<std::string, double>> saved_positions_;
5861

5962
static std::unordered_map<std::string, std::unordered_map<std::string, double>> getPredefinedPosition();

‎tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,9 +27,6 @@
2727
#define TESSERACT_EXAMPLES_FREESPACE_HYBRID_EXAMPLE_H
2828

2929
#include <tesseract_common/macros.h>
30-
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
31-
#include <string>
32-
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3330

3431
#include <tesseract_examples/example.h>
3532

@@ -44,6 +41,8 @@ class FreespaceHybridExample : public Example
4441
public:
4542
FreespaceHybridExample(tesseract_environment::Environment::Ptr env,
4643
tesseract_visualization::Visualization::Ptr plotter = nullptr,
44+
bool ifopt = false,
45+
bool debug = false,
4746
double range = 0.01,
4847
double planning_time = 60.0);
4948
~FreespaceHybridExample() override = default;
@@ -55,6 +54,8 @@ class FreespaceHybridExample : public Example
5554
bool run() override final;
5655

5756
private:
57+
bool ifopt_;
58+
bool debug_;
5859
double range_;
5960
double planning_time_;
6061

‎tesseract_examples/include/tesseract_examples/pick_and_place_example.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,8 @@ class PickAndPlaceExample : public Example
4545
public:
4646
PickAndPlaceExample(tesseract_environment::Environment::Ptr env,
4747
tesseract_visualization::Visualization::Ptr plotter = nullptr,
48+
bool ifopt = false,
49+
bool debug = false,
4850
double box_size = 0.2,
4951
std::array<double, 2> box_position = { 0.15, 0.4 });
5052
~PickAndPlaceExample() override = default;
@@ -56,6 +58,8 @@ class PickAndPlaceExample : public Example
5658
bool run() override final;
5759

5860
private:
61+
bool ifopt_;
62+
bool debug_;
5963
double box_size_;
6064
std::array<double, 2> box_position_;
6165
static tesseract_environment::Command::Ptr addBox(double box_x, double box_y, double box_side);

‎tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,9 @@ class PuzzlePieceAuxillaryAxesExample : public Example
4747
{
4848
public:
4949
PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env,
50-
tesseract_visualization::Visualization::Ptr plotter = nullptr);
50+
tesseract_visualization::Visualization::Ptr plotter = nullptr,
51+
bool ifopt = false,
52+
bool debug = false);
5153
~PuzzlePieceAuxillaryAxesExample() override = default;
5254
PuzzlePieceAuxillaryAxesExample(const PuzzlePieceAuxillaryAxesExample&) = default;
5355
PuzzlePieceAuxillaryAxesExample& operator=(const PuzzlePieceAuxillaryAxesExample&) = default;
@@ -57,6 +59,8 @@ class PuzzlePieceAuxillaryAxesExample : public Example
5759
bool run() override final;
5860

5961
private:
62+
bool ifopt_;
63+
bool debug_;
6064
static tesseract_common::VectorIsometry3d
6165
makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator);
6266
};

‎tesseract_examples/include/tesseract_examples/puzzle_piece_example.h‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,6 @@
2929

3030
#include <tesseract_common/macros.h>
3131
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
32-
#include <string>
3332
TESSERACT_COMMON_IGNORE_WARNINGS_POP
3433

3534
#include <tesseract_common/types.h>
@@ -45,7 +44,9 @@ class PuzzlePieceExample : public Example
4544
{
4645
public:
4746
PuzzlePieceExample(tesseract_environment::Environment::Ptr env,
48-
tesseract_visualization::Visualization::Ptr plotter = nullptr);
47+
tesseract_visualization::Visualization::Ptr plotter = nullptr,
48+
bool ifopt = false,
49+
bool debug = false);
4950
~PuzzlePieceExample() override = default;
5051
PuzzlePieceExample(const PuzzlePieceExample&) = default;
5152
PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default;
@@ -55,6 +56,8 @@ class PuzzlePieceExample : public Example
5556
bool run() override final;
5657

5758
private:
59+
bool ifopt_;
60+
bool debug_;
5861
static tesseract_common::VectorIsometry3d
5962
makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator);
6063
};

‎tesseract_examples/src/basic_cartesian_example.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,8 @@ bool BasicCartesianExample::run()
152152

153153
if (debug_)
154154
console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
155+
else
156+
console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
155157

156158
// Create Task Composer Plugin Factory
157159
const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);

‎tesseract_examples/src/car_seat_example.cpp‎

Lines changed: 78 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
4747

4848
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h>
4949
#include <tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h>
50+
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
51+
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
52+
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h>
5053
#include <tesseract_motion_planners/core/utils.h>
5154
#include <tesseract_visualization/markers/toolpath_marker.h>
5255

@@ -58,6 +61,7 @@ using namespace tesseract_visualization;
5861
using namespace tesseract_planning;
5962
using tesseract_common::ManipulatorInfo;
6063

64+
static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask";
6165
static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask";
6266
namespace tesseract_examples
6367
{
@@ -112,8 +116,10 @@ Commands addSeats(const tesseract_common::ResourceLocator::ConstPtr& locator)
112116
}
113117

114118
CarSeatExample::CarSeatExample(tesseract_environment::Environment::Ptr env,
115-
tesseract_visualization::Visualization::Ptr plotter)
116-
: Example(std::move(env), std::move(plotter))
119+
tesseract_visualization::Visualization::Ptr plotter,
120+
bool ifopt,
121+
bool debug)
122+
: Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug)
117123
{
118124
}
119125

@@ -215,7 +221,10 @@ Eigen::VectorXd CarSeatExample::getPositionVectorXd(const JointGroup& joint_grou
215221

216222
bool CarSeatExample::run()
217223
{
218-
console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
224+
if (debug_)
225+
console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG);
226+
else
227+
console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO);
219228

220229
// Create Task Composer Plugin Factory
221230
const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR);
@@ -245,25 +254,70 @@ bool CarSeatExample::run()
245254
// Create Executor
246255
auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");
247256

248-
// Create TrajOpt Profile
249-
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
250-
trajopt_composite_profile->collision_constraint_config.enabled = true;
251-
trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00;
252-
trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
253-
trajopt_composite_profile->collision_constraint_config.coeff = 10;
254-
trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
255-
trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
256-
trajopt_composite_profile->collision_cost_config.coeff = 50;
257-
258-
auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
259-
trajopt_solver_profile->opt_info.max_iter = 200;
260-
trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
261-
trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
262-
263257
// Create profile dictionary
264258
auto profiles = std::make_shared<ProfileDictionary>();
265-
profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile);
266-
profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile);
259+
if (ifopt_)
260+
{
261+
// Create TrajOpt_Ifopt Profile
262+
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
263+
trajopt_ifopt_composite_profile->collision_constraint_config->type =
264+
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
265+
trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config =
266+
tesseract_collision::ContactManagerConfig(0.00);
267+
trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005;
268+
trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data =
269+
trajopt_common::CollisionCoeffData(1);
270+
trajopt_ifopt_composite_profile->collision_cost_config->type =
271+
tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS;
272+
trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config =
273+
tesseract_collision::ContactManagerConfig(0.005);
274+
trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01;
275+
trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data =
276+
trajopt_common::CollisionCoeffData(50);
277+
trajopt_ifopt_composite_profile->smooth_velocities = false;
278+
trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1);
279+
trajopt_ifopt_composite_profile->smooth_accelerations = true;
280+
trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1);
281+
trajopt_ifopt_composite_profile->smooth_jerks = true;
282+
trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1);
283+
trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.1;
284+
285+
auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
286+
trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
287+
trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);
288+
289+
auto trajopt_ifopt_solver_profile = std::make_shared<TrajOptIfoptDefaultSolverProfile>();
290+
trajopt_ifopt_solver_profile->opt_info.max_iterations = 200;
291+
trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3;
292+
trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
293+
294+
profiles->addProfile<TrajOptIfoptCompositeProfile>(
295+
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile);
296+
profiles->addProfile<TrajOptIfoptPlanProfile>(
297+
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile);
298+
profiles->addProfile<TrajOptIfoptSolverProfile>(
299+
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile);
300+
}
301+
else
302+
{
303+
// Create TrajOpt Profile
304+
auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
305+
trajopt_composite_profile->collision_constraint_config.enabled = true;
306+
trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00;
307+
trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005;
308+
trajopt_composite_profile->collision_constraint_config.coeff = 10;
309+
trajopt_composite_profile->collision_cost_config.safety_margin = 0.005;
310+
trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01;
311+
trajopt_composite_profile->collision_cost_config.coeff = 50;
312+
313+
auto trajopt_solver_profile = std::make_shared<TrajOptDefaultSolverProfile>();
314+
trajopt_solver_profile->opt_info.max_iter = 200;
315+
trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
316+
trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3;
317+
318+
profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile);
319+
profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile);
320+
}
267321

268322
// Solve Trajectory
269323
CONSOLE_BRIDGE_logInform("Car Seat Demo Started");
@@ -297,7 +351,8 @@ bool CarSeatExample::run()
297351
CONSOLE_BRIDGE_logInform("Freespace plan to pick seat 1 example");
298352

299353
// Create task
300-
TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline");
354+
const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline";
355+
TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
301356
const std::string output_key = task->getOutputKeys().front();
302357

303358
// Create Task Composer Problem
@@ -381,7 +436,8 @@ bool CarSeatExample::run()
381436
CONSOLE_BRIDGE_logInform("Freespace plan to pick seat 1 example");
382437

383438
// Create task
384-
TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline");
439+
const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline";
440+
TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name);
385441
const std::string output_key = task->getOutputKeys().front();
386442

387443
// Create Task Composer Problem

‎tesseract_examples/src/car_seat_example_node.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ int main(int /*argc*/, char** /*argv*/)
4242
if (!env->init(urdf_path, srdf_path, locator))
4343
exit(1);
4444

45-
CarSeatExample example(env, nullptr);
45+
CarSeatExample example(env, nullptr, false);
4646
if (!example.run())
4747
exit(1);
4848
}

0 commit comments

Comments
 (0)