Commit 64e6efa0 authored by andreadelprete's avatar andreadelprete
Browse files

[robot-wrapper] Fix bug in mass matrix: copy upper triangular part to lower...

[robot-wrapper] Fix bug in mass matrix: copy upper triangular part to lower triangular part (before this it was set to zero)
parent c2bbbda5
......@@ -68,6 +68,8 @@ namespace tsid
void RobotWrapper::computeAllTerms(Data & data, const Vector & q, const Vector & v) const
{
se3::computeAllTerms(m_model, data, q, v);
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
// computeAllTerms does not compute the com acceleration, so we need to call centerOfMass
se3::centerOfMass<true,true,true>(m_model, data, false);
se3::framesForwardKinematics(m_model, data);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment