From 73ba1b84f931694f1aab49c0c2265c8fd3583f52 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 2 Jan 2025 08:18:54 -0600 Subject: [PATCH] Changes required now that environment return const shared pointers for getKinematicGroup and getJointGroup --- tesseract_examples/src/car_seat_example.cpp | 2 +- .../descartes/impl/descartes_motion_planner.hpp | 2 +- .../ompl/src/ompl_motion_planner.cpp | 2 +- .../ompl/src/profile/ompl_real_vector_plan_profile.cpp | 10 +++++----- .../tesseract_motion_planners/simple/interpolation.h | 4 ++-- .../simple/src/simple_motion_planner.cpp | 4 ++-- .../tesseract_motion_planners/trajopt/trajopt_utils.h | 2 +- .../src/profile/trajopt_default_composite_profile.cpp | 2 +- .../trajopt/src/trajopt_utils.cpp | 4 ++-- .../src/profile/trajopt_ifopt_default_plan_profile.cpp | 4 +--- .../core/src/task_composer_graph.cpp | 1 + .../core/src/task_composer_plugin_factory.cpp | 1 + .../src/nodes/continuous_contact_check_task.cpp | 2 +- .../planning/src/nodes/discrete_contact_check_task.cpp | 2 +- .../planning/src/nodes/fix_state_collision_task.cpp | 2 +- .../tesseract_task_composer_plugin_factories_unit.cpp | 1 + 16 files changed, 23 insertions(+), 22 deletions(-) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index bd66708943..bc9e461979 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -253,7 +253,7 @@ bool CarSeatExample::run() TaskComposerPluginFactory factory(config_path); // Get manipulator - JointGroup::UPtr joint_group = env_->getJointGroup("manipulator"); + JointGroup::ConstPtr joint_group = env_->getJointGroup("manipulator"); // Create seats and add it to the local environment Commands cmds = addSeats(env_->getResourceLocator()); diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index 52b2f57745..682d1c5c05 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -142,7 +142,7 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r response.data = std::move(solver); // Get Manipulator Information - tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(composite_mi.manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = request.env->getJointGroup(composite_mi.manipulator); const std::vector joint_names = manip->getJointNames(); const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits; diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 2d7fab61bc..c31eb93b0c 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -316,7 +316,7 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const // Get end state kinematics data tesseract_common::ManipulatorInfo end_mi = composite_mi.getCombined(end_move_instruction.getManipulatorInfo()); - tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(end_mi.manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = request.env->getJointGroup(end_mi.manipulator); // Create problem data const auto& start_move_instruction = start_instruction.get().as(); diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp index e77b80db7b..ad3cf94d11 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_real_vector_plan_profile.cpp @@ -91,7 +91,7 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in tesseract_common::ManipulatorInfo end_mi = composite_mi.getCombined(end_instruction.getManipulatorInfo()); // Get kinematics - tesseract_kinematics::JointGroup::Ptr manip = env->getJointGroup(end_mi.manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup(end_mi.manipulator); const auto dof = static_cast(manip->numJoints()); const std::vector joint_names = manip->getJointNames(); const Eigen::MatrixX2d limits = manip->getLimits().joint_limits; @@ -150,7 +150,7 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in // Add start states if (start_instruction.getWaypoint().isJointWaypoint() || start_instruction.getWaypoint().isStateWaypoint()) { - tesseract_kinematics::JointGroup::UPtr joint_group = env->getJointGroup(start_mi.manipulator); + tesseract_kinematics::JointGroup::ConstPtr joint_group = env->getJointGroup(start_mi.manipulator); assert(checkJointPositionFormat(joint_group->getJointNames(), start_instruction.getWaypoint())); contact_checker->setActiveCollisionObjects(joint_group->getActiveLinkNames()); const Eigen::VectorXd& cur_position = getJointPosition(start_instruction.getWaypoint()); @@ -161,7 +161,7 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in const auto& cur_wp = start_instruction.getWaypoint().as(); Eigen::Isometry3d tcp_offset = env->findTCPOffset(start_mi); Eigen::Isometry3d tcp_frame_cwp = cur_wp.getTransform() * tcp_offset.inverse(); - tesseract_kinematics::KinematicGroup::UPtr kin_group; + tesseract_kinematics::KinematicGroup::ConstPtr kin_group; if (start_mi.manipulator_ik_solver.empty()) kin_group = env->getKinematicGroup(start_mi.manipulator); else @@ -179,7 +179,7 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in // Add Goal states if (end_instruction.getWaypoint().isJointWaypoint() || end_instruction.getWaypoint().isStateWaypoint()) { - tesseract_kinematics::JointGroup::UPtr joint_group = env->getJointGroup(end_mi.manipulator); + tesseract_kinematics::JointGroup::ConstPtr joint_group = env->getJointGroup(end_mi.manipulator); assert(checkJointPositionFormat(joint_group->getJointNames(), end_instruction.getWaypoint())); contact_checker->setActiveCollisionObjects(joint_group->getActiveLinkNames()); const Eigen::VectorXd& cur_position = getJointPosition(end_instruction.getWaypoint()); @@ -190,7 +190,7 @@ OMPLRealVectorPlanProfile::createSimpleSetup(const MoveInstructionPoly& start_in const auto& cur_wp = end_instruction.getWaypoint().as(); Eigen::Isometry3d tcp_offset = env->findTCPOffset(end_mi); Eigen::Isometry3d tcp_frame_cwp = cur_wp.getTransform() * tcp_offset.inverse(); - tesseract_kinematics::KinematicGroup::UPtr kin_group; + tesseract_kinematics::KinematicGroup::ConstPtr kin_group; if (end_mi.manipulator_ik_solver.empty()) kin_group = env->getKinematicGroup(end_mi.manipulator); else diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h index 205b68c229..106d3ddbdc 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h @@ -57,7 +57,7 @@ struct JointGroupInstructionInfo JointGroupInstructionInfo& operator=(JointGroupInstructionInfo&&) = delete; const MoveInstructionPoly& instruction; - std::unique_ptr manip; + std::shared_ptr manip; std::string working_frame; Eigen::Isometry3d working_frame_transform{ Eigen::Isometry3d::Identity() }; std::string tcp_frame; @@ -108,7 +108,7 @@ struct KinematicGroupInstructionInfo KinematicGroupInstructionInfo& operator=(KinematicGroupInstructionInfo&&) = delete; const MoveInstructionPoly& instruction; - std::unique_ptr manip; + std::shared_ptr manip; std::string working_frame; Eigen::Isometry3d working_frame_transform{ Eigen::Isometry3d::Identity() }; std::string tcp_frame; diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index bdb8356a8e..92b1c28ee3 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -87,7 +87,7 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; // Initialize - tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = request.env->getJointGroup(manipulator); // Start State tesseract_scene_graph::SceneState start_state = request.env->getState(); @@ -175,7 +175,7 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr if (prev_instruction.isNull()) { const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = request.env->getJointGroup(manipulator); prev_instruction = base_instruction; auto& start_waypoint = prev_instruction.getWaypoint(); diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h index c191df27f6..dd74adaada 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_utils.h @@ -38,7 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -std::unique_ptr +std::shared_ptr createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, const tesseract_environment::Environment& env); diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp index 8ac38bd703..6a257e3d26 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp @@ -61,7 +61,7 @@ TrajOptDefaultCompositeProfile::create(const tesseract_common::ManipulatorInfo& // -------- Construct the problem ------------ // ------------------------------------------- TrajOptTermInfos term_infos; - tesseract_kinematics::JointGroup::UPtr joint_group = env->getJointGroup(composite_manip_info.manipulator); + tesseract_kinematics::JointGroup::ConstPtr joint_group = env->getJointGroup(composite_manip_info.manipulator); const Eigen::Index dof = joint_group->numJoints(); const Eigen::MatrixX2d joint_limits = joint_group->getLimits().joint_limits; const double lvs_length = computeLongestValidSegmentLength(joint_limits); diff --git a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp index ec54299d76..a1fca6730f 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_utils.cpp @@ -32,13 +32,13 @@ namespace tesseract_planning { -std::unique_ptr +std::shared_ptr createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, const tesseract_environment::Environment& env) { // Assign Kinematics object try { - tesseract_kinematics::KinematicGroup::UPtr kin_group; + tesseract_kinematics::KinematicGroup::ConstPtr kin_group; std::string error_msg; if (manip_info.manipulator_ik_solver.empty()) { diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index e9567d4c51..20b4c603fe 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -65,9 +65,7 @@ TrajOptIfoptDefaultPlanProfile::create(const MoveInstructionPoly& move_instructi std::vector joint_names = env->getGroupJointNames(mi.manipulator); assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); - /** @todo Levi, is this expensive? I think we should update environment to return const shared pointer because it - * maintains an internal cache for you */ - std::shared_ptr manip = env->getJointGroup(mi.manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup(mi.manipulator); Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits; TrajOptIfoptWaypointInfo info; diff --git a/tesseract_task_composer/core/src/task_composer_graph.cpp b/tesseract_task_composer/core/src/task_composer_graph.cpp index 945fc32804..f0b960331e 100644 --- a/tesseract_task_composer/core/src/task_composer_graph.cpp +++ b/tesseract_task_composer/core/src/task_composer_graph.cpp @@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP diff --git a/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp b/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp index 1e261ce958..27773ad2a6 100644 --- a/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp +++ b/tesseract_task_composer/core/src/task_composer_plugin_factory.cpp @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index af895391bb..f19fa2e056 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -149,7 +149,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(input_manip_info); - tesseract_kinematics::JointGroup::UPtr manip = env->getJointGroup(manip_info.manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup(manip_info.manipulator); tesseract_scene_graph::StateSolver::UPtr state_solver = env->getStateSolver(); tesseract_collision::ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index eb259369f8..246c028553 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -146,7 +146,7 @@ std::unique_ptr DiscreteContactCheckTask::runImpl(TaskComp // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(input_manip_info); - tesseract_kinematics::JointGroup::UPtr manip = env->getJointGroup(manip_info.manipulator); + tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup(manip_info.manipulator); tesseract_scene_graph::StateSolver::UPtr state_solver = env->getStateSolver(); tesseract_collision::DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp index 6f4c172dbd..dcd4727e29 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp @@ -295,7 +295,7 @@ bool moveWaypointFromCollisionRandomSampler(WaypointPoly& waypoint, return false; } - tesseract_kinematics::JointGroup::UPtr kin = env.getJointGroup(manip_info.manipulator); + tesseract_kinematics::JointGroup::ConstPtr kin = env.getJointGroup(manip_info.manipulator); Eigen::MatrixXd limits = kin->getLimits().joint_limits; Eigen::VectorXd range = limits.col(1).array() - limits.col(0).array(); diff --git a/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp index 78bde3db9c..43a9846cb1 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include using namespace tesseract_planning;