Add changeJointLimits Command

Author: mpowelson

tesseract/tesseract_environment/include/tesseract_environment/core/commands.h

@@ -54,7 +54,8 @@ enum class CommandType
   ADD_ALLOWED_COLLISION = 9,
   REMOVE_ALLOWED_COLLISION = 10,
   REMOVE_ALLOWED_COLLISION_LINK = 11,
-  ADD_SCENE_GRAPH = 12
+  ADD_SCENE_GRAPH = 12,
+  CHANGE_JOINT_LIMITS = 13
 };
 
 class Command
@@ -287,6 +288,23 @@ class AddSceneGraphCommand : public Command
   std::string prefix_;
 };
 
+class TESSERACT_ENVIRONMENT_CORE_PUBLIC ChangeJointLimitsCommand : public Command
+{
+public:
+  ChangeJointLimitsCommand(std::string joint_name, tesseract_scene_graph::JointLimits joint_limits)
+    : Command(CommandType::CHANGE_JOINT_LIMITS)
+    , joint_name_(std::move(joint_name))
+    , joint_limits_(std::move(joint_limits))
+  {
+  }
+  const std::string& getJointName() const { return joint_name_; }
+  const tesseract_scene_graph::JointLimits& getLimits() const { return joint_limits_; }
+
+private:
+  std::string joint_name_;
+  tesseract_scene_graph::JointLimits joint_limits_;
+};
+
 }  // namespace tesseract_environment
 
 #endif  // TESSERACT_ENVIRONMENT_COMMANDS_H

tesseract/tesseract_environment/include/tesseract_environment/core/environment.h

@@ -329,6 +329,8 @@ class Environment
    */
   virtual bool changeJointOrigin(const std::string& joint_name, const Eigen::Isometry3d& new_origin);
 
+  virtual bool changeJointLimits(const std::string& joint_name, const tesseract_scene_graph::JointLimits limits);
+
   /**
    * @brief Set whether a link should be considered during collision checking
    * @param enabled True if should be condisdered during collision checking, otherwise false

tesseract/tesseract_environment/src/core/environment.cpp

@@ -172,6 +172,14 @@ bool Environment::applyCommand(const Command& command)
       }
       break;
     }
+
+    case tesseract_environment::CommandType::CHANGE_JOINT_LIMITS:
+    {
+      const auto& cmd = static_cast<const tesseract_environment::ChangeJointLimitsCommand&>(command);
+      if (!changeJointLimits(cmd.getJointName(), cmd.getLimits()))
+        return false;
+      break;
+    }
     default:
     {
       CONSOLE_BRIDGE_logError("Unhandled environment command");
@@ -362,6 +370,20 @@ bool Environment::changeJointOrigin(const std::string& joint_name, const Eigen::
   return true;
 }
 
+bool Environment::changeJointLimits(const std::string& joint_name, const tesseract_scene_graph::JointLimits limits)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (!scene_graph_->changeJointLimits(joint_name, limits))
+    return false;
+
+  ++revision_;
+  commands_.push_back(std::make_shared<ChangeJointLimitsCommand>(joint_name, limits));
+
+  environmentChanged();
+
+  return true;
+}
+
 void Environment::setLinkCollisionEnabled(const std::string& name, bool enabled)
 {
   std::lock_guard<std::mutex> lock(mutex_);

tesseract/tesseract_scene_graph/include/tesseract_scene_graph/graph.h

@@ -261,10 +261,25 @@ class SceneGraph : public Graph
   /** @brief Changes the "origin" transform of the joint and recomputes the associated edge
    * @param name Name of the joint to be changed
    * @param new_origin The new transform associated with the joint
-   * @return
+   * @return True if successful.
    */
   bool changeJointOrigin(const std::string& name, const Eigen::Isometry3d& new_origin);
 
+  /**
+   * @brief Changes the limits of a joint. The JointLimits::Ptr remains the same, but the values passed in are assigned
+   * @param name Name of the joint to be changed
+   * @param limits The new limits associated with the joint
+   * @return True if successful.
+   */
+  bool changeJointLimits(const std::string& name, const JointLimits& limits);
+
+  /**
+   * @brief Gets the limits of the joint specified by name
+   * @param name Name of the joint which limits will be retrieved
+   * @return Limits of the joint. Returns nullptr is joint is not found.
+   */
+  JointLimits::ConstPtr getJointLimits(const std::string& name);
+
   /**
    * @brief Disable collision between two collision objects
    * @param link_name1 Collision object name

tesseract/tesseract_scene_graph/src/graph.cpp

@@ -340,6 +340,39 @@ bool SceneGraph::changeJointOrigin(const std::string& name, const Eigen::Isometr
   return true;
 }
 
+bool SceneGraph::changeJointLimits(const std::string& name, const JointLimits& limits)
+{
+  auto found = joint_map_.find(name);
+
+  if (found == joint_map_.end())
+  {
+    CONSOLE_BRIDGE_logWarn("Tried to change Joint limit with name (%s) which does not exist in scene graph.",
+                           name.c_str());
+    return false;
+  }
+
+  found->second.first->limits->lower = limits.lower;
+  found->second.first->limits->upper = limits.upper;
+  found->second.first->limits->effort = limits.effort;
+  found->second.first->limits->velocity = limits.velocity;
+  found->second.first->limits->acceleration = limits.acceleration;
+
+  return true;
+}
+
+JointLimits::ConstPtr SceneGraph::getJointLimits(const std::string& name)
+{
+  auto found = joint_map_.find(name);
+
+  if (found == joint_map_.end())
+  {
+    CONSOLE_BRIDGE_logWarn("SceneGraph::getJointLimits tried to find Joint with name (%s) which does not exist in scene graph.",
+                           name.c_str());
+    return nullptr;
+  }
+  return found->second.first->limits;
+}
+
 void SceneGraph::addAllowedCollision(const std::string& link_name1,
                                      const std::string& link_name2,
                                      const std::string& reason)