Merge pull request #1095 from proyan/devel

Multiple code-gen related fixes.

examples/codegen/codegen-crba.cpp

@@ -47,10 +47,13 @@ int main(int argc, const char ** argv)
   data_check.M.triangularView<Eigen::StrictlyLower>() = data_check.M.transpose().triangularView<Eigen::StrictlyLower>();
 
   const MatrixXd & M_check = data_check.M;
-  if(M_check.isApprox(M))
+  if(M_check.isApprox(M)) {
     std::cout << "Super! The two results are the same." << std::endl;
-  else
+    return 0;
+  }
+  else {
     std::cout << "Not Super! The results do not match." << std::endl;
+    return -1;
+  }
 
-  return -1;
 }

src/algorithm/energy.hxx

@@ -40,6 +40,7 @@ namespace pinocchio
     assert(model.check(data) && "data is not consistent with model.");;
     
     typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
+    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
     typedef typename Model::JointIndex JointIndex;
     typedef typename Model::Motion Motion;
 

src/autodiff/cppad.hpp

@@ -28,7 +28,14 @@ namespace boost
       namespace detail
       {
         template<typename Scalar>
-        struct constant_pi< CppAD::AD<Scalar> > : constant_pi<Scalar> {};
+        struct constant_pi< CppAD::AD<Scalar> > : constant_pi<Scalar> {
+          template <int N>
+          static inline CppAD::AD<Scalar> get(const mpl::int_<N>& n)
+          {
+            return CppAD::AD<Scalar>(constant_pi<Scalar>::get(n));
+          }
+
+        };
       }
     }
   }

src/codegen/cppadcg.hpp

@@ -25,7 +25,13 @@ namespace boost
       namespace detail
       {
         template<typename Scalar>
-        struct constant_pi< CppAD::cg::CG<Scalar> > : constant_pi<Scalar> {};
+        struct constant_pi< CppAD::cg::CG<Scalar> > : constant_pi<Scalar> {
+          template <int N>
+          static inline CppAD::cg::CG<Scalar> get(const mpl::int_<N>& n)
+          {
+            return CppAD::cg::CG<Scalar>(constant_pi<Scalar>::get(n));
+          }
+        };
       }
     }
   }
@@ -50,6 +56,15 @@ namespace Eigen
   }
 } // namespace Eigen
 
+namespace CppAD
+{
+  template <class Scalar>
+  bool isfinite(const cg::CG<Scalar> &x) { return std::isfinite(x.getValue()); } 
+  
+  template <class Scalar>
+  bool isfinite(const AD<Scalar>  &x) { return isfinite(Value(x)); }
+}
+
 namespace pinocchio
 {
   template<typename Scalar>

src/spatial/explog.hpp

@@ -215,7 +215,7 @@ namespace pinocchio
       Jout.diagonal().array() += Scalar(0.5) * (2 - theta*theta / Scalar(6));
       
       // Jlog += r_{\times}/2
-      addSkew(0.5 * log, Jlog);
+      addSkew(Scalar(0.5) * log, Jlog);
     }
     else
     {
@@ -229,7 +229,7 @@ namespace pinocchio
       Jout.diagonal().array() += Scalar(0.5) * (theta*st_1mct);
 
       // Jlog += r_{\times}/2
-      addSkew(0.5 * log, Jlog);
+      addSkew(Scalar(0.5) * log, Jlog);
     }
   }
 
@@ -373,7 +373,7 @@ namespace pinocchio
       beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
     }
     
-    return Motion(alpha * p - 0.5 * w.cross(p) + beta * w.dot(p) * w,
+    return Motion(alpha * p - Scalar(0.5) * w.cross(p) + beta * w.dot(p) * w,
                   w);
   }
 
@@ -523,7 +523,7 @@ namespace pinocchio
     C.noalias() = v3_tmp * w.transpose();
     C.noalias() += beta * w * p.transpose();
     C.diagonal().array() += wTp * beta;
-    addSkew(.5*p,C);
+    addSkew(Scalar(.5)*p,C);
     
     B.noalias() = C * A;
     C.setZero();