Merged in bugfix/end_effector_kinematics_cppad_segmentation (pull req…

…uest #692) Fix segment indices bug in PinocchioEndEffectorKinematicsCppAd Approved-by: Farbod Farshidian
leggedrobotics · Jan 9, 2023 · 8b64496 · 8b64496
2 parents ebde452 + 122529e
commit 8b64496
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp
@@ -253,7 +253,7 @@ auto PinocchioEndEffectorKinematicsCppAd::getOrientationError(const vector_t& st
     -> std::vector<vector3_t> {
   vector_t params(4 * endEffectorIds_.size());
   for (int i = 0; i < endEffectorIds_.size(); i++) {
-    params.segment<4>(i) = referenceOrientations[i].coeffs();
+    params.segment<4>(4 * i) = referenceOrientations[i].coeffs();
   }
 
   const vector_t errorValues = orientationErrorCppAdInterfacePtr_->getFunctionValue(state, params);
@@ -272,7 +272,7 @@ std::vector<VectorFunctionLinearApproximation> PinocchioEndEffectorKinematicsCpp
     const vector_t& state, const std::vector<quaternion_t>& referenceOrientations) const {
   vector_t params(4 * endEffectorIds_.size());
   for (int i = 0; i < endEffectorIds_.size(); i++) {
-    params.segment<4>(i) = referenceOrientations[i].coeffs();
+    params.segment<4>(4 * i) = referenceOrientations[i].coeffs();
   }
 
   const vector_t errorValues = orientationErrorCppAdInterfacePtr_->getFunctionValue(state, params);