Verified Commit a17ce4e5 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

joints: fix calc with static constructor

parent 8e5c4da2
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -239,19 +239,41 @@ namespace pinocchio
M.translation(q_joint.template head<3>());
}
template<typename Vector3Derived, typename QuaternionDerived>
EIGEN_DONT_INLINE
void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<Vector3Derived> & trans,
const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
{
data.M.translation(trans);
data.M.rotation(quat.matrix());
}
template<typename ConfigVector>
EIGEN_DONT_INLINE
void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs) const
const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
{
typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
data.M.translation(q.template head<3>());
ConstQuaternionMap quat(q.template tail<4>().data());
data.M.rotation(quat.matrix());
calc(data,q.template head<3>(),quat);
}
template<typename ConfigVector>
EIGEN_DONT_INLINE
void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs) const
{
typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
const Quaternion quat(q.template tail<4>());
calc(data,q.template head<3>(),quat);
}
template<typename ConfigVector, typename TangentVector>
......
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -425,18 +425,35 @@ namespace pinocchio
M.rotation(quat.matrix());
M.translation().setZero();
}
template<typename QuaternionDerived>
void calc(JointDataDerived & data,
const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
{
data.M.rotation(quat.matrix());
}
template<typename ConfigVector>
EIGEN_DONT_INLINE
void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs) const
const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
{
typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type & q = qs.template segment<NQ>(idx_q());
ConstQuaternionMap quat(q.data());
data.M.rotation(quat.matrix());
ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
calc(data,quat);
}
template<typename ConfigVector>
EIGEN_DONT_INLINE
void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs) const
{
typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
const Quaternion quat(qs.template segment<NQ>(idx_q()));
calc(data,quat);
}
template<typename ConfigVector, typename TangentVector>
......
//
// Copyright (c) 2018 CNRS
// Copyright (c) 2018-2019 CNRS INRIA
//
#include "pinocchio/multibody/model.hpp"
......@@ -16,6 +16,23 @@
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
{
using namespace Eigen;
using namespace pinocchio;
Model model;
buildModels::humanoidRandom(model);
Data data(model);
model.lowerPositionLimit.head<3>().fill(-1.);
model.upperPositionLimit.head<3>().fill(1.);
VectorXd q = randomConfiguration(model);
forwardKinematics(model,data,Model::ConfigVectorType::Ones(model.nq));
}
BOOST_AUTO_TEST_CASE(test_kinematics_zero)
{
using namespace Eigen;
......
Supports Markdown
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