Skip to content

Commit

Permalink
Added ifopt option to pick_and_place_example
Browse files Browse the repository at this point in the history
  • Loading branch information
@rjoomen
rjoomen committed Jan 8, 2024
1 parent 2cd7f3c commit 6c512f9
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 30 deletions.
2 changes: 2 additions & 0 deletions tesseract_examples/include/tesseract_examples/pick_and_place_example.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, 2> box_position = { 0.15, 0.4 });
~PickAndPlaceExample() override = default;
Expand All @@ -56,6 +57,7 @@ class PickAndPlaceExample : public Example
bool run() override final;

private:
bool ifopt_;
double box_size_;
std::array<double, 2> box_position_;
static tesseract_environment::Command::Ptr addBox(double box_x, double box_y, double box_side);
Expand Down
12 changes: 6 additions & 6 deletions tesseract_examples/src/car_seat_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<TrajOptIfoptDefaultPlanProfile>();
plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);
auto trajopt_ifopt_plan_profile = std::make_shared<TrajOptIfoptDefaultPlanProfile>();
trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8);

profiles->addProfile<TrajOptIfoptCompositeProfile>(
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile);
profiles->addProfile<TrajOptIfoptPlanProfile>(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", plan_profile);
profiles->addProfile<TrajOptIfoptPlanProfile>(
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile);
}
else
{
Expand Down
90 changes: 66 additions & 24 deletions tesseract_examples/src/pick_and_place_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@
* limitations under the License.
*/
#include <tesseract_common/macros.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h>
#include <tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <json/json.h>
#include <console_bridge/console.h>
Expand Down Expand Up @@ -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<double, 2> 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)
{
}

Expand Down Expand Up @@ -191,38 +195,76 @@ bool PickAndPlaceExample::run()
// Create Executor
auto executor = factory.createTaskComposerExecutor("TaskflowExecutor");

// Create TrajOpt Profile
auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);

auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
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<TrajOptDefaultSolverProfile>();
trajopt_solver_profile->opt_info.max_iter = 100;

auto post_check_profile = std::make_shared<ContactCheckProfile>();

// Create profile dictionary
auto profiles = std::make_shared<ProfileDictionary>();
profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile);
if (ifopt_)
{
// Create TrajOpt_Ifopt Profile
auto trajopt_ifopt_composite_profile = std::make_shared<TrajOptIfoptDefaultCompositeProfile>();
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<TrajOptIfoptDefaultPlanProfile>();
trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6);
trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(7);

profiles->addProfile<TrajOptIfoptCompositeProfile>(
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile);
profiles->addProfile<TrajOptIfoptPlanProfile>(
TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile);
}
else
{
// Create TrajOpt Profile
auto trajopt_plan_profile = std::make_shared<TrajOptDefaultPlanProfile>();
trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10);

auto trajopt_composite_profile = std::make_shared<TrajOptDefaultCompositeProfile>();
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<TrajOptDefaultSolverProfile>();
trajopt_solver_profile->opt_info.max_iter = 100;

profiles->addProfile<TrajOptPlanProfile>(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile);
profiles->addProfile<TrajOptCompositeProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile);
profiles->addProfile<TrajOptSolverProfile>(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile);
}

auto post_check_profile = std::make_shared<ContactCheckProfile>();

profiles->addProfile<ContactCheckProfile>(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile);
profiles->addProfile<ContactCheckProfile>(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile);

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
Expand Down

0 comments on commit 6c512f9

Please sign in to comment.