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 ...@@ -59,7 +59,7 @@ namespace se3
#ifdef WITH_HPP_FCL #ifdef WITH_HPP_FCL
.def("CreateCapsule", &GeometryObjectPythonVisitor::maker_capsule) .def("CreateCapsule", &GeometryObjectPythonVisitor::maker_capsule)
.staticmethod("CreateCapsule") .staticmethod("CreateCapsule")
#endif WITH_HPP_FCL #endif // WITH_HPP_FCL
; ;
} }
...@@ -71,7 +71,7 @@ namespace se3 ...@@ -71,7 +71,7 @@ namespace se3
SE3::Identity()); SE3::Identity());
} }
#endif WITH_HPP_FCL #endif // WITH_HPP_FCL
static void expose() static void expose()
{ {
......
...@@ -257,14 +257,14 @@ namespace se3 ...@@ -257,14 +257,14 @@ namespace se3
operator*(const typename InertiaTpl<_Scalar,_Options>::Matrix6 & Y, operator*(const typename InertiaTpl<_Scalar,_Options>::Matrix6 & Y,
const typename JointSphericalZYXTpl<_Scalar,_Options>::ConstraintRotationalSubspace & S) 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> inline Eigen::Matrix<double,6,3>
operator*(const Inertia::Matrix6 & Y, operator*(const Inertia::Matrix6 & Y,
const JointSphericalZYX::ConstraintRotationalSubspace & S) 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 namespace internal
......
...@@ -199,6 +199,12 @@ namespace se3 ...@@ -199,6 +199,12 @@ namespace se3
return M; 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 namespace internal
{ {
template<> template<>
......
...@@ -27,6 +27,7 @@ ...@@ -27,6 +27,7 @@
#include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp" #include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp" #include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/tools/timer.hpp" #include "pinocchio/tools/timer.hpp"
...@@ -35,6 +36,39 @@ ...@@ -35,6 +36,39 @@
#include <boost/test/unit_test.hpp> #include <boost/test/unit_test.hpp>
#include <boost/utility/binary.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_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( test_crba ) BOOST_AUTO_TEST_CASE ( test_crba )
...@@ -133,10 +167,13 @@ BOOST_AUTO_TEST_CASE (test_ccrb) ...@@ -133,10 +167,13 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
using namespace se3; using namespace se3;
Model model; Model model;
buildModels::humanoidSimple(model); buildModels::humanoidSimple(model);
addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7");
Data data(model), data_ref(model); Data data(model), data_ref(model);
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq); model.lowerPositionLimit.head<7>().fill(-1.);
q.segment <4> (3).normalize(); 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 v = Eigen::VectorXd::Random(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
...@@ -158,9 +195,10 @@ BOOST_AUTO_TEST_CASE (test_ccrb) ...@@ -158,9 +195,10 @@ BOOST_AUTO_TEST_CASE (test_ccrb)
Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>()))); Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));
ccrba(model,data_ref,q,v); 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(Ag_ref));
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
dccrba(model,data,q,0*v);
BOOST_CHECK(data.dAg.isZero()); BOOST_CHECK(data.dAg.isZero());
......
...@@ -86,6 +86,14 @@ void test_joint_methods (JointModel & jmodel, typename JointModel::JointDataDeri ...@@ -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 ")); 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