algo/aba-derivatives: correct dAdq and remove gravity contribution

src/algorithm/aba-derivatives.hxx

@@ -77,7 +77,6 @@ namespace se3
       typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
       ColsBlock J_cols = jmodel.jointCols(data.J);
       J_cols = data.oMi[i].act(jdata.S());
-
     }
     
   };
@@ -320,6 +319,15 @@ namespace se3
         data.doYcrb[parent] += data.doYcrb[i];
         data.of[parent] += data.of[i];
       }
+      
+      // Restore the status of dAdq_cols (remove gravity)
+      assert(model.gravity.angular().isZero() && "The gravity must be a pure force vector, no angular part");
+      for(Eigen::DenseIndex k =0; k < jmodel.nv(); ++k)
+      {
+        MotionRef<typename ColsBlock::ColXpr> min(J_cols.col(k));
+        MotionRef<typename ColsBlock::ColXpr> mout(dAdq_cols.col(k));
+        mout.linear() += model.gravity.linear().cross(min.angular());
+      }
     }
     
     template<typename Min, typename Mout>