From 3c3695984e6a15d2309f5341ddda02ed524a0176 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Wed, 1 Mar 2023 18:37:03 +0100
Subject: [PATCH] tof stuff
---
.../tof_based_grasping/Component.cpp | 73 ++++++++++++-------
.../components/tof_based_grasping/Component.h | 2 +-
source/armarx/manipulation/core/Robot.cpp | 32 ++++----
source/armarx/manipulation/core/Robot.h | 2 +-
4 files changed, 63 insertions(+), 46 deletions(-)
diff --git a/source/armarx/manipulation/components/tof_based_grasping/Component.cpp b/source/armarx/manipulation/components/tof_based_grasping/Component.cpp
index a6c80cb2..f292e46c 100644
--- a/source/armarx/manipulation/components/tof_based_grasping/Component.cpp
+++ b/source/armarx/manipulation/components/tof_based_grasping/Component.cpp
@@ -23,7 +23,8 @@
#include "Component.h"
-#include <Eigen/src/Core/util/Meta.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/MathTools.h>
// Include headers you only need in function definitions in the .cpp.
@@ -38,9 +39,9 @@
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include "Eigen/src/Core/util/Constants.h"
-#include "armarx/manipulation/core/arm.h"
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "armarx/manipulation/core/arm.h"
namespace armarx::manipulation::components::tof_based_grasping
{
@@ -54,8 +55,9 @@ namespace armarx::manipulation::components::tof_based_grasping
while (not task->isStopped())
{
- const std::optional<armem::robot::ToFArray> tofLeft =
- robot->getToFInHand(core::Hand::Left);
+ ARMARX_CHECK(robot->synchronize());
+
+ const auto tofLeft = robot->getToFInHand(core::Hand::Left);
const auto tofRight = robot->getToFInHand(core::Hand::Right);
ARMARX_CHECK(tofLeft.has_value());
@@ -64,17 +66,18 @@ namespace armarx::manipulation::components::tof_based_grasping
using MaskMatrixType = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
constexpr float minDistanceValid = 20; // FIXME magic number
+ constexpr float maxDistanceValid = 150; // FIXME magic number
constexpr float distanceGraspThreshold = 40; // FIXME magic number
- const MaskMatrixType tofLeftMask = tofLeft->array() > minDistanceValid;
- const MaskMatrixType tofRightMask = tofRight->array() > minDistanceValid;
+ const MaskMatrixType tofLeftMask = tofLeft->array.array() > minDistanceValid and tofLeft->array.array() < maxDistanceValid;
+ const MaskMatrixType tofRightMask = tofRight->array.array() > minDistanceValid and tofRight->array.array() < maxDistanceValid;
draw(core::Hand::Left, tofLeft.value(), tofLeftMask);
draw(core::Hand::Right, tofRight.value(), tofRightMask);
if (tofRightMask(3, 3)) // is valid
{
- const float distance = tofRight.value()(3, 3);
+ const float distance = tofRight.value().array(3, 3);
ARMARX_INFO << VAROUT(distance) << "mm";
if (distance < distanceGraspThreshold)
@@ -90,6 +93,15 @@ namespace armarx::manipulation::components::tof_based_grasping
value = 1;
}
+ // eventually, remove thumb circumduction
+ for(const auto& name: simox::alg::get_keys(jointValues))
+ {
+ if(simox::alg::ends_with(name, "ThumbCircumduction"))
+ {
+ jointValues.erase(name);
+ }
+ }
+
// closing hand
ARMARX_INFO << "Closing hand";
handUnit->setJointAngles(jointValues);
@@ -101,31 +113,49 @@ namespace armarx::manipulation::components::tof_based_grasping
}
void
- Component::draw(core::Hand hand,
- const armem::robot::ToFArray& tofArray,
- const MaskMatrixType& mask) const
+ Component::draw(core::Hand hand, const armem::robot::ToF& tof, const MaskMatrixType& mask)
{
auto layer = arviz.layer(core::HandNames.to_name(hand));
const float fov = VirtualRobot::MathTools::deg2rad(45);
- const float cx = tofArray.rows() / 2;
- const float cy = tofArray.cols() / 2;
+ const float cx = tof.array.rows() / 2.;
+ const float cy = tof.array.cols() / 2.;
- for(std::size_t px = 0; px < tofArray.rows(); px++)
+ for (std::size_t px = 0; px < static_cast<std::size_t>(tof.array.rows()); px++)
{
- for(std::size_t py = 0; py < tofArray.rows(); py++)
+ for (std::size_t py = 0; py < static_cast<std::size_t>(tof.array.rows()); py++)
{
- const float x = px - cx;
- const float y = py - cy;
+ if(not mask(px, py))
+ {
+ continue;
+ }
+
+ const float a = (px - cx) / cx * fov;
+ const float b = (py - cy) / cy * fov;
+
+ const auto Rx = Eigen::AngleAxisf(a, Eigen::Vector3f::UnitX());
+ const auto Ry = Eigen::AngleAxisf(b, Eigen::Vector3f::UnitY());
+
+ const float d = std::clamp<float>(tof.array(px, py), 0, 2000);
+ const Eigen::Vector3f posSensorFrame = Rx * Ry * Eigen::Vector3f::UnitZ() * d;
+ const auto sensor = robot->robot()->getSensor(tof.frame);
+ ARMARX_CHECK_NOT_NULL(sensor) << "No sensor with name `" << tof.frame << "`!";
+
+ const Eigen::Vector3f posGlobal =
+ Eigen::Isometry3f(sensor->getGlobalPose()) * posSensorFrame;
+
+ layer.add(viz::Sphere(std::to_string(px) + "/" + std::to_string(py))
+ .position(posGlobal)
+ .color(simox::Color::kit_orange()));
}
}
+ arviz.commit(layer);
}
-
armarx::PropertyDefinitionsPtr
Component::createPropertyDefinitions()
{
@@ -154,7 +184,6 @@ namespace armarx::manipulation::components::tof_based_grasping
return def;
}
-
void
Component::onInitComponent()
{
@@ -165,7 +194,6 @@ namespace armarx::manipulation::components::tof_based_grasping
// setDebugObserverBatchModeEnabled(true);
}
-
void
Component::onConnectComponent()
{
@@ -207,33 +235,28 @@ namespace armarx::manipulation::components::tof_based_grasping
*/
}
-
void
Component::onDisconnectComponent()
{
}
-
void
Component::onExitComponent()
{
}
-
std::string
Component::getDefaultName() const
{
return Component::defaultName;
}
-
std::string
Component::GetDefaultName()
{
return Component::defaultName;
}
-
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
Component::createRemoteGuiTab()
diff --git a/source/armarx/manipulation/components/tof_based_grasping/Component.h b/source/armarx/manipulation/components/tof_based_grasping/Component.h
index 7f06ef0c..36e02583 100644
--- a/source/armarx/manipulation/components/tof_based_grasping/Component.h
+++ b/source/armarx/manipulation/components/tof_based_grasping/Component.h
@@ -91,7 +91,7 @@ namespace armarx::manipulation::components::tof_based_grasping
private:
using MaskMatrixType = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
- void draw(core::Hand hand, const armem::robot::ToFArray& tofArray, const MaskMatrixType& mask) const;
+ void draw(core::Hand hand, const armem::robot::ToF& tof, const MaskMatrixType& mask);
core::Robot::Proxies robotProxies;
std::unique_ptr<core::Robot> robot;
diff --git a/source/armarx/manipulation/core/Robot.cpp b/source/armarx/manipulation/core/Robot.cpp
index ace89131..2bf1dc90 100644
--- a/source/armarx/manipulation/core/Robot.cpp
+++ b/source/armarx/manipulation/core/Robot.cpp
@@ -20,7 +20,9 @@
*/
#include "Robot.h"
+
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
#include "RobotAPI/libraries/armem_robot/types.h"
namespace armarx::manipulation::core
@@ -36,12 +38,13 @@ namespace armarx::manipulation::core
const auto timestamp = Clock::Now();
- if(robotReader.synchronizeRobot(*virtualRobot, timestamp))
+ if (robotReader.synchronizeRobot(*virtualRobot, timestamp))
{
syncTime = timestamp;
- } else
+ }
+ else
{
- ARMARX_INFO << "Failed to synchronize robot.";
+ ARMARX_INFO << "Failed to synchronize robot.";
}
nameHelper = armarx::RobotNameHelper::Create(virtualRobot->getFilename(), nullptr);
@@ -53,8 +56,6 @@ namespace armarx::manipulation::core
rightRobotArmHelper = rightArmHelper.addRobot(virtualRobot);
}
-
-
std::optional<Robot::ForceTorque>
Robot::getForceTorqueInHand(const Hand hand, const armarx::core::time::DateTime& timestamp)
{
@@ -90,7 +91,7 @@ namespace armarx::manipulation::core
};
}
- std::optional<armem::robot::ToFArray>
+ std::optional<armem::robot::ToF>
Robot::getToFInHand(const Hand hand, const armarx::core::time::DateTime& timestamp)
{
const armem::robot::RobotDescription desc{.name = robotName};
@@ -110,28 +111,26 @@ namespace armarx::manipulation::core
case Hand::Left:
ARMARX_CHECK(tof->count(armem::robot_state::RobotReader::Hand::Left) > 0);
tofArray = tof->at(armem::robot_state::RobotReader::Hand::Left);
- frame = "TimeOfFlightLeft";
+ frame = "ToF L Palm";
// frame = leftArmHelper.getTimeOfFlightSensorFrame();
break;
case Hand::Right:
ARMARX_CHECK(tof->count(armem::robot_state::RobotReader::Hand::Right) > 0);
tofArray = tof->at(armem::robot_state::RobotReader::Hand::Right);
- frame = "TimeOfFlightRight";
+ frame = "ToF R Palm";
// frame = rightArmHelper.getTimeOfFlightSensorFrame();
break;
}
- return tofArray;
+ return armem::robot::ToF{.array = tofArray, .frame = frame};
}
-
RobotUnitInterfacePrx
Robot::robotUnit() const
{
return services.proxies.robotUnitPrx;
}
-
HandUnitInterfacePrx
Robot::handUnit(Hand hand) const
{
@@ -150,28 +149,24 @@ namespace armarx::manipulation::core
return handUnit;
}
-
armem::client::MemoryNameSystem&
Robot::memoryNameSystem()
{
return services.memoryNameSystem;
}
-
const std::string&
Robot::name() const
{
return robotName;
}
-
const armarx::core::time::DateTime&
Robot::synchronizationTime() const
{
return syncTime;
}
-
bool
Robot::synchronize(const armarx::core::time::DateTime& timestamp)
{
@@ -184,14 +179,12 @@ namespace armarx::manipulation::core
return false;
}
-
const VirtualRobot::RobotPtr&
Robot::robot() const
{
return virtualRobot;
}
-
VirtualRobot::RobotNodeSetPtr
Robot::arm(const Arm arm) const
{
@@ -217,8 +210,9 @@ namespace armarx::manipulation::core
return *robotArm;
}
-
- const armarx::RobotNameHelper::Arm& Robot::armHelper(Arm arm) const
+
+ const armarx::RobotNameHelper::Arm&
+ Robot::armHelper(Arm arm) const
{
const armarx::RobotNameHelper::Arm* armHelper = nullptr;
diff --git a/source/armarx/manipulation/core/Robot.h b/source/armarx/manipulation/core/Robot.h
index 5d10f1eb..05f18262 100644
--- a/source/armarx/manipulation/core/Robot.h
+++ b/source/armarx/manipulation/core/Robot.h
@@ -83,7 +83,7 @@ namespace armarx::manipulation::core
std::optional<ForceTorque>
getForceTorqueInHand(Hand hand, const armarx::DateTime& timestamp = armarx::Clock::Now());
- std::optional<armem::robot::ToFArray>
+ std::optional<armem::robot::ToF>
getToFInHand(Hand hand, const armarx::DateTime& timestamp = armarx::Clock::Now());
--
GitLab