Merge pull request #478 from ami-iit/fix/TSID_acc

Fix the population of the jointAccelerations and baseAcceleration variables in QPTSID
ami-iit · Dec 3, 2021 · 4c1608c · 4c1608c
2 parents 6b62c91 + 2676259
commit 4c1608c
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,7 @@ All notable changes to this project are documented in this file.
 - Use yarp clock instead of system clock in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/473)
 
 ### Fix
+- Fix the population of the jointAccelerations and baseAcceleration variables in QPTSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/478)
 
 ## [0.5.0] - 2021-11-30
 ### Added

diff --git a/src/TSID/src/QPTSID.cpp b/src/TSID/src/QPTSID.cpp
@@ -506,8 +506,14 @@ bool QPTSID::advance()
  constexpr std::size_t spatialAccelerationSize = 6;
  const std::size_t joints = m_pimpl->robotAccelerationVariable.size - spatialAccelerationSize;
 
+ // the first six elements are the base acceleration
+ m_pimpl->solution.baseAcceleration
+ = m_pimpl->solver.getSolution().segment<spatialAccelerationSize>(
+ m_pimpl->robotAccelerationVariable.offset);
+
  m_pimpl->solution.jointAccelerations
- = m_pimpl->solver.getSolution().segment(m_pimpl->robotAccelerationVariable.offset,
+ = m_pimpl->solver.getSolution().segment(m_pimpl->robotAccelerationVariable.offset
+ + spatialAccelerationSize,
  joints);
 
  m_pimpl->solution.jointTorques