diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index e5762482e69b710fa361bf26dd5705849b4b0efb..35f54669285b085c2c14fb7929585f2bba6bc0c0 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -176,7 +176,7 @@ namespace armarx::articulated_object
const float t = static_cast<float>((now - start).toSecondsDouble());
// move joints at certain frequency
- const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
+ const float k = (1 + std::sin(t / (M_2_PI))) / 2; // in [0,1]
auto jointValues = articulatedObject->getJointValues();
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index 61497d62d399adf6f1c89a0a0de1c544a902ec51..5d077bca27a123793f5bd34023d9a6878dab9ae6 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -260,7 +260,7 @@ namespace armarx
std::shared_lock<std::shared_mutex> l{m_managed_obstacles_mutex};
const Eigen::Vector2f diff = goal - agentPosition;
- const Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2f32) * diff).normalized();
+ const Eigen::Vector2f orthogonal_normalized = Eigen::Vector2f(Eigen::Rotation2Df(M_PI_2) * diff).normalized();
const float sample_step = 5; // in [mm], sample step size towards goal.
const float distance_to_goal = diff.norm() + safetyRadius;
diff --git a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
index f1a6ebc2a168de7938e552b44f1d2144a915dd35..e94e1a7cc2aa544650681583ca6a1aae11f9bef6 100644
--- a/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/ObjectMemoryEditor/Editor.h
@@ -86,7 +86,7 @@ namespace armarx
struct Placeholder
{
simox::OrientedBoxf box;
- Eigen::Matrix4f transform;
+ Eigen::Matrix4f transform{Eigen::Matrix4f::Identity()};
};
void addPlaceholder(simox::OrientedBoxf box);
diff --git a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
index c9afa372dc20c08fc6e80ce2666214618d366286..c944eda90774140a09c4b2376bcfa734b000817f 100644
--- a/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
+++ b/source/RobotAPI/components/armem/addon/LegacyRobotStateMemoryAdapter/LegacyRobotStateMemoryAdapter.cpp
@@ -86,7 +86,7 @@ namespace armarx::armem
}
// is this corect??
- prop.platform.acceleration = Eigen::Vector3f();
+ prop.platform.acceleration = Eigen::Vector3f::Zero();
prop.platform.relativePosition = Eigen::Vector3f(update.platformPose.x, // this should be globasl AFAIK
update.platformPose.y,
update.platformPose.rotationAroundZ);
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
index a87ba755cb2744ece8508856d8d80e5ac8251744..678a03c3aaaa372f5f0fd9bdd28a14e29d9c9b2d 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
@@ -15,6 +15,7 @@ armarx_add_component(
VirtualRobotWriterExample.h
)
+
# Generate the application
armarx_generate_and_add_component_executable(
# If your component is not defined in ::armarx, specify its namespace here:
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
index d2a7eff51a3aaf9155ddb8915f61aca9a26c7535..675d2dae9e79c28d5a2fe2d8ce9630963d878dc0 100644
--- a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
@@ -133,7 +133,7 @@ namespace armarx::virtual_robot_writer_example
const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
- const float m = (1 + std::sin(t / (M_2_PIf32 * 10))) / 2; // in [0,1]
+ const float m = (1 + std::sin(t / (M_2_PI * 10))) / 2; // in [0,1]
auto jointValues = robot->getJointValues();
for (auto& [k, v] : jointValues)
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index f6710bc22978c2816aa8056da5795b0a84b5578a..25bdbe36948d1fecc27c6e1e7b963652e412756f 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -74,6 +74,7 @@ set(LIB_FILES
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+target_compile_options("${LIB_NAME}" PRIVATE -Wno-error=array-bounds)
add_subdirectory(ObstacleAvoidingPlatformUnit)
add_subdirectory(ObstacleAwarePlatformUnit)
diff --git a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 14f1348ce043e1fb9158d9d2eca4268b15c17d1c..3166408f4190460632625b1739066112fd1d2965 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -586,7 +586,7 @@ bool armarx::ObstacleAvoidingPlatformUnit::is_near_target(const Eigen::Vector2f&
const float sim = simox::math::cosine_similarity(target_direction, control_direction);
// if almost pointing into same direction
- if (sim > cos(M_PI_4f32))
+ if (sim > cos(M_PI_4))
{
return true;
}
diff --git a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
index fc0274a7eb932135018cf356ca39105ea70890c6..d18722607d92879c22184fd8afc06a5ed0fa9bb9 100644
--- a/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAwarePlatformUnit/ObstacleAwarePlatformUnit.cpp
@@ -564,7 +564,7 @@ const noexcept
const float sim = simox::math::cosine_similarity(target_direction, control_direction);
// if almost pointing into same direction
- if (sim > cos(M_PI_4f32))
+ if (sim > cos(M_PI_4))
{
return true;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 15baf198eb03202a929341675f01ef1ab831d234..67e940d30ad6899cc9773c14c950f8207092f905 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -269,7 +269,7 @@ namespace armarx
{
ft.force = _rtFTfake.force;
ft.torque = _rtFTfake.torque;
- ARMARX_RT_LOGF("Using fake ft values");
+ ARMARX_RT_LOGF_VERBOSE("Using fake ft values");
}
_rtFTHistory.at(_rtFTHistoryIndex) = ft;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 8ce9b397c9147d041290f5f8f73c203d6b8cb326..2956bbcf81a23ed99c16ac0ee8dc7ab5b697ace0 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -159,7 +159,7 @@ namespace armarx
getWriterControlStruct().target << x, y;
getWriterControlStruct().targetOrientation =
- simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
+ simox::math::periodic_clamp(static_cast<double>(yaw), -M_PI, M_PI);
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index 4e07bdb3e790f36875e2b080b1a1470b461eaa8b..f59f1b7bef8c52d860490b75f1dc8e2f9eb6462e 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -66,8 +66,8 @@ namespace armarx
struct NJointHolonomicPlatformGlobalPositionControllerTarget
{
- Eigen::Vector2f target; // x,y
- float targetOrientation;
+ Eigen::Vector2f target{0, 0}; // x,y
+ float targetOrientation = 0;
float translationAccuracy = 0.0f;
float rotationAccuracy = 0.0f;
bool newTargetSet = false;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index ad5b035266524a4079f84811ad8677279019f856..62760e241a7b2a4c7b4457fa6669545d040dff63 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -60,8 +60,8 @@ namespace armarx
struct NJointHolonomicPlatformRelativePositionControllerTarget
{
- Eigen::Vector2f target; // x,y
- float targetOrientation;
+ Eigen::Vector2f target{0, 0}; // x,y
+ float targetOrientation = 0;
float translationAccuracy = 0.0f;
float rotationAccuracy = 0.0f;
bool newTargetSet = false;
@@ -105,7 +105,7 @@ namespace armarx
MultiDimPIDController pid;
PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
ControlTargetHolonomicPlatformVelocity* target;
- Eigen::Vector2f startPosition, currentPosition;
+ Eigen::Vector2f startPosition{0, 0}, currentPosition{0, 0};
float startOrientation;
float currentOrientation;
// float rad2MMFactor;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index bd92c93f31a9007c338b9ae4bcd52d9ebe1c4d50..16af4f8c064391718d3aef43efac649030d41ed8 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -289,9 +289,10 @@ namespace armarx::RobotUnitModule
//try to find the correct mode (maybe the specified target was derived!
//this only works, if exactly one controller provides this mode
const JointController* selected_ctrl{nullptr};
- for (const auto ctrl : controlDevice->getJointControllers())
+ auto jointControllers = controlDevice->getJointControllers();
+ for (const auto* jointController : jointControllers)
{
- if (ctrl->getControlTarget()->isA<CtargT>())
+ if (jointController->getControlTarget()->isA<CtargT>())
{
if (selected_ctrl)
{
@@ -300,7 +301,7 @@ namespace armarx::RobotUnitModule
<< requestedControlMode << "! autoselection failed";
break;
}
- selected_ctrl = ctrl;
+ selected_ctrl = jointController;
}
}
if (selected_ctrl)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index e71cc9309d80f65ab74480c7837d1095022821f7..cb47d82b18e8c30d5b0f82c7923ae8f2328bf734 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -24,6 +24,7 @@
#include <vector>
+#include <ArmarXCore/core/logging/LoggingUtil.h> // THIS NEEDS TO BE INCLUDED BEFORE EXPRESSION EXCEPTION
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/PropagateConst.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
diff --git a/source/RobotAPI/components/units/SensorActorUnit.cpp b/source/RobotAPI/components/units/SensorActorUnit.cpp
index f93f684d72619a4ad402f8f56a5f6c285d00a866..45efd98c586e5bfa15086ddc08a80d65f7c3497a 100644
--- a/source/RobotAPI/components/units/SensorActorUnit.cpp
+++ b/source/RobotAPI/components/units/SensorActorUnit.cpp
@@ -160,6 +160,8 @@ void SensorActorUnit::release(const Ice::Current& c)
void SensorActorUnit::onExitComponent()
{
- unitMutex.try_lock(); // try to lock, to ensure that it is locked on unlock call
- unitMutex.unlock();
+ if (unitMutex.try_lock()) // try to lock, to ensure that it is locked on unlock call
+ {
+ unitMutex.unlock();
+ }
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
index a0ad9297f873496e7dbe0d1c17fb3ab0b5904ac1..57528416490797728886eb08a0ff7999bb4e4887 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/core/FinderBase.h
@@ -138,5 +138,11 @@ namespace armarx::priorknowledge::core
virtual std::optional<FinderInfoType> find(const DatasetType& dataset,
const IDType& id) const = 0;
virtual std::vector<FinderInfoType> findAll(const DatasetType& dataset) const = 0;
+
+ // Fix hidden virtual functions
+ using Base::checkAll;
+ using Base::check;
+ using Base::find;
+ using Base::findAll;
};
} // namespace armarx::priorknowledge::core
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt b/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
index 5e24563b59a6d4d470916e244a451a6878d07436..545faddd872d9ac16863b9471c6c876b5b705045 100644
--- a/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
+++ b/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
@@ -27,5 +27,7 @@ set(LIB_HEADERS
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+target_compile_options("${LIB_NAME}" PRIVATE -Wno-error=maybe-uninitialized)
+
# add unit tests
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/armem/util/util.h b/source/RobotAPI/libraries/armem/util/util.h
index 2f45b843a0419a639e000cc2594b59783df06267..050df3f880d715e44f2e07f37e06bf9b6708d760 100644
--- a/source/RobotAPI/libraries/armem/util/util.h
+++ b/source/RobotAPI/libraries/armem/util/util.h
@@ -88,18 +88,15 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- for (const auto &[ss, entitySnapshot] : entity.history())
+ entity.forEachInstance([&outV](const wm::EntityInstance& entityInstance)
{
- for (const auto& entityInstance : entitySnapshot.instances())
- {
- const auto o = tryCast<AronClass>(entityInstance);
+ const auto o = tryCast<AronClass>(entityInstance);
- if (o)
- {
- outV.push_back(*o);
- }
+ if (o)
+ {
+ outV.push_back(*o);
}
- }
+ });
}
return outV;
@@ -139,25 +136,15 @@ namespace armarx::armem
// loop over all entities and their snapshots
for (const auto &[s, entity] : entities)
{
- if (entity.history().empty())
+ entity.forEachInstance([&outV, &pred](const wm::EntityInstance& entityInstance)
{
- ARMARX_WARNING << "Empty history for " << s;
- }
-
- ARMARX_DEBUG << "History size: " << entity.history().size();
+ const auto o = tryCast<AronClass>(entityInstance);
- for (const auto &[ss, entitySnapshot] : entity.history())
- {
- for (const auto& entityInstance : entitySnapshot.instances())
+ if (o)
{
- const auto o = tryCast<AronClass>(entityInstance);
-
- if (o)
- {
- outV.push_back(pred(*o));
- }
+ outV.push_back(pred(*o));
}
- }
+ });
}
return outV;
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 9caf84f24574be547036e94fc0f07e50680d0fec..f497c3e2ddcbec47b26585202f3f97d0dedbf957 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -25,8 +25,8 @@ namespace armarx::armem::robot
struct Twist
{
- Eigen::Vector3f linear;
- Eigen::Vector3f angular;
+ Eigen::Vector3f linear{Eigen::Vector3f::Zero()};
+ Eigen::Vector3f angular{Eigen::Vector3f::Zero()};
};
struct PlatformState
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
index 29a009714506d069ebd05dcbc988acc64eafe1de..0bae7b6e9d12bf287307ec5f774bc95389ca216c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h
@@ -54,6 +54,8 @@ namespace armarx::armem::robot_state
[[nodiscard]] bool storeState(const VirtualRobot::Robot& robot,
const armem::Time& timestamp);
+ using RobotWriter::storeState;
+ using RobotWriter::storeDescription;
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
index 7d86ee5491dc5e0999e56a4089b3b468061e0909..ab76f86ff23c9b47c402bc7797c7a6d72b99a640 100644
--- a/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
+++ b/source/RobotAPI/libraries/armem_system_state/server/LightweightSystemMonitor/linux_process_load.hpp
@@ -9,11 +9,11 @@
#pragma once
-
-#include <tuple>
+#include <cstdint>
#include <map>
-#include <unordered_map>
#include <string>
+#include <tuple>
+#include <unordered_map>
class linuxProcessLoad {
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
index 6ace1783e7b6cedbd7dc8312bb3c38265450c216..404373a0a4b1c64778d597c15304ba8f30c62d49 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.cpp
@@ -69,19 +69,19 @@ namespace armarx::aron::codegenerator::cpp::generator
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
Matrix::getCtorInitializers(const std::string& name) const
{
- if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT)
+ if (type.getCols() == -1 || type.getRows() == -1)
{
return {{}, false};
}
- if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
{
return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
}
- if (type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT || type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
{
return {{{name, getInstantiatedCppTypename() + "::Zero()"}}, true};
}
- if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
{
return {{{name, getInstantiatedCppTypename() + "::One()"}}, true};
}
@@ -93,6 +93,36 @@ namespace armarx::aron::codegenerator::cpp::generator
}
}
+ CppBlockPtr
+ Matrix::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ if (type.getCols() == -1 || type.getRows() == -1)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "();");
+ }
+
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::IDENTITY)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Identity();");
+ }
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::DEFAULT || type.getDefaultValue() == aron::type::matrix::default_value::ZEROS)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Zero();");
+ }
+ else if (type.getDefaultValue() == aron::type::matrix::default_value::ONES)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::One();");
+ }
+ else if (not type.getDefaultValue().empty())
+ {
+ // try to parse num. We ensure from typereader that defaultValue is valid number
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::One() * " + type.getDefaultValue() + ";");
+ }
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
CppBlockPtr
Matrix::getResetSoftBlock(const std::string& cppAccessor) const
{
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
index 452ac7b53e5e9d996bc8cbc7ffa41555d7275e65..69790642abf5681ec78b9aa25a6fc3eac9163a0d 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Matrix.h
@@ -42,6 +42,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string> getRequiredIncludes() const final;
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
const std::string& cppAccessor,
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
index 365b3c8824d6c38b60ccde7c4381aa41bb0945b7..b4d7c92cca8521e945eb6e35d1b819c53b154ac6 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.cpp
@@ -62,13 +62,13 @@ namespace armarx::aron::codegenerator::cpp::generator
{
if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
{
- return {{}, false};
+ return {{{name, getInstantiatedCppTypename() + "::Identity()"}}, true};
}
- if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
{
return {{{name, getInstantiatedCppTypename() + "(0, 0, 0, 0)"}}, true};
}
- if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
{
return {{{name, getInstantiatedCppTypename() + "(1, 1, 1, 1)"}}, true};
}
@@ -80,6 +80,31 @@ namespace armarx::aron::codegenerator::cpp::generator
}
}
+ CppBlockPtr
+ Quaternion::getResetHardBlock(const std::string& cppAccessor) const
+ {
+ CppBlockPtr block_if_data = std::make_shared<CppBlock>();
+
+ if (type.getDefaultValue() == aron::type::quaternion::default_value::DEFAULT)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "::Identity();");
+ }
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ZEROS)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(0, 0, 0, 0);");
+ }
+ else if (type.getDefaultValue() == aron::type::quaternion::default_value::ONES)
+ {
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(1, 1, 1, 1);");
+ }
+ else if (not type.getDefaultValue().empty())
+ {
+ // try to parse num. We ensure from typereader that defaultValue is valid number
+ block_if_data->addLine(cppAccessor + " = " + getInstantiatedCppTypename() + "(" + type.getDefaultValue() + ");");
+ }
+ return resolveMaybeResetHardBlock(block_if_data, cppAccessor);
+ }
+
CppBlockPtr
Quaternion::getResetSoftBlock(const std::string& cppAccessor) const
{
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
index 4fd41b7dbd382c0231337ea30ba661f9b1e9eab3..70f8e890e8ed9f38ed75074fab8d7113dcec1d97 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/generator/ndarray/Quaternion.h
@@ -42,6 +42,7 @@ namespace armarx::aron::codegenerator::cpp::generator
std::vector<std::string> getRequiredIncludes() const final;
std::pair<std::vector<std::pair<std::string, std::string>>, bool>
getCtorInitializers(const std::string&) const final;
+ CppBlockPtr getResetHardBlock(const std::string& cppAccessor) const final;
CppBlockPtr getResetSoftBlock(const std::string& cppAccessor) const final;
CppBlockPtr getWriteTypeBlock(const std::string& typeAccessor,
const std::string& cppAccessor,
diff --git a/source/RobotAPI/libraries/ukfm/CMakeLists.txt b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
index 798d6767bec35847f7f86e3cd64a5db5083ed230..08ef9a9c8344ef4c5346293b910f4a26a9883c06 100644
--- a/source/RobotAPI/libraries/ukfm/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ukfm/CMakeLists.txt
@@ -34,6 +34,8 @@ armarx_add_library(
"${LIBS}"
)
+target_compile_options("${LIB_NAME}" PRIVATE -Wno-error=array-bounds)
+
if(Eigen3_FOUND)
target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${Eigen3_INCLUDE_DIR} ${Eigen3_INCLUDE_DIRS})
endif()
@@ -41,3 +43,4 @@ endif()
if(manif_FOUND)
target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
endif()
+
diff --git a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
index 6be767cbd80f870ff1ba98fef2546fc29215a997..e21c2c1785fb690f1b26d7e8a9c52e9aafc558ac 100644
--- a/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
+++ b/source/RobotAPI/statecharts/ForceTorqueUtility/DetectForceSpike.cpp
@@ -46,7 +46,7 @@ void DetectForceSpike::run()
DatafieldRefPtr forceDf = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getForceDatafield(in.getFTDatafieldName()));
const Eigen::Vector3f weights = in.getForceWeights()->toEigen();
const Eigen::Vector3f axis = in.getAxis()->toEigen().normalized();
- auto getForceAlongAxis = [&]
+ auto getForceAlongAxis = [&]() -> float
{
return forceDf->getDataField()->get<FramedDirection>()->toEigen().cwiseProduct(weights).transpose() * axis;
};