Add ability to replace link and joint pair where the link is the child link of joint

Levi Armstrong · Levi-Armstrong · commit 7239e5e24859 · 2021-02-16T19:19:48.000-06:00
diff --git a/tesseract_environment/include/tesseract_environment/core/commands/add_link_command.h b/tesseract_environment/include/tesseract_environment/core/commands/add_link_command.h
@@ -72,17 +72,29 @@ class AddLinkCommand : public Command
   /**
    * @brief Adds a link and joint in the environment
    *
-   * If the link or joint exists:
+   * If the link and joint exist and replace is allowed
+   *
+   *        This command will replace both link and joint if the link is the child link, otherwise this results in error
+   *
+   * If the link and joint exist and replace is not allowed
+   *
+   *        This command should result in an error
+   *
+   * If the link or joint only 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.
+   * @param replace_allowed If true then if the link and joint exists it will be replaced, otherwise if false it will
+   * fail.
    */
-  AddLinkCommand(const tesseract_scene_graph::Link& link, const tesseract_scene_graph::Joint& joint)
+  AddLinkCommand(const tesseract_scene_graph::Link& link,
+                 const tesseract_scene_graph::Joint& joint,
+                 bool replace_allowed = false)
     : link_(std::make_shared<tesseract_scene_graph::Link>(link.clone()))
     , joint_(std::make_shared<tesseract_scene_graph::Joint>(joint.clone()))
+    , replace_allowed_(replace_allowed)
   {
     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 "
diff --git a/tesseract_environment/src/core/environment.cpp b/tesseract_environment/src/core/environment.cpp
@@ -1063,7 +1063,10 @@ bool Environment::addSceneGraph(const tesseract_scene_graph::SceneGraph& scene_g
 
 bool Environment::applyAddCommand(AddLinkCommand::ConstPtr cmd)
 {
+  // The command should not allow this to occur but adding an assert to catch if something changes
   assert(!(!cmd->getLink() && !cmd->getJoint()));
+  assert(!((cmd->getLink() != nullptr) && (cmd->getJoint() != nullptr) &&
+           (cmd->getJoint()->child_link_name != cmd->getLink()->getName())));
 
   bool link_exists = false;
   bool joint_exists = false;
@@ -1088,16 +1091,28 @@ bool Environment::applyAddCommand(AddLinkCommand::ConstPtr cmd)
     return false;
   }
 
-  if ((link_exists && cmd->getJoint()))
+  if ((joint_exists && !cmd->replaceAllowed()))
   {
-    CONSOLE_BRIDGE_logWarn("Tried to add link (%s) which already exists with a joint provided. This is not supported.",
+    CONSOLE_BRIDGE_logWarn("Tried to replace link (%s) and joint (%s) where the joint exist but the link does not. "
+                           "This is not supported.",
+                           link_name.c_str(),
+                           joint_name.c_str());
+    return false;
+  }
+
+  if (!link_exists && joint_exists)
+  {
+    CONSOLE_BRIDGE_logWarn("Tried to add link (%s) which already exists with a joint provided which does not exist. "
+                           "This is not supported.",
                            link_name.c_str());
     return false;
   }
 
-  if (joint_exists)
+  if ((link_exists && cmd->getJoint() && !joint_exists))
   {
-    CONSOLE_BRIDGE_logWarn("Tried to add link (%s) with a joint that already exists.", joint_name.c_str());
+    CONSOLE_BRIDGE_logWarn("Tried to add link (%s) which already exists with a joint provided which does not exist. "
+                           "This is not supported.",
+                           link_name.c_str());
     return false;
   }
 
@@ -1106,6 +1121,45 @@ bool Environment::applyAddCommand(AddLinkCommand::ConstPtr cmd)
     if (!scene_graph_->addLink(*cmd->getLink(), true))
       return false;
   }
+  else if (link_exists && joint_exists)
+  {  // A link and joint pair is being replaced
+    tesseract_scene_graph::Link::ConstPtr orig_link = getLink(link_name);
+    tesseract_scene_graph::Joint::ConstPtr orig_joint = getJoint(joint_name);
+
+    if (orig_joint->child_link_name != orig_link->getName())
+    {
+      CONSOLE_BRIDGE_logWarn("Tried to replace link (%s) and joint (%s) which are currently not linked. This is not "
+                             "supported.",
+                             link_name.c_str(),
+                             joint_name.c_str());
+      return false;
+    }
+
+    if (!scene_graph_->addLink(*cmd->getLink(), true))
+      return false;
+
+    if (!scene_graph_->removeJoint(joint_name))
+    {
+      // Replace with original link
+      if (!scene_graph_->addLink(*orig_link, true))
+        throw std::runtime_error("Environment: Failed to replace link and joint and reset to original state.");
+
+      return false;
+    }
+
+    if (!scene_graph_->addJoint(*cmd->getJoint()))
+    {
+      // Replace with original link
+      if (!scene_graph_->addLink(*orig_link, true))
+        throw std::runtime_error("Environment: Failed to replace link and joint reset to original state.");
+
+      // Replace with original link
+      if (!scene_graph_->addJoint(*orig_joint))
+        throw std::runtime_error("Environment: Failed to replace link and joint and reset to original state.");
+
+      return false;
+    }
+  }
   else if (!link_exists && !cmd->getJoint())
   {  // Add a new link is being added attached to the world
     std::string joint_name = "joint_" + link_name;
diff --git a/tesseract_environment/src/ofkt/ofkt_state_solver.cpp b/tesseract_environment/src/ofkt/ofkt_state_solver.cpp
@@ -328,13 +328,20 @@ void OFKTStateSolver::onEnvironmentChanged(const Commands& commands)
 
         // These check are handled by the environment but just as a precaution adding asserts here
         assert(!(link_exists && !cmd.replaceAllowed()));
-        assert(!(link_exists && cmd.getJoint()));
-        assert(!joint_exists);
+        assert(!(joint_exists && !cmd.replaceAllowed()));
+        assert(!(link_exists && cmd.getJoint() && !joint_exists));
+        assert(!(!link_exists && joint_exists));
 
         if (link_exists && !cmd.getJoint())
         {  // A link is being replaced there is nothing to be done
           break;
         }
+        else if (link_exists && joint_exists)
+        {  // A link and joint is being replaced
+          assert(!((cmd.getLink() != nullptr) && (cmd.getJoint() != nullptr) &&
+                   (cmd.getJoint()->child_link_name != cmd.getLink()->getName())));
+          replaceJointHelper(new_kinematic_joints, cmd.getJoint());
+        }
         else
         {  // A new joint and link was added
           const tesseract_scene_graph::Joint::ConstPtr& joint = cmd.getJoint();
diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp
@@ -1690,6 +1690,85 @@ void runApplyCommandsStateSolverCompareTest()
     runGetLinkTransformsTest(*base_env);
     runGetLinkTransformsTest(*compare_env);
   }
+  {  // Replace link and joint which is allowed
+    // Get the environment
+    auto base_env = getEnvironment<KDLStateSolver>();
+    auto compare_env = getEnvironment<S>();
+
+    Link link_1("link_1");
+    Joint joint_1("joint_a1");
+    joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
+    joint_1.parent_link_name = "base_link";
+    joint_1.child_link_name = "link_1";
+    joint_1.type = JointType::FIXED;
+
+    Commands commands{ std::make_shared<AddLinkCommand>(link_1, joint_1, true) };
+    EXPECT_TRUE(base_env->applyCommands(commands));
+    EXPECT_TRUE(compare_env->applyCommands(commands));
+    runCompareStateSolvers(*base_env->getStateSolver(), *compare_env->getStateSolver());
+    runGetLinkTransformsTest(*base_env);
+    runGetLinkTransformsTest(*compare_env);
+  }
+
+  {  // Replace link and joint which is not allowed
+    // Get the environment
+    auto base_env = getEnvironment<KDLStateSolver>();
+    auto compare_env = getEnvironment<S>();
+
+    Link link_1("link_1");
+    Joint joint_1("joint_a1");
+    joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
+    joint_1.parent_link_name = "base_link";
+    joint_1.child_link_name = "link_1";
+    joint_1.type = JointType::FIXED;
+
+    Commands commands{ std::make_shared<AddLinkCommand>(link_1, joint_1, false) };
+    EXPECT_FALSE(base_env->applyCommands(commands));
+    EXPECT_FALSE(compare_env->applyCommands(commands));
+    runCompareStateSolvers(*base_env->getStateSolver(), *compare_env->getStateSolver());
+    runGetLinkTransformsTest(*base_env);
+    runGetLinkTransformsTest(*compare_env);
+  }
+
+  {  // Replace joint wich exist but the link does not which should fail
+    // Get the environment
+    auto base_env = getEnvironment<KDLStateSolver>();
+    auto compare_env = getEnvironment<S>();
+
+    Link link_1("link_2_does_not_exist");
+    Joint joint_1("joint_a1");
+    joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
+    joint_1.parent_link_name = "base_link";
+    joint_1.child_link_name = "link_2_does_not_exist";
+    joint_1.type = JointType::FIXED;
+
+    Commands commands{ std::make_shared<AddLinkCommand>(link_1, joint_1, true) };
+    EXPECT_FALSE(base_env->applyCommands(commands));
+    EXPECT_FALSE(compare_env->applyCommands(commands));
+    runCompareStateSolvers(*base_env->getStateSolver(), *compare_env->getStateSolver());
+    runGetLinkTransformsTest(*base_env);
+    runGetLinkTransformsTest(*compare_env);
+  }
+
+  {  // Replace link and joint which is not allowed but they are not currently linked
+    // Get the environment
+    auto base_env = getEnvironment<KDLStateSolver>();
+    auto compare_env = getEnvironment<S>();
+
+    Link link_1("link_2");
+    Joint joint_1("joint_a1");
+    joint_1.parent_to_joint_origin_transform.translation()(0) = 1.25;
+    joint_1.parent_link_name = "base_link";
+    joint_1.child_link_name = "link_2";
+    joint_1.type = JointType::FIXED;
+
+    Commands commands{ std::make_shared<AddLinkCommand>(link_1, joint_1, false) };
+    EXPECT_FALSE(base_env->applyCommands(commands));
+    EXPECT_FALSE(compare_env->applyCommands(commands));
+    runCompareStateSolvers(*base_env->getStateSolver(), *compare_env->getStateSolver());
+    runGetLinkTransformsTest(*base_env);
+    runGetLinkTransformsTest(*compare_env);
+  }
 
   {  // Replace link which is not allowed
     // Get the environment
