diff --git a/tesseract_environment/include/tesseract_environment/environment.h b/tesseract_environment/include/tesseract_environment/environment.h index 67cf1062417..26ad0c9c593 100644 --- a/tesseract_environment/include/tesseract_environment/environment.h +++ b/tesseract_environment/include/tesseract_environment/environment.h @@ -204,7 +204,8 @@ class Environment * @return A kinematics group */ std::unique_ptr getKinematicGroup(const std::string& group_name, - const std::string& ik_solver_name = "") const; + const std::string& ik_solver_name = "", + bool check_kinematics = true) const; /** * @brief Find tool center point provided in the manipulator info diff --git a/tesseract_environment/src/environment.cpp b/tesseract_environment/src/environment.cpp index 0222f220fbb..9f802400d05 100644 --- a/tesseract_environment/src/environment.cpp +++ b/tesseract_environment/src/environment.cpp @@ -301,7 +301,8 @@ struct Environment::Implementation const std::vector& joint_names) const; std::unique_ptr getKinematicGroup(const std::string& group_name, - std::string ik_solver_name) const; + std::string ik_solver_name, + bool check_kinematics) const; Eigen::Isometry3d findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const; @@ -806,7 +807,7 @@ Environment::Implementation::getJointGroup(const std::string& name, const std::v } std::unique_ptr -Environment::Implementation::getKinematicGroup(const std::string& group_name, std::string ik_solver_name) const +Environment::Implementation::getKinematicGroup(const std::string& group_name, std::string ik_solver_name, bool check_kinematics) const { std::unique_lock cache_lock(kinematic_group_cache_mutex); std::pair key = std::make_pair(group_name, ik_solver_name); @@ -839,7 +840,7 @@ Environment::Implementation::getKinematicGroup(const std::string& group_name, st kinematic_group_cache[key] = std::make_unique(*kg); #ifndef NDEBUG - if (!tesseract_kinematics::checkKinematics(*kg)) + if (check_kinematics && !tesseract_kinematics::checkKinematics(*kg)) { CONSOLE_BRIDGE_logError("Check Kinematics failed. This means that inverse kinematics solution for a pose do not " "match forward kinematics solution. Did you change the URDF recently?"); @@ -2324,10 +2325,10 @@ Environment::getJointGroup(const std::string& name, const std::vector -Environment::getKinematicGroup(const std::string& group_name, const std::string& ik_solver_name) const +Environment::getKinematicGroup(const std::string& group_name, const std::string& ik_solver_name, bool check_kinematics) const { std::shared_lock lock(mutex_); - return std::as_const(*impl_).getKinematicGroup(group_name, ik_solver_name); + return std::as_const(*impl_).getKinematicGroup(group_name, ik_solver_name, check_kinematics); } // NOLINTNEXTLINE