Unverified Commit 70c4b172 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1095 from proyan/devel

Multiple code-gen related fixes.
parents a5ce89eb bff64596
......@@ -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;
}
......@@ -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;
......
......@@ -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));
}
};
}
}
}
......
......@@ -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>
......
......@@ -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();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment