Skip to content
Snippets Groups Projects
Commit 82a9b16d authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[Joint Composite] re-implemented calc

parent 4244aecc
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 1
− 0
View file @ 82a9b16d
......@@ -203,6 +203,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx
multibody/joint/joint-composite.hpp
multibody/joint/joint-composite.hxx
)
SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
......
src/multibody/joint/joint-composite.hpp
+ 27
− 89
View file @ 82a9b16d
......@@ -80,7 +80,7 @@ namespace se3
// JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
JointDataComposite(const JointDataVector & joint_data, const int /*nq*/, const int nv)
: joints(joint_data), iMlast(joint_data.size())
: joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
, S(nv)
, M(), v(), c()
, U(6,nv), Dinv(nv,nv), UDinv(6,nv)
......@@ -89,9 +89,12 @@ namespace se3
/// \brief Vector of joints
JointDataVector joints;
/// \brief Transform from the joint i to the last joint
/// \brief Transforms from previous joint to last joint
container::aligned_vector<Transformation_t> iMlast;
// boost::array<Transformation_t,_njoints> liMi;
/// \brief Transforms from previous joint to joint i
container::aligned_vector<Transformation_t> pjMi;
Constraint_t S;
Transformation_t M;
Motion_t v;
......@@ -137,7 +140,6 @@ namespace se3
, jointPlacements()
, m_nq(0)
, m_nv(0)
, max_nv(0)
{}
///
......@@ -154,7 +156,6 @@ namespace se3
, m_nv(jmodel.nv())
, m_idx_q(1), m_nqs(1,jmodel.nq())
, m_idx_v(1), m_nvs(1,jmodel.nv())
, max_nv(jmodel.nv())
{}
///
......@@ -170,7 +171,6 @@ namespace se3
, m_nv(other.m_nv)
, m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
, m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
, max_nv(other.max_nv)
{}
......@@ -187,7 +187,6 @@ namespace se3
jointPlacements.push_back(placement);
m_nq += jmodel.nq(); m_nv += jmodel.nv();
max_nv = std::max(max_nv,jmodel.nv());
updateJointIndexes();
}
......@@ -200,89 +199,27 @@ namespace se3
return JointDataDerived(jdata,nq(),nv());
}
void EIGEN_DONT_INLINE
calc(JointData & data, const Eigen::VectorXd & qs) const
friend void jointCompositeCalcZeroOrder(const JointModelComposite &, JointDataComposite &, const Eigen::VectorXd &);
friend struct JointCompositeCalcZeroOrderStep;
void calc(JointData & data, const Eigen::VectorXd & qs) const
{
assert(joints.size() > 0);
assert(data.joints.size() == joints.size());
Transformation_t M_tmp;
Constraint_t::DenseBase S_tmp(6,max_nv);
for (int k = (int)(joints.size()-1); k >= 0; --k)
{
const JointModelVariant & jmodel = joints[(size_t)k];
const JointDataVariant & jdata = data.joints[(size_t)k];
calc_zero_order(jmodel,data.joints[(size_t)k],qs);
const int idx_v = m_idx_v[(size_t)k] - m_idx_v[0];
if(k == (int)(joints.size()-1))
{
data.iMlast[(size_t)k].setIdentity();
data.S.matrix().middleCols(idx_v,m_nvs[(size_t)k]) = constraint_xd(jdata).matrix();
}
else
{
M_tmp = jointPlacements[(size_t)k+1] * joint_transform(data.joints[(size_t)k+1]);
data.iMlast[(size_t)k] = M_tmp * data.iMlast[(size_t)k+1];
S_tmp.leftCols(m_nvs[(size_t)k]) = constraint_xd(jdata).matrix();
motionSet::se3Action(data.iMlast[(size_t)k].inverse(),
S_tmp.leftCols(m_nvs[(size_t)k]),
data.S.matrix().middleCols(idx_v,m_nvs[(size_t)k]));
}
}
M_tmp = jointPlacements[0] * joint_transform(data.joints[0]);
data.M = M_tmp * data.iMlast[0];
jointCompositeCalcZeroOrder(*this, data, qs);
}
void EIGEN_DONT_INLINE
calc(JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs) const
friend void jointCompositeCalcFirstOrder(const JointModelComposite &, JointDataComposite &, const Eigen::VectorXd &, const Eigen::VectorXd &);
friend struct JointCompositeCalcFirstOrderStep;
void calc(JointData & data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs) const
{
Transformation_t M_tmp;
Motion v_tmp;
Motion bias_tmp;
Constraint_t::DenseBase S_tmp(6,max_nv);
for (int k = (int)(joints.size()-1); k >= 0; --k)
{
const JointDataVariant & jdata = data.joints[(size_t)k];
calc_first_order(joints[(size_t)k],data.joints[(size_t)k],qs,vs);
const int idx_v = m_idx_v[(size_t)k] - m_idx_v[0];
if(k == (int)(joints.size()-1))
{
data.iMlast[(size_t)k].setIdentity();
data.v = motion(jdata);
data.c = bias(jdata);
data.S.matrix().middleCols(idx_v,m_nvs[(size_t)k]) = constraint_xd(jdata).matrix();
}
else
{
M_tmp = jointPlacements[(size_t)k+1] * joint_transform(data.joints[(size_t)k+1]);
data.iMlast[(size_t)k] = M_tmp * data.iMlast[(size_t)k+1];
v_tmp = data.iMlast[(size_t)k].actInv(motion(jdata));
data.v += v_tmp;
data.c -= data.v.cross(v_tmp);
bias_tmp = bias(jdata);
data.c += data.iMlast[(size_t)k].actInv(bias_tmp);
S_tmp.leftCols(m_nvs[(size_t)k]) = constraint_xd(jdata).matrix();
motionSet::se3Action(data.iMlast[(size_t)k].inverse(),
S_tmp.leftCols(m_nvs[(size_t)k]),
data.S.matrix().middleCols(idx_v,m_nvs[(size_t)k]));
}
}
M_tmp = jointPlacements[0] * joint_transform(data.joints[0]);
data.M = M_tmp * data.iMlast[0];
}
assert(joints.size() > 0);
assert(data.joints.size() == joints.size());
jointCompositeCalcFirstOrder(*this, data, qs, vs);
}
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
{
......@@ -387,14 +324,15 @@ namespace se3
}
ConfigVector_t neutralConfiguration_impl() const
{
{
ConfigVector_t result(nq());
int idx_q=0;
for (size_t i = 0; i < joints.size(); ++i)
{
const JointModelVariant & jmodel = joints[i];
const int idx_q = m_idx_q[i];
const int nq = m_nqs[i];
result.segment(idx_q,nq) = ::se3::neutralConfiguration(jmodel);
idx_q += nq;
}
return result;
}
......@@ -524,10 +462,6 @@ namespace se3
std::vector<int> m_idx_v;
/// \brief Dimension of the segment in the tangent vector
std::vector<int> m_nvs;
/// \brief Max nv dimensions for all joints contained in joints.
int max_nv;
};
......@@ -543,5 +477,9 @@ namespace se3
} // namespace se3
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/joint/joint-composite.hxx"
#endif // ifndef __se3_joint_composite_hpp__
src/multibody/joint/joint-composite.hxx 0 → 100644
+ 141
− 0
View file @ 82a9b16d
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terljMj of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_composite_hxx__
#define __se3_joint_composite_hxx__
#include "pinocchio/multibody/visitor.hpp"
namespace se3
{
struct JointCompositeCalcZeroOrderStep : public fusion::JointVisitor<JointCompositeCalcZeroOrderStep>
{
typedef boost::fusion::vector<const se3::JointModelComposite &,
se3::JointDataComposite &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT (JointCompositeCalcZeroOrderStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::JointModelComposite & model,
se3::JointDataComposite & data,
const Eigen::VectorXd & q)
{
const JointIndex & i = jmodel.id();
const JointIndex succ = i+1; // successor
jmodel.calc(jdata.derived(), q);
data.pjMi[i] = model.jointPlacements[i] * jdata.M ();
if ( succ == model.joints.size() )
{
data.iMlast[i] = data.pjMi[i];
data.S.matrix().rightCols(model.m_nvs[i]) = constraint_xd(jdata.derived()).matrix();
}
else
{
const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];
data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].inverse().act(jdata.S()); // TODO: avoid computing inverse
}
}
};
inline void jointCompositeCalcZeroOrder(const JointModelComposite & model, JointDataComposite & data, const Eigen::VectorXd & qs)
{
for (int i=(int)(model.joints.size()-1); i >= 0; --i)
{
JointCompositeCalcZeroOrderStep::run(model.joints[(size_t)i], data.joints[(size_t)i],
JointCompositeCalcZeroOrderStep::ArgsType (model,data,qs)
);
}
data.M = data.iMlast.front();
}
struct JointCompositeCalcFirstOrderStep : public fusion::JointVisitor<JointCompositeCalcFirstOrderStep>
{
typedef boost::fusion::vector<const se3::JointModelComposite &,
se3::JointDataComposite &,
const Eigen::VectorXd &,
const Eigen::VectorXd &
> ArgsType;
JOINT_VISITOR_INIT (JointCompositeCalcFirstOrderStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
se3::JointDataBase<typename JointModel::JointDataDerived> & jdata,
const se3::JointModelComposite & model,
se3::JointDataComposite & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v)
{
const JointIndex & i = jmodel.id();
const JointIndex succ = i+1; // successor
jmodel.calc(jdata.derived(), q, v);
data.pjMi[i] = model.jointPlacements[i] * jdata.M ();
if ( succ == model.joints.size() )
{
data.iMlast[i] = data.pjMi[i];
data.S.matrix().rightCols(model.m_nvs[i]) = constraint_xd(jdata.derived()).matrix();
data.v = jdata.v();
data.c = jdata.c();
}
else
{
const int idx_v = model.m_idx_v[i] - model.m_idx_v[0];
data.iMlast[i] = data.pjMi[i] * data.iMlast[succ];
data.S.matrix().middleCols(idx_v,model.m_nvs[i]) = data.iMlast[succ].inverse().act(jdata.S()); // TODO: avoid computing inverse
Motion v_tmp = data.iMlast[succ].actInv(motion(jdata.derived())); // TODO: avoid calling motion
data.v += v_tmp;
data.c -= data.v.cross(v_tmp);
data.c += data.iMlast[succ].actInv(bias(jdata.derived())); // TODO: avoid calling bias
}
}
};
inline void jointCompositeCalcFirstOrder(const JointModelComposite & model, JointDataComposite & data, const Eigen::VectorXd & qs, const Eigen::VectorXd & vs)
{
for (int i=(int)(model.joints.size()-1); i >= 0; --i)
{
JointCompositeCalcFirstOrderStep::run(model.joints[(size_t)i], data.joints[(size_t)i],
JointCompositeCalcFirstOrderStep::ArgsType (model,data,qs,vs)
);
}
data.M = data.iMlast.front();
}
} // namespace se3
#endif // ifndef __se3_joint_composite_hxx__
unittest/joint-composite.cpp
+ 94
− 30
View file @ 82a9b16d
......@@ -17,6 +17,7 @@
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
......@@ -38,26 +39,26 @@ template<typename JointModel>
void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelComposite & jmodel_composite)
{
typedef typename JointModelBase<JointModel>::JointDataDerived JointData;
JointData jdata = jmodel.createData();
typedef typename JointModel::ConfigVector_t ConfigVector_t;
typedef typename JointModel::TangentVector_t TangentVector_t;
JointData jdata = jmodel.createData();
JointDataComposite jdata_composite = jmodel_composite.createData();
jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v());
typedef typename JointModel::ConfigVector_t ConfigVector_t;
typedef typename JointModel::TangentVector_t TangentVector_t;
BOOST_CHECK(jmodel.nv() == jmodel_composite.nv());
BOOST_CHECK(jmodel.nq() == jmodel_composite.nq());
ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));
ConfigVector_t q = jmodel.randomConfiguration(ql,qu);
ConfigVector_t q = jmodel.randomConfiguration(ql,qu);
jmodel.calc(jdata,q);
BOOST_CHECK(jmodel.nv() == jmodel_composite.nv());
BOOST_CHECK(jmodel.nq() == jmodel_composite.nq());
jmodel_composite.calc(jdata_composite,q);
BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
BOOST_CHECK(constraint_xd(jdata_composite).matrix().isApprox(constraint_xd(jdata).matrix()));
q = jmodel.randomConfiguration(ql,qu);
TangentVector_t v = TangentVector_t::Random(jmodel.nv());
......@@ -65,6 +66,7 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
jmodel_composite.calc(jdata_composite,q,v);
BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
BOOST_CHECK(constraint_xd(jdata_composite).matrix().isApprox(constraint_xd(jdata).matrix()));
BOOST_CHECK(jdata_composite.v.isApprox((Motion)jdata.v));
BOOST_CHECK(jdata_composite.c.isApprox((Motion)jdata.c));
......@@ -76,7 +78,11 @@ void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelCom
VectorXd v(VectorXd::Random(jmodel.nv()));
BOOST_CHECK(jmodel_composite.integrate(q1,v).isApprox(jmodel.integrate(q1,v)));
BOOST_CHECK(jmodel_composite.difference(q1,q2).isApprox(jmodel.difference(q1,q2)));
TangentVector_t v1 = jmodel_composite.difference(q1,q2);
TangentVector_t v2 = jmodel.difference(q1,q2);
BOOST_CHECK(v1.isApprox(v2));
const double alpha = 0.2;
BOOST_CHECK(jmodel_composite.interpolate(q1,q2,alpha).isApprox(jmodel.interpolate(q1,q2,alpha)));
......@@ -146,29 +152,28 @@ BOOST_AUTO_TEST_CASE(test_basic)
boost::mpl::for_each<JointModelVariant::types>(TestJointComposite());
}
BOOST_AUTO_TEST_CASE(test_equivalent)
BOOST_AUTO_TEST_CASE(vsZYX)
{
{
JointModelSphericalZYX jmodel_spherical;
jmodel_spherical.setIndexes(0,0,0);
JointModelComposite jmodel_composite((JointModelRZ()));
jmodel_composite.addJoint(JointModelRY());
jmodel_composite.addJoint(JointModelRX());
test_joint_methods(jmodel_spherical, jmodel_composite);
}
JointModelSphericalZYX jmodel_spherical;
jmodel_spherical.setIndexes(0,0,0);
{
JointModelTranslation jmodel_translation;
jmodel_translation.setIndexes(0,0,0);
JointModelComposite jmodel_composite((JointModelPX()));
jmodel_composite.addJoint(JointModelPY());
jmodel_composite.addJoint(JointModelPZ());
test_joint_methods(jmodel_translation, jmodel_composite);
}
JointModelComposite jmodel_composite((JointModelRZ()));
jmodel_composite.addJoint(JointModelRY());
jmodel_composite.addJoint(JointModelRX());
test_joint_methods(jmodel_spherical, jmodel_composite);
}
BOOST_AUTO_TEST_CASE(vsTranslation)
{
JointModelTranslation jmodel_translation;
jmodel_translation.setIndexes(0,0,0);
JointModelComposite jmodel_composite((JointModelPX()));
jmodel_composite.addJoint(JointModelPY());
jmodel_composite.addJoint(JointModelPZ());
test_joint_methods(jmodel_translation, jmodel_composite);
}
BOOST_AUTO_TEST_CASE (test_recursive_variant)
......@@ -207,5 +212,64 @@ BOOST_AUTO_TEST_CASE(test_copy)
}
BOOST_AUTO_TEST_CASE(test_kinematics)
{
Model model;
JointModelComposite jmodel_composite;
SE3 config=SE3::Random();
//JointModelVariant joint_model;
JointIndex parent=0;
for(int i=0; i<10; i++)
{
//joint_model = JointModelPX();
//JointModelPX joint_model();
parent = model.addJoint(parent, JointModelRX(), config, "joint");
jmodel_composite.addJoint(JointModelRX(),config);
config.setRandom();
}
Data data(model);
Model model_c;
model_c.addJoint(0,jmodel_composite,SE3::Identity(),"joint");
Data data_c(model_c);
BOOST_CHECK(model.nv == model_c.nv);
BOOST_CHECK(model.nq == model_c.nq);
// VectorXd ql(ConfigVector_t::Constant(model.nq,-M_PI));
// VectorXd qu(ConfigVector_t::Constant(model.nq,M_PI));
//
// ConfigVector_t q = jmodel.randomConfiguration(ql,qu);
VectorXd q(VectorXd::Random(model.nv));
forwardKinematics(model,data,q);
forwardKinematics(model_c,data_c,q);
BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
q.setRandom(model.nq);
VectorXd v(VectorXd::Random(model.nv));
forwardKinematics(model,data,q,v);
forwardKinematics(model_c,data_c,q,v);
BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
BOOST_CHECK(data.v.back().isApprox(data_c.v.back()));
q.setRandom(model.nq);
v.setRandom(model.nv);
VectorXd a(VectorXd::Random(model.nv));
forwardKinematics(model,data,q,v,a);
forwardKinematics(model_c,data_c,q,v,a);
BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
BOOST_CHECK(data.v.back().isApprox(data_c.v.back()));
BOOST_CHECK(data.a.back().isApprox(data_c.a.back()));
}
BOOST_AUTO_TEST_SUITE_END ()
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