Commit 6dc09bc4 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[Minor] Remove terminal s in Model::NeutralConfigurations.

parent d7199bb8
...@@ -84,7 +84,7 @@ namespace se3 ...@@ -84,7 +84,7 @@ namespace se3
std::vector<std::string> names; std::vector<std::string> names;
/// \brief Vector of joint's neutral configurations /// \brief Vector of joint's neutral configurations
Eigen::VectorXd neutralConfigurations; Eigen::VectorXd neutralConfiguration;
/// \brief Vector of maximal joint torques /// \brief Vector of maximal joint torques
Eigen::VectorXd effortLimit; Eigen::VectorXd effortLimit;
......
...@@ -93,11 +93,21 @@ namespace se3 ...@@ -93,11 +93,21 @@ namespace se3
nq += j.nq(); nq += j.nq();
nv += j.nv(); nv += j.nv();
neutralConfigurations.conservativeResize(nq);neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration(); neutralConfiguration.conservativeResize(nq);
effortLimit.conservativeResize(nv);effortLimit.bottomRows<D::NV>().fill(std::numeric_limits<double>::infinity()); neutralConfiguration.bottomRows<D::NQ>() = j.neutralConfiguration();
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()); effortLimit.conservativeResize(nv);
upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows<D::NQ>().fill(std::numeric_limits<double>::infinity()); 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());
upperPositionLimit.conservativeResize(nq);
upperPositionLimit.bottomRows<D::NQ>().fill(std::numeric_limits<double>::infinity());
return idx; return idx;
} }
...@@ -135,15 +145,15 @@ namespace se3 ...@@ -135,15 +145,15 @@ namespace se3
// Ensure that limits are not inf // Ensure that limits are not inf
Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 ); Eigen::VectorXd neutralConf((lowerPositionLimit.bottomRows<D::NQ>() + upperPositionLimit.bottomRows<D::NQ>())/2 );
neutralConfigurations.conservativeResize(nq); neutralConfiguration.conservativeResize(nq);
if ( std::isfinite(neutralConf.norm()) ) if ( std::isfinite(neutralConf.norm()) )
{ {
neutralConfigurations.bottomRows<D::NQ>() = neutralConf; neutralConfiguration.bottomRows<D::NQ>() = neutralConf;
} }
else else
{ {
assert( false && "One of the position limit is inf or NaN"); assert( false && "One of the position limit is inf or NaN");
neutralConfigurations.bottomRows<D::NQ>() = j.neutralConfiguration(); neutralConfiguration.bottomRows<D::NQ>() = j.neutralConfiguration();
} }
addFrame((jointName!="")?jointName:random(8), idx, SE3::Identity(), JOINT); addFrame((jointName!="")?jointName:random(8), idx, SE3::Identity(), JOINT);
......
...@@ -174,8 +174,8 @@ namespace se3 ...@@ -174,8 +174,8 @@ namespace se3
if (joint_id != model.joints.size()) // != model.njoints if (joint_id != model.joints.size()) // != model.njoints
{ {
model.neutralConfigurations(joint.idx_q()) = joint_config; // joint with 1 dof model.neutralConfiguration(joint.idx_q()) = joint_config; // joint with 1 dof
// model.neutralConfigurations.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof // model.neutralConfiguration.segment(joint.idx_q(),joint.nq()) = joint_config; // joint with more than 1 dof
} }
else else
{ {
......
...@@ -125,7 +125,7 @@ namespace se3 ...@@ -125,7 +125,7 @@ namespace se3
.add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort") .add_property("effortLimit", bp::make_function(&ModelPythonVisitor::effortLimit), "Joint max effort")
.add_property("neutralConfigurations", bp::make_function(&ModelPythonVisitor::neutralConfigurations), "Joint's neutral configurations") .add_property("neutralConfiguration", bp::make_function(&ModelPythonVisitor::neutralConfiguration), "Joint's neutral configurations")
.add_property("velocityLimit", bp::make_function(&ModelPythonVisitor::velocityLimit), "Joint max velocity") .add_property("velocityLimit", bp::make_function(&ModelPythonVisitor::velocityLimit), "Joint max velocity")
.add_property("lowerPositionLimit", bp::make_function(&ModelPythonVisitor::lowerPositionLimit), "Limit for joint lower position") .add_property("lowerPositionLimit", bp::make_function(&ModelPythonVisitor::lowerPositionLimit), "Limit for joint lower position")
.add_property("upperPositionLimit", bp::make_function(&ModelPythonVisitor::upperPositionLimit), "Limit for joint upper position") .add_property("upperPositionLimit", bp::make_function(&ModelPythonVisitor::upperPositionLimit), "Limit for joint upper position")
...@@ -172,7 +172,7 @@ namespace se3 ...@@ -172,7 +172,7 @@ namespace se3
} }
static Eigen::VectorXd neutralConfigurations(ModelHandler & m) {return m->neutralConfigurations;} static Eigen::VectorXd neutralConfiguration(ModelHandler & m) {return m->neutralConfiguration;}
static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;} static Eigen::VectorXd effortLimit(ModelHandler & m) {return m->effortLimit;}
static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;} static Eigen::VectorXd velocityLimit(ModelHandler & m) {return m->velocityLimit;}
static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;} static Eigen::VectorXd lowerPositionLimit(ModelHandler & m) {return m->lowerPositionLimit;}
......
...@@ -483,7 +483,7 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test ) ...@@ -483,7 +483,7 @@ BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
0,0,0; 0,0,0;
BOOST_CHECK_MESSAGE(model.neutralConfigurations.isApprox(expected, 1e-12), "neutral configurations - wrong results"); BOOST_CHECK_MESSAGE(model.neutralConfiguration.isApprox(expected, 1e-12), "neutral configuration - wrong results");
} }
BOOST_AUTO_TEST_CASE ( uniform_sampling_test ) BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
......
Supports Markdown
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