Calibrate src pose from perception for parts in the gripper

project/the_italian_job/tijchallenger/src/SceneConfigReader.cpp

@@ -240,10 +240,10 @@ SceneConfigReader::getListOfApproachHints(const std::string& robot_name) const
       build_hint(-3.50, +3.00, degreesToRadians(0)) },
     // first station left
     { build_hint(-7.50, +3.00, degreesToRadians(0)),
-      build_hint(-5.20, +3.00, degreesToRadians(0)) },
+      build_hint(-5.00, +3.00, degreesToRadians(0)) },
     // second station left
     { build_hint(-12.50, +3.00, degreesToRadians(0)),
-      build_hint(-10.20, +3.00, degreesToRadians(0)) },
+      build_hint(-10.00, +3.00, degreesToRadians(0)) },
     //
     // right
     //
@@ -252,10 +252,10 @@ SceneConfigReader::getListOfApproachHints(const std::string& robot_name) const
       build_hint(-3.50, -3.00, degreesToRadians(0)) },
     // first station left
     { build_hint(-7.50, -3.00, degreesToRadians(0)),
-      build_hint(-5.20, -3.00, degreesToRadians(0)) },
+      build_hint(-5.00, -3.00, degreesToRadians(0)) },
     // second station right
     { build_hint(-12.20, -3.00, degreesToRadians(0)),
-      build_hint(-10.20, -3.00, degreesToRadians(0)) },
+      build_hint(-10.00, -3.00, degreesToRadians(0)) },
     //
     // drop bucket
     //
@@ -273,10 +273,10 @@ SceneConfigReader::getListOfApproachHints(const std::string& robot_name) const
   auto add_tricky_agvs_hints = [&](const double x, const double y) {
     gantry_poses.emplace_back(
         PickAndPlacePoseHintsData{ build_hint(x, y + 0.1, degreesToRadians(0)),
-                                   build_hint(x + 0.2, y - 1.0, degreesToRadians(0)) });
+                                   build_hint(x + 0.2, y - 1.7, degreesToRadians(0)) });
     gantry_poses.emplace_back(
         PickAndPlacePoseHintsData{ build_hint(x, y - 0.1, degreesToRadians(0)),
-                                   build_hint(x + 0.2, y + 1.0, degreesToRadians(0)) });
+                                   build_hint(x + 0.2, y + 1.7, degreesToRadians(0)) });
   };
 
   // agv col 1

project/the_italian_job/tijcore/behavior/behavior.xml

@@ -334,6 +334,7 @@
                 <SubTree task_parameters="task_parameters" ID="GetInVerticalLandingPoseInJointSpace" target_pose="target_pose" />
                 <SubTree task_parameters="task_parameters" ID="GraspPartAtTargetPose" target_pose="target_pose" />
                 <SubTree task_parameters="task_parameters" ID="GetInVerticalLandingPoseInJointSpace" target_pose="target_pose" />
+                <CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossible task_parameters="{task_parameters}" />
                 <SubTree task_parameters="task_parameters" ID="SetupCollisionEnvelope" target_pose="target_pose" />
             </Sequence>
         </TraceLoggerDecorator>
@@ -733,7 +734,7 @@
                 <GetGripperIn3DPoseJoinSpace task_parameters="{task_parameters}" target_pose="{pre_insert_pose}" />
                 <RemoveRobotGripperPayloadEnvelope task_parameters="{task_parameters}" />
                 <ForceSuccess>
-                    <GetGripperIn3DPoseJoinSpace task_parameters="{task_parameters}" target_pose="{insert_pose}" />
+                    <GetGripperIn3DPoseCartesianSpace task_parameters="{task_parameters}" target_pose="{insert_pose}" />
                 </ForceSuccess>
                 <Delay delay_msec="1000">
                     <AlwaysSuccess />

project/the_italian_job/tijcore/include/tijcore/abstractions/PickAndPlaceRobotMovementsInterface.hpp

@@ -87,6 +87,8 @@ class PickAndPlaceRobotMovementsInterface
                                            const tijmath::RelativePose3& payload_pose) const = 0;
 
   virtual tijmath::RelativePose3 getCurrentRobotPose() const = 0;
+
+  virtual tijmath::RelativePose3 getCurrentEndEffectorPose() const = 0;
 };
 
 }  // namespace tijcore

project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode.hpp

@@ -0,0 +1,114 @@
+/* Copyright [2022] <TheItalianJob>
+ * Distributed under the MIT License (http://opensource.org/licenses/MIT)
+ * Author: Gerardo Puga */
+
+#pragma once
+
+// standard library
+#include <algorithm>
+#include <string>
+
+// external
+#include "behaviortree_cpp_v3/action_node.h"
+
+// tijcore
+#include <tijcore/abstractions/ResourceManagerInterface.hpp>
+#include <tijcore/tasking/BTTaskParameters.hpp>
+
+namespace tijcore
+{
+class CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode : public BT::SyncActionNode
+{
+public:
+  CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode(
+      const std::string& name, const BT::NodeConfiguration& config)
+    : SyncActionNode(name, config)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<BTTaskParameters::SharedPtr>("task_parameters"),
+    };
+  }
+
+  BT::NodeStatus tick() override
+  {
+    auto task_parameters = getInput<BTTaskParameters::SharedPtr>("task_parameters").value();
+
+    auto toolbox = task_parameters->toolbox.get();
+    auto unfiltered_model_perception_chain = toolbox->getUnfilteredModelPerceptionChain();
+    auto frame_transformer = toolbox->getFrameTransformer();
+    auto scene_reader = toolbox->getSceneConfigReader();
+    const auto world_frame = scene_reader->getWorldFrameId();
+    const auto adapter_ = task_parameters->primary_robot.value().resource();
+
+    const auto end_effector_pose = adapter_->getCurrentEndEffectorPose();
+
+    tijmath::RelativePose3 closest_part_pose;
+
+    // the following scope is only to find the pose of the part in view that's closest to the
+    // gripper
+    {
+      auto shortest_distance_to_pose = [&](const ObservedItem& lhs_locus,
+                                           const ObservedItem& rhs_locus) {
+        // get all poses in the same frame
+        auto lhs_pose =
+            frame_transformer->transformPoseToFrame(lhs_locus.pose, end_effector_pose.frameId());
+        auto rhs_pose =
+            frame_transformer->transformPoseToFrame(rhs_locus.pose, end_effector_pose.frameId());
+
+        const auto lhs_distance =
+            (lhs_pose.position().vector() - end_effector_pose.position().vector()).norm();
+        const auto rhs_distance =
+            (rhs_pose.position().vector() - end_effector_pose.position().vector()).norm();
+
+        return lhs_distance < rhs_distance;
+      };
+
+      auto visible_models = unfiltered_model_perception_chain->getObservedModels();
+
+      auto is_not_a_part = [&](const ObservedItem& item) {
+        return !item.item.is<QualifiedPartInfo>();
+      };
+
+      visible_models.erase(
+          std::remove_if(visible_models.begin(), visible_models.end(), is_not_a_part),
+          visible_models.end());
+
+      if (visible_models.size() == 0)
+      {
+        WARNING("No parts found in the scene, so the source part cannot be calibrated.");
+        return BT::NodeStatus::SUCCESS;
+      }
+
+      auto closest_part =
+          std::min_element(visible_models.begin(), visible_models.end(), shortest_distance_to_pose);
+
+      closest_part_pose =
+          frame_transformer->transformPoseToFrame(closest_part->pose, end_effector_pose.frameId());
+
+      const auto calibration_distance =
+          (closest_part_pose.position().vector() - end_effector_pose.position().vector()).norm();
+
+      if (calibration_distance > 0.2)
+      {
+        WARNING(
+            "The closest visible part in the scene is not close enough ({} m), and will be "
+            "ignored.",
+            calibration_distance);
+        return BT::NodeStatus::SUCCESS;
+      }
+    }
+
+    // we calculate the calibrated ee_to_payload pose as view through the cameras
+    task_parameters->ee_to_payload_iso =
+        adapter_->calculatePayloadIntoEndEffectorTransform(end_effector_pose, closest_part_pose);
+
+    return BT::NodeStatus::SUCCESS;
+  }
+
+};  // namespace tijcore
+
+}  // namespace tijcore

project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformNode.hpp

@@ -39,9 +39,22 @@ class CalculatePayloadIntoEndEffectorTransformNode : public BT::SyncActionNode
     auto end_effector_pose = getInput<tijmath::RelativePose3>("end_effector_pose").value();
     auto payload_pose = getInput<tijmath::RelativePose3>("payload_pose").value();
     auto task_parameters = getInput<BTTaskParameters::SharedPtr>("task_parameters").value();
-    const auto adapter_ = task_parameters->primary_robot.value().resource();
-    const auto payload_into_end_effector_transform =
-        adapter_->calculatePayloadIntoEndEffectorTransform(end_effector_pose, payload_pose);
+
+    tijmath::Isometry payload_into_end_effector_transform;
+
+    if (task_parameters->ee_to_payload_iso)
+    {
+      // we have actual data obtained from cameras
+      payload_into_end_effector_transform = task_parameters->ee_to_payload_iso.value();
+    }
+    else
+    {
+      // estimate from other data
+      const auto adapter_ = task_parameters->primary_robot.value().resource();
+      payload_into_end_effector_transform =
+          adapter_->calculatePayloadIntoEndEffectorTransform(end_effector_pose, payload_pose);
+    }
+
     setOutput("payload_into_end_effector_transform", payload_into_end_effector_transform);
     return BT::NodeStatus::SUCCESS;
   }

project/the_italian_job/tijcore/include/tijcore/btnodes/CalculateRegulatorPreInsertAndInsertPosesNode.hpp

@@ -52,12 +52,10 @@ class CalculateRegulatorPreInsertAndInsertPosesNode : public BT::SyncActionNode
 
     auto pre_insert_pose = target_in_world;
     pre_insert_pose.position().vector().z() += 0.06;
-    pre_insert_pose.position().vector().x() += 0.011;  // TODO(glpuga) fine tuning
     setOutput("pre_insert_pose", pre_insert_pose);
 
     auto insert_pose = target_in_world;
     insert_pose.position().vector().z() += 0.04;
-    insert_pose.position().vector().x() += 0.011;  // TODO(glpuga) fine tuning
     setOutput("insert_pose", insert_pose);
 
     auto gripper_exit_pose = target_in_world;

project/the_italian_job/tijcore/include/tijcore/btnodes/RandomizeTargetPoseNode.hpp

@@ -51,10 +51,10 @@ class RandomizeTargetPoseNode : public BT::SyncActionNode
 
     std::random_device rd;
     std::mt19937 gen(rd());
-    std::uniform_int_distribution<int> unif_dist(-4, 3);
+    std::uniform_int_distribution<int> unif_dist(-2, 1);
 
     const auto distance = max_radius;
-    const auto angle = static_cast<double>(unif_dist(gen)) * 2 * M_PI / 8.0;
+    const auto angle = static_cast<double>(unif_dist(gen)) * 2 * M_PI / 4.0;
 
     const auto x_diff = distance * std::cos(angle);
     const auto y_diff = distance * std::sin(angle);

project/the_italian_job/tijcore/include/tijcore/tasking/BTTaskParameters.hpp

@@ -13,6 +13,7 @@
 #include <tijcore/abstractions/ResourceManagerInterface.hpp>
 #include <tijcore/abstractions/SpatialMutualExclusionManagerInterface.hpp>
 #include <tijcore/coremodels/Toolbox.hpp>
+#include <tijmath/Isometry.hpp>
 
 namespace tijcore
 {
@@ -39,6 +40,8 @@ struct BTTaskParameters
   // Temporary state
   std::optional<SpatialMutualExclusionManagerInterface::VolumeHandle> spatial_lock_handle;
   std::optional<ResourceManagerInterface::ManagedLocusHandle> aux_locus;
+
+  std::optional<tijmath::Isometry> ee_to_payload_iso;
 };
 
 }  // namespace tijcore

project/the_italian_job/tijcore/src/BTRobotTaskFactory.cpp

@@ -26,6 +26,7 @@
 #include <tijcore/btnodes/AbortCurrentActionNode.hpp>
 #include <tijcore/btnodes/CalculateEndEffectorPoseFromPayloadPoseNode.hpp>
 #include <tijcore/btnodes/CalculateEnvelopeAndOffsetForVerticalPickUpNode.hpp>
+#include <tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode.hpp>
 #include <tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformNode.hpp>
 #include <tijcore/btnodes/CalculateRegulatorPreInsertAndInsertPosesNode.hpp>
 #include <tijcore/btnodes/CalculateSensorPreInsertAndInsertPosesNode.hpp>
@@ -154,6 +155,8 @@ void factoryLoaderMethod(BT::BehaviorTreeFactory& factory)
   factory.registerNodeType<HackyDestinationPoseUpdateNode>("HackyDestinationPoseUpdate");
   factory.registerNodeType<CalibrateSourcePartFromPerceptionIfPossibleNode>(
       "CalibrateSourcePartFromPerceptionIfPossible");
+  factory.registerNodeType<CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode>(
+      "CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossible");
 };
 
 }  // namespace

project/the_italian_job/tijcore/src/OrderProcessingStrategy.cpp

@@ -661,6 +661,9 @@ OrderProcessingStrategy::processAssemblyShipment(const OrderId& order,
 
   auto world_state = getInitialWorldState(target_container_name, shipment.products);
 
+  world_state.broken_parts.clear();
+  world_state.unwanted_parts.clear();
+
   //
   // with highest priority of all, we need to remove broken pieces
   if (world_state.broken_parts.size())

project/the_italian_job/tijcore/tests/mocks/PickAndPlaceRobotMovementsMock.hpp

@@ -84,6 +84,8 @@ class PickAndPlaceRobotMovementsMock : public PickAndPlaceRobotMovementsInterfac
                                                    const tijmath::RelativePose3& aim_target));
 
   MOCK_CONST_METHOD0(getCurrentRobotPose, tijmath::RelativePose3());
+
+  MOCK_CONST_METHOD0(getCurrentEndEffectorPose, tijmath::RelativePose3());
 };
 
 }  // namespace tijcore

project/the_italian_job/tijros/include/tijros/PickAndPlaceRobotMovements.hpp

@@ -88,6 +88,8 @@ class PickAndPlaceRobotMovements : public tijcore::PickAndPlaceRobotMovementsInt
 
   tijmath::RelativePose3 getCurrentRobotPose() const override;
 
+  tijmath::RelativePose3 getCurrentEndEffectorPose() const override;
+
 private:
   tijcore::PickAndPlaceRobotSpecificInterface::Ptr robot_specific_interface_;
   tijcore::Toolbox::SharedPtr toolbox_;

project/the_italian_job/tijros/src/PickAndPlaceRobotMovements.cpp

@@ -43,7 +43,7 @@ static const double twist_height_correction = 0.10;
 
 static const double pickup_displacement_jump_threshold_ = 10.0;
 static const double pickup_displacement_step_ = 0.00125;
-static const double max_planning_time_ = 10.0;
+static const double max_planning_time_ = 5.0;
 static const int max_planning_attempts_large = 100;
 static const int max_planning_attempts_small = 5;
 
@@ -680,8 +680,8 @@ void PickAndPlaceRobotMovements::buildObstacleSceneFromDescription() const
                                                    operation));
     collision_objects.push_back(createCollisionBox(item.name, "regulator_support",
                                                    item.frame_id,         //
-                                                   0.15, 0.15, 0.16,      // wx, wy, wz
-                                                   -0.222, -0.164, 0.08,  // cx, cy, cz
+                                                   0.15, 0.15, 0.14,      // wx, wy, wz
+                                                   -0.222, -0.164, 0.07,  // cx, cy, cz
                                                    operation));
     collision_objects.push_back(createCollisionBox(item.name, "table_left", item.frame_id,  //
                                                    1.6, 0.1, 1.0,     // wx, wy, wz
@@ -933,6 +933,11 @@ tijmath::RelativePose3 PickAndPlaceRobotMovements::getCurrentRobotPose() const
   return robot_specific_interface_->getCurrentRobotPose();
 }
 
+tijmath::RelativePose3 PickAndPlaceRobotMovements::getCurrentEndEffectorPose() const
+{
+  return robot_specific_interface_->getCurrentEndEffectorPose();
+}
+
 void PickAndPlaceRobotMovements::debounceRobotMovement() const
 {
   const double debouncing_threshold_ = 1e-4;