From 24394d3abd562fe426470692ed01bf5e37f05a7c Mon Sep 17 00:00:00 2001 From: Gerardo Puga Date: Thu, 2 Jun 2022 23:24:45 -0300 Subject: [PATCH] Calibrate src pose from perception for parts in the gripper --- .../tijchallenger/src/SceneConfigReader.cpp | 12 +- .../tijcore/behavior/behavior.xml | 3 +- .../PickAndPlaceRobotMovementsInterface.hpp | 2 + ...ctorTransformFromCamerasIfPossibleNode.hpp | 114 ++++++++++++++++++ ...atePayloadIntoEndEffectorTransformNode.hpp | 19 ++- ...teRegulatorPreInsertAndInsertPosesNode.hpp | 2 - .../btnodes/RandomizeTargetPoseNode.hpp | 4 +- .../tijcore/tasking/BTTaskParameters.hpp | 3 + .../tijcore/src/BTRobotTaskFactory.cpp | 3 + .../tijcore/src/OrderProcessingStrategy.cpp | 3 + .../mocks/PickAndPlaceRobotMovementsMock.hpp | 2 + .../tijros/PickAndPlaceRobotMovements.hpp | 2 + .../tijros/src/PickAndPlaceRobotMovements.cpp | 11 +- 13 files changed, 163 insertions(+), 17 deletions(-) create mode 100644 project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode.hpp diff --git a/project/the_italian_job/tijchallenger/src/SceneConfigReader.cpp b/project/the_italian_job/tijchallenger/src/SceneConfigReader.cpp index 28a4f88..424cbaf 100644 --- a/project/the_italian_job/tijchallenger/src/SceneConfigReader.cpp +++ b/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 diff --git a/project/the_italian_job/tijcore/behavior/behavior.xml b/project/the_italian_job/tijcore/behavior/behavior.xml index d9dd373..78128d7 100644 --- a/project/the_italian_job/tijcore/behavior/behavior.xml +++ b/project/the_italian_job/tijcore/behavior/behavior.xml @@ -334,6 +334,7 @@ + @@ -733,7 +734,7 @@ - + diff --git a/project/the_italian_job/tijcore/include/tijcore/abstractions/PickAndPlaceRobotMovementsInterface.hpp b/project/the_italian_job/tijcore/include/tijcore/abstractions/PickAndPlaceRobotMovementsInterface.hpp index 285e181..c2fb29c 100644 --- a/project/the_italian_job/tijcore/include/tijcore/abstractions/PickAndPlaceRobotMovementsInterface.hpp +++ b/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 diff --git a/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode.hpp b/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode.hpp new file mode 100644 index 0000000..5d1156d --- /dev/null +++ b/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossibleNode.hpp @@ -0,0 +1,114 @@ +/* Copyright [2022] + * Distributed under the MIT License (http://opensource.org/licenses/MIT) + * Author: Gerardo Puga */ + +#pragma once + +// standard library +#include +#include + +// external +#include "behaviortree_cpp_v3/action_node.h" + +// tijcore +#include +#include + +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("task_parameters"), + }; + } + + BT::NodeStatus tick() override + { + auto task_parameters = getInput("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(); + }; + + 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 diff --git a/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformNode.hpp b/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformNode.hpp index 9662328..192db7a 100644 --- a/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformNode.hpp +++ b/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculatePayloadIntoEndEffectorTransformNode.hpp @@ -39,9 +39,22 @@ class CalculatePayloadIntoEndEffectorTransformNode : public BT::SyncActionNode auto end_effector_pose = getInput("end_effector_pose").value(); auto payload_pose = getInput("payload_pose").value(); auto task_parameters = getInput("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; } diff --git a/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculateRegulatorPreInsertAndInsertPosesNode.hpp b/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculateRegulatorPreInsertAndInsertPosesNode.hpp index 72adce5..f27c429 100644 --- a/project/the_italian_job/tijcore/include/tijcore/btnodes/CalculateRegulatorPreInsertAndInsertPosesNode.hpp +++ b/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; diff --git a/project/the_italian_job/tijcore/include/tijcore/btnodes/RandomizeTargetPoseNode.hpp b/project/the_italian_job/tijcore/include/tijcore/btnodes/RandomizeTargetPoseNode.hpp index 705cd6b..eb8d894 100644 --- a/project/the_italian_job/tijcore/include/tijcore/btnodes/RandomizeTargetPoseNode.hpp +++ b/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 unif_dist(-4, 3); + std::uniform_int_distribution unif_dist(-2, 1); const auto distance = max_radius; - const auto angle = static_cast(unif_dist(gen)) * 2 * M_PI / 8.0; + const auto angle = static_cast(unif_dist(gen)) * 2 * M_PI / 4.0; const auto x_diff = distance * std::cos(angle); const auto y_diff = distance * std::sin(angle); diff --git a/project/the_italian_job/tijcore/include/tijcore/tasking/BTTaskParameters.hpp b/project/the_italian_job/tijcore/include/tijcore/tasking/BTTaskParameters.hpp index 0a499af..3c0df4c 100644 --- a/project/the_italian_job/tijcore/include/tijcore/tasking/BTTaskParameters.hpp +++ b/project/the_italian_job/tijcore/include/tijcore/tasking/BTTaskParameters.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace tijcore { @@ -39,6 +40,8 @@ struct BTTaskParameters // Temporary state std::optional spatial_lock_handle; std::optional aux_locus; + + std::optional ee_to_payload_iso; }; } // namespace tijcore diff --git a/project/the_italian_job/tijcore/src/BTRobotTaskFactory.cpp b/project/the_italian_job/tijcore/src/BTRobotTaskFactory.cpp index 1f9091f..1dcd16a 100644 --- a/project/the_italian_job/tijcore/src/BTRobotTaskFactory.cpp +++ b/project/the_italian_job/tijcore/src/BTRobotTaskFactory.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -154,6 +155,8 @@ void factoryLoaderMethod(BT::BehaviorTreeFactory& factory) factory.registerNodeType("HackyDestinationPoseUpdate"); factory.registerNodeType( "CalibrateSourcePartFromPerceptionIfPossible"); + factory.registerNodeType( + "CalculatePayloadIntoEndEffectorTransformFromCamerasIfPossible"); }; } // namespace diff --git a/project/the_italian_job/tijcore/src/OrderProcessingStrategy.cpp b/project/the_italian_job/tijcore/src/OrderProcessingStrategy.cpp index f5e379b..9555ea9 100644 --- a/project/the_italian_job/tijcore/src/OrderProcessingStrategy.cpp +++ b/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()) diff --git a/project/the_italian_job/tijcore/tests/mocks/PickAndPlaceRobotMovementsMock.hpp b/project/the_italian_job/tijcore/tests/mocks/PickAndPlaceRobotMovementsMock.hpp index b2b4f25..10d9069 100644 --- a/project/the_italian_job/tijcore/tests/mocks/PickAndPlaceRobotMovementsMock.hpp +++ b/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 diff --git a/project/the_italian_job/tijros/include/tijros/PickAndPlaceRobotMovements.hpp b/project/the_italian_job/tijros/include/tijros/PickAndPlaceRobotMovements.hpp index 09e5c10..9c0cc2c 100644 --- a/project/the_italian_job/tijros/include/tijros/PickAndPlaceRobotMovements.hpp +++ b/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_; diff --git a/project/the_italian_job/tijros/src/PickAndPlaceRobotMovements.cpp b/project/the_italian_job/tijros/src/PickAndPlaceRobotMovements.cpp index 3b96910..d48e06b 100644 --- a/project/the_italian_job/tijros/src/PickAndPlaceRobotMovements.cpp +++ b/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;