Changes required now that environment return const shared pointers fo…

…r getKinematicGroup and getJointGroup
tesseract-robotics · Jan 2, 2025 · 73ba1b8 · 73ba1b8
1 parent 99f8b97
commit 73ba1b8
Show file tree

Hide file tree

Showing 16 changed files with 23 additions and 22 deletions.
diff --git 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/...s/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/...s/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
@@ -142,7 +142,7 @@ PlannerResponse DescartesMotionPlanner<FloatType>::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<std::string> 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
@@ -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<MoveInstructionPoly>();

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
@@ -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<unsigned>(manip->numJoints());
   const std::vector<std::string> 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<CartesianWaypointPoly>();
     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<CartesianWaypointPoly>();
     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
@@ -57,7 +57,7 @@ struct JointGroupInstructionInfo
   JointGroupInstructionInfo& operator=(JointGroupInstructionInfo&&) = delete;
 
   const MoveInstructionPoly& instruction;
-  std::unique_ptr<tesseract_kinematics::JointGroup> manip;
+  std::shared_ptr<const tesseract_kinematics::JointGroup> 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<tesseract_kinematics::KinematicGroup> manip;
+  std::shared_ptr<const tesseract_kinematics::KinematicGroup> 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
@@ -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
@@ -38,7 +38,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-std::unique_ptr<const tesseract_kinematics::JointGroup>
+std::shared_ptr<const tesseract_kinematics::JointGroup>
 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
@@ -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
@@ -32,13 +32,13 @@
 
 namespace tesseract_planning
 {
-std::unique_ptr<const tesseract_kinematics::JointGroup>
+std::shared_ptr<const tesseract_kinematics::JointGroup>
 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
@@ -65,9 +65,7 @@ TrajOptIfoptDefaultPlanProfile::create(const MoveInstructionPoly& move_instructi
   std::vector<std::string> 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<const tesseract_kinematics::JointGroup> 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
@@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tesseract_common/serialization.h>
 #include <tesseract_common/plugin_info.h>
 #include <tesseract_common/yaml_utils.h>
+#include <tesseract_common/yaml_extenstions.h>
 #include <tesseract_common/stopwatch.h>
 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
@@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/plugin_loader.hpp>
 #include <tesseract_common/yaml_utils.h>
+#include <tesseract_common/yaml_extenstions.h>
 #include <tesseract_task_composer/core/task_composer_plugin_factory.h>
 #include <tesseract_task_composer/core/task_composer_node.h>
 #include <tesseract_task_composer/core/task_composer_executor.h>

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
@@ -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
@@ -146,7 +146,7 @@ std::unique_ptr<TaskComposerNodeInfo> 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
@@ -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
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_common/utils.h>
 #include <tesseract_common/types.h>
 #include <tesseract_common/yaml_utils.h>
+#include <tesseract_common/yaml_extenstions.h>
 
 using namespace tesseract_planning;