fixed hemisphere errors

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)

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)