From 89888fb718e45d723883317c60cd4d75dc48aa3a Mon Sep 17 00:00:00 2001 From: mripperger Date: Tue, 1 Oct 2019 14:05:38 -0500 Subject: [PATCH] Updated TrajOpt planner unit test --- .../test/trajopt_planner_tests.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/test/trajopt_planner_tests.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/test/trajopt_planner_tests.cpp index a887d9867bd..d05afea451a 100644 --- a/tesseract/tesseract_planning/tesseract_motion_planners/test/trajopt_planner_tests.cpp +++ b/tesseract/tesseract_planning/tesseract_motion_planners/test/trajopt_planner_tests.cpp @@ -239,8 +239,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespacePlanner2) std::make_shared(std::vector({ 0, 0, 0, -1.57, 0, 0, 0 }), joint_names); // Specify a Cartesian Waypoint as the finish - config.end_waypoint_ = - std::make_shared(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0)); + config.end_waypoint_ = std::make_shared( + Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link"); // Create test planner used for testing problem creation tesseract_tests::TrajOptFreespacePlannerTest test_planner; @@ -284,8 +284,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespacePlanner3) tesseract_ptr_->getFwdKinematicsManagerConst()->getFwdKinematicSolver("manipulator")->getJointNames(); // Specify a Cartesian Waypoint as the start - config.start_waypoint_ = - std::make_shared(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0)); + config.start_waypoint_ = std::make_shared( + Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link"); // Specify a Joint Waypoint as the finish config.end_waypoint_ = std::make_shared(std::vector({ 0, 0, 0, -1.57, 0, 0, 0 }), joint_names); @@ -329,12 +329,12 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespacePlanner4) config.num_steps_ = NUM_STEPS; // Specify a Cartesian Waypoint as the start - config.start_waypoint_ = - std::make_shared(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0)); + config.start_waypoint_ = std::make_shared( + Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link"); // Specify a Cartesian Waypoint as the finish - config.end_waypoint_ = - std::make_shared(Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0)); + config.end_waypoint_ = std::make_shared( + Eigen::Vector3d(-.20, .4, 0.2), Eigen::Quaterniond(0, 0, 1.0, 0), "base_link"); // Create test planner used for testing problem creation tesseract_tests::TrajOptFreespacePlannerTest test_planner; @@ -377,8 +377,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayPlanner0) // These specify the series of points to be optimized for (int ind = 0; ind < NUM_STEPS; ind++) { - CartesianWaypoint::Ptr waypoint = std::make_shared(Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8), - Eigen::Quaterniond(0, 1.0, 0, 0)); + CartesianWaypoint::Ptr waypoint = std::make_shared( + Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8), Eigen::Quaterniond(0, 1.0, 0, 0), "base_link"); waypoint->setIsCritical(true); config.target_waypoints_.push_back(waypoint); } @@ -428,8 +428,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayPlanner1) // These specify the series of points to be optimized for (int ind = 0; ind < NUM_STEPS; ind++) { - CartesianWaypoint::Ptr waypoint = std::make_shared(Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8), - Eigen::Quaterniond(0, 1.0, 0, 0)); + CartesianWaypoint::Ptr waypoint = std::make_shared( + Eigen::Vector3d(-0.2 + ind * .02, 0.4, 0.8), Eigen::Quaterniond(0, 1.0, 0, 0), "base_link"); waypoint->setIsCritical(false); config.target_waypoints_.push_back(waypoint); }