From ee4b14b548a6a1d22492b7a057b501f7210b83d0 Mon Sep 17 00:00:00 2001
From: Frederik Koch <ulxvc@student.kit.edu>
Date: Thu, 2 May 2024 17:40:57 +0200
Subject: [PATCH] fixed hemisphere errors
---
.../units/RobotUnit/util/DynamicsHelper.cpp | 24 ++++++++++++++++---
.../libraries/armem_mps/CMakeLists.txt | 2 +-
2 files changed, 22 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
index 79d076f9b..cec8ac628 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp
@@ -78,7 +78,9 @@ namespace armarx
const IceUtil::Time& timeSinceLastIteration)
{
for (auto& pair : dataMap)
- {
+ {
+ // std::cerr << pair.first->getName() << std::endl;
+
// first get current state of the robot (position, velocity, acceleration)
auto& data = pair.second;
auto& dynamics = data.dynamics;
@@ -86,7 +88,11 @@ namespace armarx
size_t i = 0;
for (auto& node : data.nodeSensorVec)
{
- ARMARX_CHECK_EXPRESSION(node.second) << node.first->getName();
+
+ if (not node.second) {
+ continue;
+ }
+ // ARMARX_CHECK_EXPRESSION(node.second) << node.first->getName();
data.q(i) = node.second->position;
if (i < data.velocityFilter.size() && data.velocityFilter.at(i) &&
!std::isnan(node.second->velocity))
@@ -111,9 +117,21 @@ namespace armarx
i++;
}
}
- // calculate external torques (tau) applied to robot induced by dynamics
+
+
+
+ // std::cerr << "getGravityMatrix" << std::endl;
+ // // calculate external torques (tau) applied to robot induced by dynamics
+ // std::cerr << data.q.rows() << std::endl;
+ // std::cerr << dynamics.getModel()->dof_count << std::endl;
dynamics.getGravityMatrix(data.q, data.tauGravity);
+
+
+
+ // FIXME handle constraints
dynamics.getInverseDynamics(data.q, data.qDot, data.qDDot, data.tau);
+
+
// update virtual sensor values
size_t i = 0;
for (auto& node : data.nodeSensorVec)
diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
index e51a85347..a67fba39d 100644
--- a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
@@ -50,5 +50,5 @@ armarx_add_library(
add_library(RobotAPI::armem_mps ALIAS armem_mps)
-target_link_libraries(armem_mps LINK_PUBLIC rbdl)
+# target_link_libraries(armem_mps LINK_PUBLIC rbdl)
add_subdirectory(server)
--
GitLab