diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp index a1ef547b4201722f367705aa2890058bf52ce255..2f7c56acedb1168d07bdd4e88feb63bb519120eb 100644 --- a/src/multibody/model.hpp +++ b/src/multibody/model.hpp @@ -83,6 +83,9 @@ namespace se3 /// \brief Name of joint <i> std::vector<std::string> names; + /// \brief Vector of joint's neutral configurations + Eigen::VectorXd neutralConfigurations; + /// \brief Vector of maximal joint torques Eigen::VectorXd effortLimit; /// \brief Vector of maximal joint velocities diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx index 93510925818dc384d524fa56c18f097287c2951b..9ed02774fac50fff7874d3f3102d41db4df74909 100644 --- a/src/multibody/model.hxx +++ b/src/multibody/model.hxx @@ -93,6 +93,7 @@ namespace se3 nq += j.nq(); nv += j.nv(); + neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration(); effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity()); velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity()); lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>().fill(-std::numeric_limits<double>::infinity()); @@ -125,6 +126,7 @@ namespace se3 nq += j.nq(); nv += j.nv(); + neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration(); effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>() = effort; velocityLimit.conservativeResize(nv);velocityLimit.bottomRows<D::NV>() = velocity; lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows<D::NQ>() = lowPos; diff --git a/unittest/joint-configurations.cpp b/unittest/joint-configurations.cpp index a782be0bf4943c49579e05ba0750bab7f3a85710..8c3542ca037b781546ce6858c242b2f788d4330e 100644 --- a/unittest/joint-configurations.cpp +++ b/unittest/joint-configurations.cpp @@ -385,6 +385,46 @@ BOOST_AUTO_TEST_CASE ( distance_computation_test ) BOOST_CHECK_MESSAGE(result.isApprox(expected, 1e-12), "Distance between two configs of full model - wrong results"); } +BOOST_AUTO_TEST_CASE ( neutral_configuration_test ) +{ + se3::Model model; + + using namespace se3; + + model.addJointAndBody(model.getJointId("universe"),JointModelFreeFlyer(),SE3::Identity(),Inertia::Random(), + "freeflyer_joint", "freeflyer_body"); + model.addJointAndBody(model.getJointId("freeflyer_joint"),JointModelSpherical(),SE3::Identity(),Inertia::Random(), + "spherical_joint", "spherical_body"); + model.addJointAndBody(model.getJointId("spherical_joint"),JointModelRX(),SE3::Identity(),Inertia::Random(), + "revolute_joint", "revolute_body"); + model.addJointAndBody(model.getJointId("revolute_joint"),JointModelPX(),SE3::Identity(),Inertia::Random(), + "px_joint", "px_body"); + model.addJointAndBody(model.getJointId("px_joint"),JointModelPrismaticUnaligned(Eigen::Vector3d(1,0,0)),SE3::Identity(),Inertia::Random(), + "pu_joint", "pu_body"); + model.addJointAndBody(model.getJointId("pu_joint"),JointModelRevoluteUnaligned(Eigen::Vector3d(0,0,1)),SE3::Identity(),Inertia::Random(), + "ru_joint", "ru_body"); + model.addJointAndBody(model.getJointId("ru_joint"),JointModelSphericalZYX(),SE3::Identity(),Inertia::Random(), + "sphericalZYX_joint", "sphericalZYX_body"); + model.addJointAndBody(model.getJointId("sphericalZYX_joint"),JointModelTranslation(),SE3::Identity(),Inertia::Random(), + "translation_joint", "translation_body"); + model.addJointAndBody(model.getJointId("translation_joint"),JointModelPlanar(),SE3::Identity(),Inertia::Random(), + "planar_joint", "planar_body"); + + + Eigen::VectorXd expected(model.nq); + expected << 0,0,0,0,0,0,1, + 0,0,0,1, + 0, + 0, + 0, + 0, + 0,0,0, + 0,0,0, + 0,0,0; + + + BOOST_CHECK_MESSAGE(model.neutralConfigurations.isApprox(expected, 1e-12), "neutral configurations - wrong results"); +} BOOST_AUTO_TEST_CASE ( uniform_sampling_test ) {