From f9f4e7981983b8d6ac5f76f8435c093dc5b4c69f Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit.edu>
Date: Thu, 14 Jul 2022 16:39:23 +0200
Subject: [PATCH] fix: global_T_robot sensor
---
.../Devices/GlobalRobotPoseSensorDevice.cpp | 4 ++++
.../Devices/GlobalRobotPoseSensorDevice.h | 11 +++++------
...HolonomicPlatformGlobalPositionController.cpp | 8 ++++++--
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 16 +++++++++++++---
4 files changed, 28 insertions(+), 11 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
index f248ce502..57f7cf102 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -1,4 +1,5 @@
#include "GlobalRobotPoseSensorDevice.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
@@ -138,6 +139,7 @@ namespace armarx
if (sensorGlobalPositionCorrection == nullptr or sensorRelativePosition == nullptr)
{
+ ARMARX_ERROR << "one of the sensors is not available";
return;
}
@@ -150,5 +152,7 @@ namespace armarx
0,
sensorRelativePosition->relativePositionRotation);
const auto global_T_robot = global_T_relative * relative_T_robot;
+
+ sensor.global_T_root = global_T_robot;
}
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
index d60e8c887..c3cd19f4a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
@@ -29,6 +29,8 @@ namespace armarx
using Transformation = Eigen::Matrix4f;
Transformation global_T_odom = Transformation::Identity();
+ // TODO add timestamp
+
static SensorValueInfo<SensorValueGlobalPoseCorrection> GetClassMemberInfo();
bool isAvailable() const;
@@ -93,8 +95,7 @@ namespace armarx
void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
-
- private:
+ protected:
SensorValueType sensor;
};
@@ -110,11 +111,9 @@ namespace armarx
void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration) override;
- SensorValueGlobalPoseCorrection* sensorGlobalPositionCorrection;
- SensorValueHolonomicPlatformRelativePosition* sensorRelativePosition;
+ const SensorValueGlobalPoseCorrection* sensorGlobalPositionCorrection;
+ const SensorValueHolonomicPlatformRelativePosition* sensorRelativePosition;
- private:
- SensorValueType sensor;
};
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 013af7163..d847d403f 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -22,9 +22,13 @@
* GNU General Public License
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <cmath>
@@ -42,8 +46,8 @@ namespace armarx
opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{
- const SensorValueBase* sv = useSensorValue(cfg->platformName);
- this->sv = sv->asA<SensorValueHolonomicPlatform>();
+ const SensorValueBase* sv = useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
+ this->sv = sv->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 226274e21..80efce08a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -156,6 +156,11 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "initializing default units";
initializeKinematicUnit();
ARMARX_DEBUG << "KinematicUnit initialized";
+
+ ARMARX_DEBUG << "initializing LocalizationUnit";
+ initializeLocalizationUnit();
+ ARMARX_DEBUG << "LocalizationUnit initialized";
+
initializePlatformUnit();
ARMARX_DEBUG << "PlatformUnit initialized";
initializeForceTorqueUnit();
@@ -166,9 +171,7 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
- ARMARX_DEBUG << "initializing LocalizationUnit";
- initializeLocalizationUnit();
- ARMARX_DEBUG << "LocalizationUnit initialized";
+
}
ARMARX_INFO << "initializing default units...done! " << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
}
@@ -460,6 +463,13 @@ namespace armarx::RobotUnitModule
unit->globalPositionCorrectionSensorDevice = dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
_module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName()).get());
+
+ const SensorDevicePtr& sensorDeviceGlobalRobotLocalization = _module<Devices>().getSensorDevices().at(GlobalRobotLocalizationSensorDevice::DeviceName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
+
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>();
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorGlobalPositionCorrection = unit->globalPositionCorrectionSensorDevice->getSensorValue()->asA<SensorValueGlobalPoseCorrection>();
+
//add
addUnit(unit);
}
--
GitLab