Update ChangeJointLimits is only for joint limits

Author: Levi-Armstrong

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

@@ -55,7 +55,9 @@ enum class CommandType
   REMOVE_ALLOWED_COLLISION = 10,
   REMOVE_ALLOWED_COLLISION_LINK = 11,
   ADD_SCENE_GRAPH = 12,
-  CHANGE_JOINT_LIMITS = 13
+  CHANGE_JOINT_POSITION_LIMITS = 13,
+  CHANGE_JOINT_VELOCITY_LIMITS = 14,
+  CHANGE_JOINT_ACCELERATION_LIMITS = 15
 };
 
 class Command
@@ -288,19 +290,68 @@ class AddSceneGraphCommand : public Command
   std::string prefix_;
 };
 
-class ChangeJointLimitsCommand : public Command
+class ChangeJointPositionLimitsCommand : 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_(joint_limits)
+  ChangeJointPositionLimitsCommand(std::string joint_name, double lower, double upper)
+    : Command(CommandType::CHANGE_JOINT_POSITION_LIMITS)
+    , limits_({ std::make_pair(joint_name, std::make_pair(lower, upper)) })
   {
+    assert(upper > lower);
+  }
+
+  ChangeJointPositionLimitsCommand(std::unordered_map<std::string, std::pair<double, double>> limits)
+    : Command(CommandType::CHANGE_JOINT_POSITION_LIMITS), limits_(std::move(limits))
+  {
+    assert(std::all_of(limits_.begin(), limits_.end(), [](const auto& p) { return p.second.second > p.second.first; }));
   }
-  const std::string& getJointName() const { return joint_name_; }
-  const tesseract_scene_graph::JointLimits& getLimits() const { return joint_limits_; }
+
+  const std::unordered_map<std::string, std::pair<double, double>>& getLimits() const { return limits_; }
 
 private:
-  std::string joint_name_;
-  tesseract_scene_graph::JointLimits joint_limits_;
+  std::unordered_map<std::string, std::pair<double, double>> limits_;
+};
+
+class ChangeJointVelocityLimitsCommand : public Command
+{
+public:
+  ChangeJointVelocityLimitsCommand(std::string joint_name, double limit)
+    : Command(CommandType::CHANGE_JOINT_VELOCITY_LIMITS), limits_({ std::make_pair(joint_name, limit) })
+  {
+    assert(limit > 0);
+  }
+
+  ChangeJointVelocityLimitsCommand(std::unordered_map<std::string, double> limits)
+    : Command(CommandType::CHANGE_JOINT_VELOCITY_LIMITS), limits_(std::move(limits))
+  {
+    assert(std::all_of(limits_.begin(), limits_.end(), [](const auto& p) { return p.second > 0; }));
+  }
+
+  const std::unordered_map<std::string, double>& getLimits() const { return limits_; }
+
+private:
+  std::unordered_map<std::string, double> limits_;
+};
+
+class ChangeJointAccelerationLimitsCommand : public Command
+{
+public:
+  ChangeJointAccelerationLimitsCommand(std::string joint_name, double limit)
+    : Command(CommandType::CHANGE_JOINT_ACCELERATION_LIMITS), limits_({ std::make_pair(joint_name, limit) })
+  {
+    assert(limit > 0);
+  }
+
+  ChangeJointAccelerationLimitsCommand(std::unordered_map<std::string, double> limits)
+    : Command(CommandType::CHANGE_JOINT_ACCELERATION_LIMITS), limits_(std::move(limits))
+  {
+    assert(std::all_of(limits_.begin(), limits_.end(), [](const auto& p) { return p.second > 0; }));
+  }
+
+  const std::unordered_map<std::string, double>& getLimits() const { return limits_; }
+
+private:
+  std::unordered_map<std::string, double> limits_;
 };
 
 }  // namespace tesseract_environment

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

@@ -345,12 +345,49 @@ class Environment
   virtual bool changeJointOrigin(const std::string& joint_name, const Eigen::Isometry3d& new_origin);
 
   /**
-   * @brief Changes the limits associated with a joint
+   * @brief Changes the position limits associated with a joint
    * @param joint_name Name of the joint to be updated
-   * @param limits New limits to be set as the joint limits
+   * @param limits New position limits to be set as the joint limits
    * @return
    */
-  virtual bool changeJointLimits(const std::string& joint_name, const tesseract_scene_graph::JointLimits& limits);
+  virtual bool changeJointPositionLimits(const std::string& joint_name, double lower, double upper);
+
+  /**
+   * @brief Changes the position limits associated with a joint
+   * @param limits A map of joint names to new position limits
+   * @return
+   */
+  virtual bool changeJointPositionLimits(const std::unordered_map<std::string, std::pair<double, double>>& limits);
+
+  /**
+   * @brief Changes the velocity limits associated with a joint
+   * @param joint_name Name of the joint to be updated
+   * @param limits New velocity limits to be set as the joint limits
+   * @return
+   */
+  virtual bool changeJointVelocityLimits(const std::string& joint_name, double limit);
+
+  /**
+   * @brief Changes the velocity limits associated with a joint
+   * @param limits A map of joint names to new velocity limits
+   * @return
+   */
+  virtual bool changeJointVelocityLimits(const std::unordered_map<std::string, double>& limits);
+
+  /**
+   * @brief Changes the acceleration limits associated with a joint
+   * @param joint_name Name of the joint to be updated
+   * @param limits New acceleration limits to be set as the joint limits
+   * @return
+   */
+  virtual bool changeJointAccelerationLimits(const std::string& joint_name, double limit);
+
+  /**
+   * @brief Changes the acceleration limits associated with a joint
+   * @param limits A map of joint names to new acceleration limits
+   * @return
+   */
+  virtual bool changeJointAccelerationLimits(const std::unordered_map<std::string, double>& limits);
 
   /**
    * @brief Gets the limits associated with a joint

tesseract/tesseract_environment/src/core/environment.cpp

@@ -172,11 +172,24 @@ bool Environment::applyCommand(const Command& command)
       }
       break;
     }
-
-    case tesseract_environment::CommandType::CHANGE_JOINT_LIMITS:
+    case tesseract_environment::CommandType::CHANGE_JOINT_POSITION_LIMITS:
+    {
+      const auto& cmd = static_cast<const tesseract_environment::ChangeJointPositionLimitsCommand&>(command);
+      if (!changeJointPositionLimits(cmd.getLimits()))
+        return false;
+      break;
+    }
+    case tesseract_environment::CommandType::CHANGE_JOINT_VELOCITY_LIMITS:
+    {
+      const auto& cmd = static_cast<const tesseract_environment::ChangeJointVelocityLimitsCommand&>(command);
+      if (!changeJointVelocityLimits(cmd.getLimits()))
+        return false;
+      break;
+    }
+    case tesseract_environment::CommandType::CHANGE_JOINT_ACCELERATION_LIMITS:
     {
-      const auto& cmd = static_cast<const tesseract_environment::ChangeJointLimitsCommand&>(command);
-      if (!changeJointLimits(cmd.getJointName(), cmd.getLimits()))
+      const auto& cmd = static_cast<const tesseract_environment::ChangeJointAccelerationLimitsCommand&>(command);
+      if (!changeJointAccelerationLimits(cmd.getLimits()))
         return false;
       break;
     }
@@ -370,18 +383,161 @@ 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)
+bool Environment::changeJointPositionLimits(const std::string& joint_name, double lower, double upper)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(joint_name);
+  if (jl == nullptr)
+    return false;
+
+  tesseract_scene_graph::JointLimits jl_copy = *jl;
+  jl_copy.lower = lower;
+  jl_copy.upper = upper;
+
+  if (!scene_graph_->changeJointLimits(joint_name, jl_copy))
+    return false;
+
+  // TODO: Only change joint limits rather than completely rebuilding kinematics
+  if (!manipulator_manager_->update())
+    return false;
+
+  ++revision_;
+  commands_.push_back(std::make_shared<ChangeJointPositionLimitsCommand>(joint_name, lower, upper));
+
+  environmentChanged();
+
+  return true;
+}
+
+bool Environment::changeJointPositionLimits(const std::unordered_map<std::string, std::pair<double, double> >& limits)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (const auto& l : limits)
+  {
+    tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(l.first);
+    if (jl == nullptr)
+      return false;
+
+    tesseract_scene_graph::JointLimits jl_copy = *jl;
+    jl_copy.lower = l.second.first;
+    jl_copy.upper = l.second.second;
+
+    if (!scene_graph_->changeJointLimits(l.first, jl_copy))
+      return false;
+  }
+
+  // TODO: Only change joint limits rather than completely rebuilding kinematics
+  if (!manipulator_manager_->update())
+    return false;
+
+  ++revision_;
+  commands_.push_back(std::make_shared<ChangeJointPositionLimitsCommand>(limits));
+
+  environmentChanged();
+
+  return true;
+}
+
+bool Environment::changeJointVelocityLimits(const std::string& joint_name, double limit)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(joint_name);
+  if (jl == nullptr)
+    return false;
+
+  tesseract_scene_graph::JointLimits jl_copy = *jl;
+  jl_copy.velocity = limit;
+
+  if (!scene_graph_->changeJointLimits(joint_name, jl_copy))
+    return false;
+
+  // TODO: Only change joint limits rather than completely rebuilding kinematics
+  if (!manipulator_manager_->update())
+    return false;
+
+  ++revision_;
+  commands_.push_back(std::make_shared<ChangeJointVelocityLimitsCommand>(joint_name, limit));
+
+  environmentChanged();
+
+  return true;
+}
+
+bool Environment::changeJointVelocityLimits(const std::unordered_map<std::string, double>& limits)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (const auto& l : limits)
+  {
+    tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(l.first);
+    if (jl == nullptr)
+      return false;
+
+    tesseract_scene_graph::JointLimits jl_copy = *jl;
+    jl_copy.velocity = l.second;
+
+    if (!scene_graph_->changeJointLimits(l.first, jl_copy))
+      return false;
+  }
+
+  // TODO: Only change joint limits rather than completely rebuilding kinematics
+  if (!manipulator_manager_->update())
+    return false;
+
+  ++revision_;
+  commands_.push_back(std::make_shared<ChangeJointVelocityLimitsCommand>(limits));
+
+  environmentChanged();
+
+  return true;
+}
+
+bool Environment::changeJointAccelerationLimits(const std::string& joint_name, double limit)
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  if (!scene_graph_->changeJointLimits(joint_name, limits))
+  tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(joint_name);
+  if (jl == nullptr)
+    return false;
+
+  tesseract_scene_graph::JointLimits jl_copy = *jl;
+  jl_copy.acceleration = limit;
+
+  if (!scene_graph_->changeJointLimits(joint_name, jl_copy))
     return false;
 
   // TODO: Only change joint limits rather than completely rebuilding kinematics
   if (!manipulator_manager_->update())
     return false;
 
   ++revision_;
-  commands_.push_back(std::make_shared<ChangeJointLimitsCommand>(joint_name, limits));
+  commands_.push_back(std::make_shared<ChangeJointAccelerationLimitsCommand>(joint_name, limit));
+
+  environmentChanged();
+
+  return true;
+}
+
+bool Environment::changeJointAccelerationLimits(const std::unordered_map<std::string, double>& limits)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  for (const auto& l : limits)
+  {
+    tesseract_scene_graph::JointLimits::ConstPtr jl = scene_graph_->getJointLimits(l.first);
+    if (jl == nullptr)
+      return false;
+
+    tesseract_scene_graph::JointLimits jl_copy = *jl;
+    jl_copy.acceleration = l.second;
+
+    if (!scene_graph_->changeJointLimits(l.first, jl_copy))
+      return false;
+  }
+
+  // TODO: Only change joint limits rather than completely rebuilding kinematics
+  if (!manipulator_manager_->update())
+    return false;
+
+  ++revision_;
+  commands_.push_back(std::make_shared<ChangeJointAccelerationLimitsCommand>(limits));
 
   environmentChanged();
 

tesseract/tesseract_environment/src/ofkt/ofkt_state_solver.cpp

@@ -392,18 +392,19 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
 
         break;
       }
-      case tesseract_environment::CommandType::CHANGE_JOINT_LIMITS:
+      case tesseract_environment::CommandType::CHANGE_JOINT_POSITION_LIMITS:
       {
-        const auto& cmd = static_cast<const tesseract_environment::ChangeJointLimitsCommand&>(*command);
+        const auto& cmd = static_cast<const tesseract_environment::ChangeJointPositionLimitsCommand&>(*command);
+        const std::unordered_map<std::string, std::pair<double, double>>& limits = cmd.getLimits();
         // Loop through all names until we find the one we need
         for (std::size_t i = 0; i < joint_names_.size(); i++)
         {
+          auto it = limits.find(joint_names_[i]);
           // Assign the lower/upper. Velocity, acceleration, and effort are ignored
-          if (joint_names_[i] == cmd.getJointName())
+          if (it != limits.end())
           {
-            limits_(static_cast<Eigen::Index>(i), 0) = cmd.getLimits().lower;
-            limits_(static_cast<Eigen::Index>(i), 1) = cmd.getLimits().upper;
-            break;
+            limits_(static_cast<Eigen::Index>(i), 0) = it->second.first;
+            limits_(static_cast<Eigen::Index>(i), 1) = it->second.second;
           }
         }