fixed 1d force derivatives

loco-3d · Oct 25, 2023 · 75240e2 · 75240e2
1 parent b0d40e7
commit 75240e2
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/bindings/python/crocoddyl/utils/__init__.py b/bindings/python/crocoddyl/utils/__init__.py
@@ -1183,8 +1183,8 @@ def createData(self, data):
     def updateForce(self, data, force):
         assert force.shape[0] == 1
         nv = self.state.nv
-        data.f.linear[0] = force[0]
-        data.f.linear[1:] = np.zeros(2)
+        data.f.linear[2] = force[0]
+        data.f.linear[:2] = np.zeros(2)
         data.f.angular = np.zeros(3)
         if self.type == pinocchio.LOCAL:
             data.fext.linear = data.jMf.rotation[:,2] * force[0]

diff --git a/include/crocoddyl/multibody/contacts/contact-1d.hxx b/include/crocoddyl/multibody/contacts/contact-1d.hxx
@@ -182,8 +182,8 @@ void ContactModel1DTpl<Scalar>::updateForce(
   }
   Data* d = static_cast<Data*>(data.get());
   const Eigen::Ref<const Matrix3s> R = d->jMf.rotation();
-  data->f.linear()[0] = force[0];
-  data->f.linear().template tail<2>().setZero();
+  data->f.linear()[2] = force[0];
+  data->f.linear().template head<2>().setZero();
   data->f.angular().setZero();
   switch (type_) {
     case pinocchio::ReferenceFrame::LOCAL: