Skip to content
Snippets Groups Projects
Commit ee4b14b5 authored by Frederik Koch's avatar Frederik Koch
Browse files

fixed hemisphere errors

parent 7632ad87
1 merge request!408Current control
Pipeline #19076 failed
......@@ -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)
......
......@@ -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)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment