Skip to content

Commit 55601ae

Browse files
committed
Fix linear approx in pinocchio ee kinematics
This commit fixes issue leggedrobotics#35 - CentroidalModelPinocchioMappingTpl::getOcs2Jacobian is called with Jv a 0×Nv matrix. This causes a runtime error in line 164 of pinocchio mapping. - Subsequently PinocchioEndEffectorKinematics::getPositionLinearApproximation is broken when calling getOcs2Jacobian. - By changing Jv to a zero matrix of size 3×Nv, the runtime error is fixed.
1 parent d7694c6 commit 55601ae

File tree

1 file changed

+1
-1
lines changed

1 file changed

+1
-1
lines changed

ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ std::vector<VectorFunctionLinearApproximation> PinocchioEndEffectorKinematics::g
134134

135135
VectorFunctionLinearApproximation pos;
136136
pos.f = data.oMf[frameId].translation();
137-
std::tie(pos.dfdx, std::ignore) = mappingPtr_->getOcs2Jacobian(state, J.topRows<3>(), matrix_t::Zero(0, model.nv));
137+
std::tie(pos.dfdx, std::ignore) = mappingPtr_->getOcs2Jacobian(state, J.topRows<3>(), matrix_t::Zero(3, model.nv));
138138
positions.emplace_back(std::move(pos));
139139
}
140140
return positions;

0 commit comments

Comments
 (0)