Add support for replacing links and joints

Author: Levi Armstrong

tesseract_environment/include/tesseract_environment/core/command.h

@@ -56,7 +56,8 @@ enum class CommandType
   CHANGE_JOINT_ACCELERATION_LIMITS = 15,
   ADD_KINEMATICS_INFORMATION = 16,
   CHANGE_DEFAULT_CONTACT_MARGIN = 17,
-  CHANGE_PAIR_CONTACT_MARGIN = 18
+  CHANGE_PAIR_CONTACT_MARGIN = 18,
+  REPLACE_JOINT = 19
 };
 
 class Command

tesseract_environment/include/tesseract_environment/core/commands.h

@@ -47,6 +47,7 @@
 #include <tesseract_environment/core/commands/remove_allowed_collision_link_command.h>
 #include <tesseract_environment/core/commands/remove_joint_command.h>
 #include <tesseract_environment/core/commands/remove_link_command.h>
+#include <tesseract_environment/core/commands/replace_joint_command.h>
 
 #ifdef SWIG
 
@@ -56,6 +57,7 @@
 %shared_ptr(tesseract_environment::MoveJointCommand)
 %shared_ptr(tesseract_environment::RemoveLinkCommand)
 %shared_ptr(tesseract_environment::RemoveJointCommand)
+%shared_ptr(tesseract_environment::ReplaceJointCommand)
 %shared_ptr(tesseract_environment::ChangeLinkOriginCommand)
 %shared_ptr(tesseract_environment::ChangeJointOriginCommand)
 %shared_ptr(tesseract_environment::ChangeLinkCollisionEnabledCommand)
@@ -75,15 +77,16 @@
 %shared_factory(
   tesseract_environment::Command,
   tesseract_environment::AddLinkCommand,
-	tesseract_environment::MoveLinkCommand,
-	tesseract_environment::MoveJointCommand,
-	tesseract_environment::RemoveLinkCommand,
-	tesseract_environment::RemoveJointCommand,
-	tesseract_environment::ChangeLinkOriginCommand,
-	tesseract_environment::ChangeJointOriginCommand,
-	tesseract_environment::ChangeLinkCollisionEnabledCommand,
-	tesseract_environment::ChangeLinkVisibilityCommand,
-	tesseract_environment::AddAllowedCollisionCommand,
+  tesseract_environment::MoveLinkCommand,
+  tesseract_environment::MoveJointCommand,
+  tesseract_environment::RemoveLinkCommand,
+  tesseract_environment::RemoveJointCommand,
+  tesseract_environment::ReplaceJointCommand,
+  tesseract_environment::ChangeLinkOriginCommand,
+  tesseract_environment::ChangeJointOriginCommand,
+  tesseract_environment::ChangeLinkCollisionEnabledCommand,
+  tesseract_environment::ChangeLinkVisibilityCommand,
+  tesseract_environment::AddAllowedCollisionCommand,
   tesseract_environment::RemoveAllowedCollisionCommand,
   tesseract_environment::RemoveAllowedCollisionLinkCommand,
   tesseract_environment::AddSceneGraphCommand,

tesseract_environment/include/tesseract_environment/core/commands/add_link_command.h

@@ -23,8 +23,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef TESSERACT_ENVIRONMENT_ADD_COMMAND_H
-#define TESSERACT_ENVIRONMENT_ADD_COMMAND_H
+#ifndef TESSERACT_ENVIRONMENT_ADD_LINK_COMMAND_H
+#define TESSERACT_ENVIRONMENT_ADD_LINK_COMMAND_H
 
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
@@ -44,36 +44,62 @@ class AddLinkCommand : public Command
   using ConstPtr = std::shared_ptr<const AddLinkCommand>;
 
   /**
-   * @brief Adds a link to the environment
+   * @brief Adds or replace a link to the environment
    *
-   *        This method should attach the link to the root link with a fixed joint
+   * If the link exists and replace_allowed equals true:
+   *
+   *        This command should replace the current link with the new link
+   *
+   * If the link exists and replace_allowed equals false:
+   *
+   *        This command should result in an error
+   *
+   * If the link does not exist:
+   *
+   *        This command should attach the link to the root link with a fixed joint
    *        with a joint name of joint_{link name}".
    *
    * @param link The link to be added to the graph
+   * @param replace_allowed If true then if the link exists it will be replaced, otherwise if false it will fail.
    */
-  AddLinkCommand(const tesseract_scene_graph::Link& link)
-    : link_(std::make_shared<tesseract_scene_graph::Link>(link.clone())), joint_(nullptr)
+  AddLinkCommand(const tesseract_scene_graph::Link& link, bool replace_allowed = false)
+    : link_(std::make_shared<tesseract_scene_graph::Link>(link.clone()))
+    , joint_(nullptr)
+    , replace_allowed_(replace_allowed)
   {
   }
 
   /**
-   * @brief Adds a link to the environment
+   * @brief Adds a link and joint in the environment
+   *
+   * If the link or joint exists:
+   *
+   *        This command should result in an error
+   *
    * @param link The link to be added to the graph
    * @param joint The joint to be used to attach link to environment
+   * @param replace_allowed If true then if the link exists it will be replaced, otherwise if false it will fail.
    */
   AddLinkCommand(const tesseract_scene_graph::Link& link, const tesseract_scene_graph::Joint& joint)
     : link_(std::make_shared<tesseract_scene_graph::Link>(link.clone()))
     , joint_(std::make_shared<tesseract_scene_graph::Joint>(joint.clone()))
   {
+    if (joint_->child_link_name != link.getName())
+      throw std::runtime_error("AddLinkCommand: The provided joint child link name must equal the name of the provided "
+                               "link.");
+
+    /** @todo if joint is not fixed we should verify that limits are provided */
   }
 
   CommandType getType() const final { return CommandType::ADD_LINK; }
   const tesseract_scene_graph::Link::ConstPtr& getLink() const { return link_; }
   const tesseract_scene_graph::Joint::ConstPtr& getJoint() const { return joint_; }
+  bool replaceAllowed() const { return replace_allowed_; }
 
 private:
   tesseract_scene_graph::Link::ConstPtr link_;
   tesseract_scene_graph::Joint::ConstPtr joint_;
+  bool replace_allowed_{ false };
 };
 }  // namespace tesseract_environment
 

tesseract_environment/include/tesseract_environment/core/commands/add_scene_graph_command.h

@@ -69,7 +69,7 @@ class AddSceneGraphCommand : public Command
    */
   AddSceneGraphCommand(const tesseract_scene_graph::SceneGraph& scene_graph,
                        const tesseract_scene_graph::Joint& joint,
-                       std::string prefix)
+                       std::string prefix = "")
     : scene_graph_(scene_graph.clone())
     , joint_(std::make_shared<tesseract_scene_graph::Joint>(joint.clone()))
     , prefix_(std::move(prefix))

tesseract_environment/include/tesseract_environment/core/commands/replace_joint_command.h

@@ -0,0 +1,76 @@
+/**
+ * @file replace_joint_command.h
+ * @brief Used to replace joint in environment
+ *
+ * @author Levi Armstrong
+ * @date February 11, 2021
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2021, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef TESSERACT_ENVIRONMENT_REPLACE_JOINT_COMMAND_H
+#define TESSERACT_ENVIRONMENT_REPLACE_JOINT_COMMAND_H
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <memory>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_environment/core/command.h>
+#include <tesseract_scene_graph/joint.h>
+
+namespace tesseract_environment
+{
+class ReplaceJointCommand : public Command
+{
+public:
+  using Ptr = std::shared_ptr<ReplaceJointCommand>;
+  using ConstPtr = std::shared_ptr<const ReplaceJointCommand>;
+
+  /**
+   * @brief Replace a joint in the environment
+   *
+   * If the joint does not exist:
+   *
+   *        This command should result in an error
+   *
+   * If the child link is not the same:
+   *
+   *        This command should result in an error
+   *
+   * @param joint The joint to be replaced
+   */
+  ReplaceJointCommand(const tesseract_scene_graph::Joint& joint)
+    : joint_(std::make_shared<tesseract_scene_graph::Joint>(joint.clone()))
+  {
+    if (joint_->type != tesseract_scene_graph::JointType::FIXED)
+    {
+      //      if ()
+      /** @todo check limits */
+    }
+  }
+
+  CommandType getType() const final { return CommandType::REPLACE_JOINT; }
+  const tesseract_scene_graph::Joint::ConstPtr& getJoint() const { return joint_; }
+
+private:
+  tesseract_scene_graph::Joint::ConstPtr joint_;
+};
+
+}  // namespace tesseract_environment
+
+#endif  // TESSERACT_ENVIRONMENT_REPLACE_JOINT_COMMAND_H

tesseract_environment/include/tesseract_environment/core/environment.h

@@ -213,7 +213,11 @@ class Environment
    */
   Environment::Ptr clone() const;
 
-  /** @brief reset to initialized state */
+  /**
+   * @brief reset to initialized state
+   * @details If the environment has not been initialized then this returns false
+   * @return True if environment was successfully reset, otherwise false.
+   */
   bool reset();
 
   /** @brief clear content and uninitialize */
@@ -449,7 +453,7 @@ class Environment
    * @brief Get the transform corresponding to the link.
    * @return Transform and is identity when no transform is available.
    */
-  virtual const Eigen::Isometry3d& getLinkTransform(const std::string& link_name) const;
+  virtual Eigen::Isometry3d getLinkTransform(const std::string& link_name) const;
 
   /**
    * @brief Returns a clone of the environments state solver
@@ -765,6 +769,7 @@ class Environment
   bool applyMoveJointCommand(const MoveJointCommand::ConstPtr& cmd);
   bool applyRemoveLinkCommand(const RemoveLinkCommand::ConstPtr& cmd);
   bool applyRemoveJointCommand(const RemoveJointCommand::ConstPtr& cmd);
+  bool applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& cmd);
   bool applyChangeLinkOriginCommand(const ChangeLinkOriginCommand::ConstPtr& cmd);
   bool applyChangeJointOriginCommand(const ChangeJointOriginCommand::ConstPtr& cmd);
   bool applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisionEnabledCommand::ConstPtr& cmd);

tesseract_environment/include/tesseract_environment/core/state_solver.h

@@ -100,6 +100,12 @@ class StateSolver
    */
   virtual EnvState::Ptr getRandomState() const = 0;
 
+  /**
+   * @brief Get the vector of joint names which align with the limits
+   * @return A vector of joint names
+   */
+  virtual const std::vector<std::string>& getJointNames() const = 0;
+
   /**
    * @brief Getter for kinematic limits
    * @return The kinematic limits

tesseract_environment/include/tesseract_environment/kdl/kdl_state_solver.h

@@ -58,6 +58,8 @@ class KDLStateSolver : public StateSolver
 
   EnvState::Ptr getRandomState() const override;
 
+  const std::vector<std::string>& getJointNames() const override;
+
   const tesseract_common::KinematicLimits& getLimits() const override;
 
 private:

tesseract_environment/include/tesseract_environment/ofkt/ofkt_state_solver.h

@@ -101,6 +101,8 @@ class OFKTStateSolver : public StateSolver
 
   EnvState::Ptr getRandomState() const override;
 
+  const std::vector<std::string>& getJointNames() const override;
+
   const tesseract_common::KinematicLimits& getLimits() const override;
 
 private:
@@ -164,6 +166,30 @@ class OFKTStateSolver : public StateSolver
    */
   void removeNode(OFKTNode* node, std::vector<std::string>& removed_joints, std::vector<long>& removed_joints_indices);
 
+  /**
+   * @brief This a helper function for moving a link
+   * @param new_kinematic_joints The vector to store new kinematic joints added to the solver
+   * @param joint The joint performing the move
+   */
+  void moveLinkHelper(std::vector<tesseract_scene_graph::Joint::ConstPtr>& new_kinematic_joints,
+                      const tesseract_scene_graph::Joint::ConstPtr& joint);
+
+  /**
+   * @brief This is a helper function for replacing a joint
+   * @param new_kinematic_joints The vector to store new kinematic joints added to the solver
+   * @param joint The joint performing the replacement
+   */
+  void replaceJointHelper(std::vector<tesseract_scene_graph::Joint::ConstPtr>& new_kinematic_joints,
+                          const tesseract_scene_graph::Joint::ConstPtr& joint);
+
+  /**
+   * @brief This will clean up member variables joint_names_ and limits_
+   * @param removed_joints The removed joint names container
+   * @param removed_joints_indices The removed joint names indices container
+   */
+  void removeJointHelper(const std::vector<std::string>& removed_joints,
+                         const std::vector<long>& removed_joints_indices);
+
   friend struct ofkt_builder;
 };