Commit d03e6adc authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #409 from jcarpent/topic/devel

Fix bug related to joint Spherical
parents db2f658d d377018f
......@@ -59,7 +59,7 @@ namespace se3
#ifdef WITH_HPP_FCL
.def("CreateCapsule", &GeometryObjectPythonVisitor::maker_capsule)
.staticmethod("CreateCapsule")
#endif WITH_HPP_FCL
#endif // WITH_HPP_FCL
;
}
......@@ -71,7 +71,7 @@ namespace se3
SE3::Identity());
}
#endif WITH_HPP_FCL
#endif // WITH_HPP_FCL
static void expose()
{
......
......@@ -257,14 +257,14 @@ namespace se3
operator*(const typename InertiaTpl<_Scalar,_Options>::Matrix6 & Y,
const typename JointSphericalZYXTpl<_Scalar,_Options>::ConstraintRotationalSubspace & S)
{
return Y.template block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal;
return Y.template middleCols<3>(Inertia::ANGULAR) * S.S_minimal;
}
inline Eigen::Matrix<double,6,3>
operator*(const Inertia::Matrix6 & Y,
const JointSphericalZYX::ConstraintRotationalSubspace & S)
{
return Y.block<6,3> (0,Inertia::ANGULAR,0,3) * S.S_minimal;
return Y.middleCols<3>(Inertia::ANGULAR) * S.S_minimal;
}
namespace internal
......
......@@ -199,6 +199,12 @@ namespace se3
return M;
}
/* [ABA] Y*S operator*/
inline SizeDepType<3>::ColsReturn<Inertia::Matrix6>::ConstType operator* (const Inertia::Matrix6 & Y, const ConstraintRotationalSubspace &)
{
return Y.middleCols<3>(Inertia::ANGULAR);
}
namespace internal
{
template<>
......
......@@ -27,6 +27,7 @@
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp"
......@@ -35,6 +36,39 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
template<typename JointModel>
static void addJointAndBody(se3::Model & model,
const se3::JointModelBase<JointModel> & joint,
const std::string & parent_name,
const std::string & name,
const se3::SE3 placement = se3::SE3::Random(),
bool setRandomLimits = true)
{
using namespace se3;
typedef typename JointModel::ConfigVector_t CV;
typedef typename JointModel::TangentVector_t TV;
Model::JointIndex idx;
if (setRandomLimits)
idx = model.addJoint(model.getJointId(parent_name),joint,
SE3::Random(),
name + "_joint",
TV::Random() + TV::Constant(1),
TV::Random() + TV::Constant(1),
CV::Random() - CV::Constant(1),
CV::Random() + CV::Constant(1)
);
else
idx = model.addJoint(model.getJointId(parent_name),joint,
placement, name + "_joint");
model.addJointFrame(idx);
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
model.addBodyFrame(name + "_body", idx);
}
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( test_crba )
......@@ -133,10 +167,13 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
using namespace se3;
Model model;
buildModels::humanoidSimple(model);
addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7");
Data data(model), data_ref(model);
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
q.segment <4> (3).normalize();
model.lowerPositionLimit.head<7>().fill(-1.);
model.upperPositionLimit.head<7>().fill(1.);
Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
......@@ -158,9 +195,10 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));
ccrba(model,data_ref,q,v);
dccrba(model,data,q,0*v);
dccrba(model,data,q,v);
BOOST_CHECK(data.Ag.isApprox(Ag_ref));
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
dccrba(model,data,q,0*v);
BOOST_CHECK(data.dAg.isZero());
......
......@@ -86,6 +86,14 @@ void test_joint_methods (JointModel & jmodel, typename JointModel::JointDataDeri
BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));
// Test Y*S
const Inertia Isparse(Inertia::Random());
const Inertia::Matrix6 Idense(Isparse.matrix());
const ConstraintDense IsparseS = Isparse * jdata.S;
const ConstraintDense IdenseS = Idense * jdata.S;
BOOST_CHECK_MESSAGE(IdenseS.isApprox(IsparseS),std::string(error_prefix + "- Joint YS operation "));
}
......
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