From bf37463188e01397bf997fe28211e173c699d996 Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Thu, 7 Jul 2016 19:24:44 +0200
Subject: [PATCH] [C++][unittest] Added neutralConfiguration vector in Model,
and fill it when adding joints + test on neutralConfiguration
---
src/multibody/model.hpp | 3 +++
src/multibody/model.hxx | 2 ++
unittest/joint-configurations.cpp | 40 +++++++++++++++++++++++++++++++
3 files changed, 45 insertions(+)
diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index a1ef547b4..2f7c56ace 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 935109258..9ed02774f 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 a782be0bf..8c3542ca0 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 )
{
--
GitLab