Update StateSolver to leverage KinematicLimits (#494)

Author: Levi-Armstrong

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

@@ -368,7 +368,7 @@ class ChangeJointPositionLimitsCommand : public Command
 public:
   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)) })
+    , limits_({ std::make_pair(std::move(joint_name), std::make_pair(lower, upper)) })
   {
     assert(upper > lower);
   }
@@ -389,7 +389,7 @@ 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) })
+    : Command(CommandType::CHANGE_JOINT_VELOCITY_LIMITS), limits_({ std::make_pair(std::move(joint_name), limit) })
   {
     assert(limit > 0);
   }
@@ -410,7 +410,7 @@ 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) })
+    : Command(CommandType::CHANGE_JOINT_ACCELERATION_LIMITS), limits_({ std::make_pair(std::move(joint_name), limit) })
   {
     assert(limit > 0);
   }

tesseract/tesseract_environment/include/tesseract_environment/core/state_solver.h

@@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_scene_graph/graph.h>
 #include <tesseract_environment/core/types.h>
 #include <tesseract_environment/core/commands.h>
+#include <tesseract_common/types.h>
 
 #ifdef SWIG
 %shared_ptr(tesseract_environment::StateSolver)
@@ -100,10 +101,10 @@ class StateSolver
   virtual EnvState::Ptr getRandomState() const = 0;
 
   /**
-   * @brief Getter for joint_limits_
-   * @return Matrix of joint limits
+   * @brief Getter for kinematic limits
+   * @return The kinematic limits
    */
-  virtual const Eigen::MatrixX2d& getLimits() const = 0;
+  virtual const tesseract_common::KinematicLimits& getLimits() const = 0;
 
   /**
    * @brief This should clone the object so it may be used in a multi threaded application where each thread would

tesseract/tesseract_environment/include/tesseract_environment/kdl/kdl_state_solver.h

@@ -58,15 +58,15 @@ class KDLStateSolver : public StateSolver
 
   EnvState::Ptr getRandomState() const override;
 
-  const Eigen::MatrixX2d& getLimits() const override;
+  const tesseract_common::KinematicLimits& getLimits() const override;
 
 private:
   tesseract_scene_graph::SceneGraph::ConstPtr scene_graph_;    /**< Tesseract Scene Graph */
   EnvState::Ptr current_state_;                                /**< Current state of the environment */
   KDL::Tree kdl_tree_;                                         /**< KDL tree object */
   std::unordered_map<std::string, unsigned int> joint_to_qnr_; /**< Map between joint name and kdl q index */
   KDL::JntArray kdl_jnt_array_;                                /**< The kdl joint array */
-  Eigen::MatrixX2d joint_limits_;                              /**< The joint limits */
+  tesseract_common::KinematicLimits limits_;                   /**< The kinematic limits */
   std::vector<std::string> joint_names_;                       /**< The active joint names */
 
   /**

tesseract/tesseract_environment/include/tesseract_environment/ofkt/ofkt_state_solver.h

@@ -99,14 +99,14 @@ class OFKTStateSolver : public StateSolver
 
   EnvState::Ptr getRandomState() const override;
 
-  const Eigen::MatrixX2d& getLimits() const override;
+  const tesseract_common::KinematicLimits& getLimits() const override;
 
 private:
   EnvState::Ptr current_state_{ std::make_shared<EnvState>() }; /**< Current state of the environment */
   std::vector<std::string> joint_names_;                        /**< The active joint names */
   std::unordered_map<std::string, OFKTNode::UPtr> nodes_;       /**< The joint name map to node */
   std::unordered_map<std::string, OFKTNode*> link_map_;         /**< The link name map to node */
-  Eigen::MatrixX2d limits_;                                     /**< The joint limits corresponding to joint_names_ */
+  tesseract_common::KinematicLimits limits_;                    /**< The kinematic limits */
   OFKTNode::UPtr root_;                                         /**< The root node of the tree */
   int revision_{ 0 };                                           /**< The environment revision number */
 
@@ -144,13 +144,13 @@ class OFKTStateSolver : public StateSolver
    * @param joint_name The joints name
    * @param parent_link_name The joints parent link name
    * @param child_link_name The joints child link name
-   * @param limits The limits container for new joints.
+   * @param kinematic_joints The vector to store new kinematic joints added to the solver
    */
   void addNode(const tesseract_scene_graph::Joint::ConstPtr& joint,
                const std::string& joint_name,
                const std::string& parent_link_name,
                const std::string& child_link_name,
-               std::vector<std::pair<std::string, std::array<double, 2>>>& limits);
+               std::vector<tesseract_scene_graph::Joint::ConstPtr>& kinematic_joints);
 
   /**
    * @brief Remove a node and all of its children

tesseract/tesseract_environment/src/kdl/kdl_state_solver.cpp

@@ -56,7 +56,7 @@ bool KDLStateSolver::init(const KDLStateSolver& solver)
   kdl_tree_ = solver.kdl_tree_;
   joint_to_qnr_ = solver.joint_to_qnr_;
   kdl_jnt_array_ = solver.kdl_jnt_array_;
-  joint_limits_ = solver.joint_limits_;
+  limits_ = solver.limits_;
   joint_names_ = solver.joint_names_;
 
   return true;
@@ -164,10 +164,10 @@ EnvState::ConstPtr KDLStateSolver::getCurrentState() const { return current_stat
 
 EnvState::Ptr KDLStateSolver::getRandomState() const
 {
-  return getState(joint_names_, tesseract_common::generateRandomNumber(joint_limits_));
+  return getState(joint_names_, tesseract_common::generateRandomNumber(limits_.joint_limits));
 }
 
-const Eigen::MatrixX2d& KDLStateSolver::getLimits() const { return joint_limits_; }
+const tesseract_common::KinematicLimits& KDLStateSolver::getLimits() const { return limits_; }
 
 bool KDLStateSolver::createKDETree()
 {
@@ -180,7 +180,9 @@ bool KDLStateSolver::createKDETree()
 
   current_state_ = std::make_shared<EnvState>();
   kdl_jnt_array_.resize(kdl_tree_.getNrOfJoints());
-  joint_limits_.resize(kdl_tree_.getNrOfJoints(), 2);
+  limits_.joint_limits.resize(static_cast<long int>(kdl_tree_.getNrOfJoints()), 2);
+  limits_.velocity_limits.resize(static_cast<long int>(kdl_tree_.getNrOfJoints()));
+  limits_.acceleration_limits.resize(static_cast<long int>(kdl_tree_.getNrOfJoints()));
   joint_names_.resize(kdl_tree_.getNrOfJoints());
   size_t j = 0;
   for (const auto& seg : kdl_tree_.getSegments())
@@ -197,8 +199,10 @@ bool KDLStateSolver::createKDETree()
 
     // Store joint limits.
     const auto& sj = scene_graph_->getJoint(jnt.getName());
-    joint_limits_(static_cast<long>(j), 0) = sj->limits->lower;
-    joint_limits_(static_cast<long>(j), 0) = sj->limits->upper;
+    limits_.joint_limits(static_cast<long>(j), 0) = sj->limits->lower;
+    limits_.joint_limits(static_cast<long>(j), 1) = sj->limits->upper;
+    limits_.velocity_limits(static_cast<long>(j)) = sj->limits->velocity;
+    limits_.acceleration_limits(static_cast<long>(j)) = sj->limits->acceleration;
 
     j++;
   }

tesseract/tesseract_environment/src/ofkt/ofkt_state_solver.cpp

@@ -38,9 +38,9 @@ namespace tesseract_environment
 struct ofkt_builder : public boost::dfs_visitor<>
 {
   ofkt_builder(OFKTStateSolver& tree,
-               std::vector<std::pair<std::string, std::array<double, 2>>>& limits,
+               std::vector<tesseract_scene_graph::Joint::ConstPtr>& kinematic_joints,
                std::string prefix = "")
-    : tree_(tree), limits_(limits), prefix_(prefix)
+    : tree_(tree), kinematic_joints_(kinematic_joints), prefix_(prefix)
   {
   }
 
@@ -60,12 +60,12 @@ struct ofkt_builder : public boost::dfs_visitor<>
     std::string parent_link_name = prefix_ + joint->parent_link_name;
     std::string child_link_name = prefix_ + joint->child_link_name;
 
-    tree_.addNode(joint, joint_name, parent_link_name, child_link_name, limits_);
+    tree_.addNode(joint, joint_name, parent_link_name, child_link_name, kinematic_joints_);
   }
 
 protected:
   OFKTStateSolver& tree_;
-  std::vector<std::pair<std::string, std::array<double, 2>>>& limits_;
+  std::vector<tesseract_scene_graph::Joint::ConstPtr>& kinematic_joints_;
   std::string prefix_;
 };
 
@@ -156,9 +156,9 @@ bool OFKTStateSolver::init(tesseract_scene_graph::SceneGraph::ConstPtr scene_gra
   link_map_[root_name] = root_.get();
   current_state_->link_transforms[root_name] = root_->getWorldTransformation();
 
-  std::vector<std::pair<std::string, std::array<double, 2>>> limits;
-  limits.reserve(scene_graph->getJoints().size());
-  ofkt_builder builder(*this, limits);
+  std::vector<tesseract_scene_graph::Joint::ConstPtr> kinematic_joints;
+  kinematic_joints.reserve(scene_graph->getJoints().size());
+  ofkt_builder builder(*this, kinematic_joints);
 
   std::map<tesseract_scene_graph::SceneGraph::Vertex, size_t> index_map;
   boost::associative_property_map<std::map<tesseract_scene_graph::SceneGraph::Vertex, size_t>> prop_index_map(
@@ -174,11 +174,15 @@ bool OFKTStateSolver::init(tesseract_scene_graph::SceneGraph::ConstPtr scene_gra
       boost::visitor(builder).root_vertex(scene_graph->getVertex(root_name)).vertex_index_map(prop_index_map));
 
   // Populate Joint Limits
-  limits_.resize(static_cast<long>(limits.size()), 2);
-  for (std::size_t i = 0; i < limits.size(); ++i)
+  limits_.joint_limits.resize(static_cast<long int>(kinematic_joints.size()), 2);
+  limits_.velocity_limits.resize(static_cast<long int>(kinematic_joints.size()));
+  limits_.acceleration_limits.resize(static_cast<long int>(kinematic_joints.size()));
+  for (std::size_t i = 0; i < kinematic_joints.size(); ++i)
   {
-    limits_(static_cast<long>(i), 0) = limits[i].second[0];
-    limits_(static_cast<long>(i), 1) = limits[i].second[1];
+    limits_.joint_limits(static_cast<long>(i), 0) = kinematic_joints[i]->limits->lower;
+    limits_.joint_limits(static_cast<long>(i), 1) = kinematic_joints[i]->limits->upper;
+    limits_.velocity_limits(static_cast<long>(i)) = kinematic_joints[i]->limits->velocity;
+    limits_.acceleration_limits(static_cast<long>(i)) = kinematic_joints[i]->limits->acceleration;
   }
 
   revision_ = 1;
@@ -263,15 +267,15 @@ EnvState::ConstPtr OFKTStateSolver::getCurrentState() const { return current_sta
 
 EnvState::Ptr OFKTStateSolver::getRandomState() const
 {
-  return getState(joint_names_, tesseract_common::generateRandomNumber(limits_));
+  return getState(joint_names_, tesseract_common::generateRandomNumber(limits_.joint_limits));
 }
 
-const Eigen::MatrixX2d& OFKTStateSolver::getLimits() const { return limits_; }
+const tesseract_common::KinematicLimits& OFKTStateSolver::getLimits() const { return limits_; }
 
 void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
 {
-  std::vector<std::pair<std::string, std::array<double, 2>>> new_limits;
-  new_limits.reserve(commands.size());
+  std::vector<tesseract_scene_graph::Joint::ConstPtr> new_kinematic_joints;
+  new_kinematic_joints.reserve(commands.size());
 
   std::vector<std::string> removed_joints;
   removed_joints.reserve(commands.size());
@@ -292,7 +296,7 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
         const auto& cmd = static_cast<const tesseract_environment::AddCommand&>(*command);
         const tesseract_scene_graph::Joint::ConstPtr& joint = cmd.getJoint();
 
-        addNode(joint, joint->getName(), joint->parent_link_name, joint->child_link_name, new_limits);
+        addNode(joint, joint->getName(), joint->parent_link_name, joint->child_link_name, new_kinematic_joints);
         break;
       }
       case tesseract_environment::CommandType::MOVE_LINK:
@@ -317,7 +321,7 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
 
         nodes_.erase(old_joint_name);
 
-        addNode(joint, joint->getName(), joint->parent_link_name, joint->child_link_name, new_limits);
+        addNode(joint, joint->getName(), joint->parent_link_name, joint->child_link_name, new_kinematic_joints);
         break;
       }
       case tesseract_environment::CommandType::MOVE_JOINT:
@@ -369,9 +373,9 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
         const auto& cmd = static_cast<const tesseract_environment::AddSceneGraphCommand&>(*command);
         const tesseract_scene_graph::Joint::ConstPtr& joint = cmd.getJoint();
 
-        addNode(joint, joint->getName(), joint->parent_link_name, joint->child_link_name, new_limits);
+        addNode(joint, joint->getName(), joint->parent_link_name, joint->child_link_name, new_kinematic_joints);
 
-        ofkt_builder builder(*this, new_limits, cmd.getPrefix());
+        ofkt_builder builder(*this, new_kinematic_joints, cmd.getPrefix());
 
         std::map<tesseract_scene_graph::SceneGraph::Vertex, size_t> index_map;
         boost::associative_property_map<std::map<tesseract_scene_graph::SceneGraph::Vertex, size_t>> prop_index_map(
@@ -400,8 +404,8 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
           // Assign the lower/upper. Velocity, acceleration, and effort are ignored
           if (it != limits.end())
           {
-            limits_(static_cast<Eigen::Index>(i), 0) = it->second.first;
-            limits_(static_cast<Eigen::Index>(i), 1) = it->second.second;
+            limits_.joint_limits(static_cast<Eigen::Index>(i), 0) = it->second.first;
+            limits_.joint_limits(static_cast<Eigen::Index>(i), 1) = it->second.second;
           }
         }
 
@@ -429,26 +433,49 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
         std::remove_if(joint_names_.begin(), joint_names_.end(), [removed_joints](const std::string& joint_name) {
           return (std::find(removed_joints.begin(), removed_joints.end(), joint_name) != removed_joints.end());
         }));
-    Eigen::MatrixX2d l1(limits_.rows() - static_cast<long>(removed_joints.size()), 2);
+
+    tesseract_common::KinematicLimits l1;
+    l1.joint_limits.resize(static_cast<long int>(joint_names_.size()), 2);
+    l1.velocity_limits.resize(static_cast<long int>(joint_names_.size()));
+    l1.acceleration_limits.resize(static_cast<long int>(joint_names_.size()));
+
     long cnt = 0;
-    for (long i = 0; i < limits_.rows(); ++i)
+    for (long i = 0; i < limits_.joint_limits.rows(); ++i)
+    {
       if (std::find(removed_joints_indices.begin(), removed_joints_indices.end(), i) == removed_joints_indices.end())
-        l1.row(cnt++) = limits_.row(i);
+      {
+        l1.joint_limits.row(cnt) = limits_.joint_limits.row(i);
+        l1.velocity_limits(cnt) = limits_.velocity_limits(i);
+        l1.acceleration_limits(cnt) = limits_.acceleration_limits(i);
+        ++cnt;
+      }
+    }
 
     limits_ = l1;
   }
 
   // Populate Joint Limits
-  if (new_limits.empty() == false)
+  if (new_kinematic_joints.empty() == false)
   {
-    Eigen::MatrixX2d l(limits_.rows() + static_cast<long>(new_limits.size()), 2);
-    Eigen::MatrixX2d l2(static_cast<long>(new_limits.size()), 2);
-    for (std::size_t i = 0; i < new_limits.size(); ++i)
+    tesseract_common::KinematicLimits l;
+    long s = limits_.joint_limits.rows() + static_cast<long>(new_kinematic_joints.size());
+    l.joint_limits.resize(s, 2);
+    l.velocity_limits.resize(s);
+    l.acceleration_limits.resize(s);
+
+    limits_.joint_limits.block(0, 0, limits_.joint_limits.rows(), 2) = limits_.joint_limits;
+    limits_.velocity_limits.head(limits_.joint_limits.rows()) = limits_.velocity_limits;
+    limits_.acceleration_limits.head(limits_.joint_limits.rows()) = limits_.acceleration_limits;
+
+    long cnt = limits_.joint_limits.size();
+    for (std::size_t i = 0; i < new_kinematic_joints.size(); ++i)
     {
-      l2(static_cast<long>(i), 0) = new_limits[i].second[0];
-      l2(static_cast<long>(i), 1) = new_limits[i].second[1];
+      limits_.joint_limits(cnt, 0) = new_kinematic_joints[i]->limits->lower;
+      limits_.joint_limits(cnt, 1) = new_kinematic_joints[i]->limits->upper;
+      limits_.velocity_limits(cnt) = new_kinematic_joints[i]->limits->velocity;
+      limits_.acceleration_limits(cnt) = new_kinematic_joints[i]->limits->acceleration;
+      ++cnt;
     }
-    l << limits_, l2;
     limits_ = l;
   }
 
@@ -514,7 +541,7 @@ void OFKTStateSolver::addNode(const tesseract_scene_graph::Joint::ConstPtr& join
                               const std::string& joint_name,
                               const std::string& parent_link_name,
                               const std::string& child_link_name,
-                              std::vector<std::pair<std::string, std::array<double, 2>>>& limits)
+                              std::vector<tesseract_scene_graph::Joint::ConstPtr>& kinematic_joints)
 {
   OFKTNode::UPtr n;
   switch (joint->type)
@@ -544,8 +571,7 @@ void OFKTStateSolver::addNode(const tesseract_scene_graph::Joint::ConstPtr& join
       current_state_->joint_transforms[n->getJointName()] = n->getWorldTransformation();
       nodes_[joint_name] = std::move(n);
       joint_names_.push_back(joint_name);
-      limits.push_back(
-          std::make_pair(joint_name, std::array<double, 2>({ joint->limits->lower, joint->limits->upper })));
+      kinematic_joints.push_back(joint);
       break;
     }
     case tesseract_scene_graph::JointType::CONTINUOUS:
@@ -560,8 +586,7 @@ void OFKTStateSolver::addNode(const tesseract_scene_graph::Joint::ConstPtr& join
       current_state_->joint_transforms[n->getJointName()] = n->getWorldTransformation();
       nodes_[joint_name] = std::move(n);
       joint_names_.push_back(joint_name);
-      limits.push_back(
-          std::make_pair(joint_name, std::array<double, 2>({ joint->limits->lower, joint->limits->upper })));
+      kinematic_joints.push_back(joint);
       break;
     }
     case tesseract_scene_graph::JointType::PRISMATIC:
@@ -576,8 +601,7 @@ void OFKTStateSolver::addNode(const tesseract_scene_graph::Joint::ConstPtr& join
       current_state_->joint_transforms[n->getJointName()] = n->getWorldTransformation();
       nodes_[joint_name] = std::move(n);
       joint_names_.push_back(joint_name);
-      limits.push_back(
-          std::make_pair(joint_name, std::array<double, 2>({ joint->limits->lower, joint->limits->upper })));
+      kinematic_joints.push_back(joint);
       break;
     }
     default: