From 6c512f9acb6dabf98f2ac5ef48a812b3cb7a7617 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 6 Oct 2023 15:08:29 +0200 Subject: [PATCH] Added ifopt option to pick_and_place_example --- .../pick_and_place_example.h | 2 + tesseract_examples/src/car_seat_example.cpp | 12 +-- .../src/pick_and_place_example.cpp | 90 ++++++++++++++----- 3 files changed, 74 insertions(+), 30 deletions(-) diff --git a/tesseract_examples/include/tesseract_examples/pick_and_place_example.h b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h index 1b7e84453e4..7dafe346078 100644 --- a/tesseract_examples/include/tesseract_examples/pick_and_place_example.h +++ b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h @@ -45,6 +45,7 @@ class PickAndPlaceExample : public Example public: PickAndPlaceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, + bool ifopt = false, double box_size = 0.2, std::array box_position = { 0.15, 0.4 }); ~PickAndPlaceExample() override = default; @@ -56,6 +57,7 @@ class PickAndPlaceExample : public Example bool run() override final; private: + bool ifopt_; double box_size_; std::array box_position_; static tesseract_environment::Command::Ptr addBox(double box_x, double box_y, double box_side); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 27aa4026572..31dac95bc50 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -267,8 +267,7 @@ bool CarSeatExample::run() trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.005); trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01; - trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = - trajopt_ifopt::CollisionCoeffData(5); + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_ifopt::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; @@ -277,13 +276,14 @@ bool CarSeatExample::run() trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.1; - auto plan_profile = std::make_shared(); - plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); - plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); + auto trajopt_ifopt_plan_profile = std::make_shared(); + trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); + trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); } else { diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index c37d8683597..36fe3854ec4 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -24,6 +24,8 @@ * limitations under the License. */ #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include @@ -63,14 +65,16 @@ const std::string LINK_BOX_NAME = "box"; const std::string LINK_BASE_NAME = "world"; const std::string LINK_END_EFFECTOR_NAME = "iiwa_tool0"; const std::string DISCRETE_CONTACT_CHECK_TASK_NAME = "DiscreteContactCheckTask"; +static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"; namespace tesseract_examples { PickAndPlaceExample::PickAndPlaceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, + bool ifopt, double box_size, std::array box_position) - : Example(std::move(env), std::move(plotter)), box_size_(box_size), box_position_(box_position) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), box_size_(box_size), box_position_(box_position) { } @@ -191,30 +195,67 @@ bool PickAndPlaceExample::run() // Create Executor auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - // Create TrajOpt Profile - auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); - - auto trajopt_composite_profile = std::make_shared(); - trajopt_composite_profile->longest_valid_segment_length = 0.05; - trajopt_composite_profile->collision_constraint_config.enabled = true; - trajopt_composite_profile->collision_constraint_config.safety_margin = 0.0; - trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005; - trajopt_composite_profile->collision_constraint_config.coeff = 10; - trajopt_composite_profile->collision_cost_config.safety_margin = 0.005; - trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01; - trajopt_composite_profile->collision_cost_config.coeff = 50; - - auto trajopt_solver_profile = std::make_shared(); - trajopt_solver_profile->opt_info.max_iter = 100; - - auto post_check_profile = std::make_shared(); - // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + if (ifopt_) + { + // Create TrajOpt_Ifopt Profile + auto trajopt_ifopt_composite_profile = std::make_shared(); + trajopt_ifopt_composite_profile->collision_constraint_config->type = + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config = + tesseract_collision::ContactManagerConfig(0.00); + trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005; + trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data = + trajopt_ifopt::CollisionCoeffData(1); + trajopt_ifopt_composite_profile->collision_cost_config->type = + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = + tesseract_collision::ContactManagerConfig(0.005); + trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01; + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_ifopt::CollisionCoeffData(5); + trajopt_ifopt_composite_profile->smooth_velocities = false; + trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_accelerations = true; + trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_jerks = true; + trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.05; + + auto trajopt_ifopt_plan_profile = std::make_shared(); + trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); + trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); + + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + } + else + { + // Create TrajOpt Profile + auto trajopt_plan_profile = std::make_shared(); + trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); + + auto trajopt_composite_profile = std::make_shared(); + trajopt_composite_profile->longest_valid_segment_length = 0.05; + trajopt_composite_profile->collision_constraint_config.enabled = true; + trajopt_composite_profile->collision_constraint_config.safety_margin = 0.0; + trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005; + trajopt_composite_profile->collision_constraint_config.coeff = 10; + trajopt_composite_profile->collision_cost_config.safety_margin = 0.005; + trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01; + trajopt_composite_profile->collision_cost_config.coeff = 50; + + auto trajopt_solver_profile = std::make_shared(); + trajopt_solver_profile->opt_info.max_iter = 100; + + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + } + + auto post_check_profile = std::make_shared(); profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile); @@ -222,7 +263,8 @@ bool PickAndPlaceExample::run() CONSOLE_BRIDGE_logInform("Pick plan"); // Create task - TaskComposerNode::UPtr pick_task = factory.createTaskComposerNode("TrajOptPipeline"); + const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; + TaskComposerNode::UPtr pick_task = factory.createTaskComposerNode(task_name); const std::string pick_output_key = pick_task->getOutputKeys().front(); // Create Task Composer Problem