Verified Commit 6f7408e9 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

model: add nqs, idx_qs, nvs and idx_vs fields to ModelTpl

parent 5c102bd6
......@@ -127,6 +127,16 @@ namespace pinocchio
CHECK_DATA( (int)data.parents_fromRow.size() == model.nv );
CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv );
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
{
const typename Model::JointModel & jmodel = model.joints[joint_id];
CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
}
for( JointIndex j=1;int(j)<model.njoints;++j )
{
JointIndex c = (JointIndex)data.lastChild[j];
......
......@@ -94,6 +94,18 @@ namespace pinocchio
/// \brief Model of joint *i*, encapsulated in a JointModelAccessor.
JointModelVector joints;
/// \brief Starting index of the joint *i* in the configuration space
std::vector<int> idx_qs;
/// \brief Dimension of the joint *i* configuration subspace
std::vector<int> nqs;
/// \brief Starting index of the joint *i* in the tangent configuration space
std::vector<int> idx_vs;
/// \brief Dimension of the joint *i* tangent subspace
std::vector<int> nvs;
/// \brief Joint parent of joint *i*, denoted *li* (li==parents[i]).
std::vector<JointIndex> parents;
......@@ -157,6 +169,10 @@ namespace pinocchio
, inertias(1, Inertia::Zero())
, jointPlacements(1, SE3::Identity())
, joints(1)
, idx_qs(1,0)
, nqs(1,0)
, idx_vs(1,0)
, nvs(1,0)
, parents(1, 0)
, names(1)
, supports(1,IndexVector(1,0))
......@@ -187,8 +203,13 @@ namespace pinocchio
res.gravity = gravity.template cast<NewScalar>();
res.name = name;
/// Eigen Vectors
/// TODO: remove this pragma when neutralConfiguration will be removed
res.idx_qs = idx_qs;
res.nqs = nqs;
res.idx_vs = idx_vs;
res.nvs = nvs;
// Eigen Vectors
// TODO: remove this pragma when neutralConfiguration will be removed
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
res.neutralConfiguration = neutralConfiguration.template cast<NewScalar>();
......@@ -209,8 +230,7 @@ namespace pinocchio
rit++;
}
/// reserve vectors
// reserve vectors
res.inertias.resize(inertias.size());
res.jointPlacements.resize(jointPlacements.size());
res.joints.resize(joints.size());
......@@ -250,7 +270,13 @@ namespace pinocchio
&& other.subtrees == subtrees
&& other.gravity == gravity
&& other.name == name;
res &=
other.idx_qs == idx_qs
&& other.nqs == nqs
&& other.idx_vs == idx_vs
&& other.nvs == nvs;
/// TODO: remove this pragma when neutralConfiguration will be removed
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
......@@ -468,7 +494,8 @@ namespace pinocchio
///
/// \return Name of the joint.
///
PINOCCHIO_DEPRECATED const std::string & getJointName(const JointIndex index) const;
PINOCCHIO_DEPRECATED
const std::string & getJointName(const JointIndex index) const;
///
/// \brief Returns the index of a frame given by its name.
......@@ -508,7 +535,7 @@ namespace pinocchio
///
int addFrame(const Frame & frame);
/// Check the validity of the attributes of Model with respect to the specification of some
/// \brief Check the validity of the attributes of Model with respect to the specification of some
/// algorithms.
///
/// The method is a template so that the checkers can be defined in each algorithms.
......@@ -522,7 +549,7 @@ namespace pinocchio
/// Run check(fusion::list) with DEFAULT_CHECKERS as argument.
inline bool check() const;
/// Run checkData on data and current model.
/// \brief Run checkData on data and current model.
///
/// \param[in] data to be checked wrt *this.
///
......
......@@ -83,17 +83,23 @@ namespace pinocchio
JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back());
jmodel.setIndexes(idx,nq,nv);
assert(jmodel.idx_q() >= 0);
assert(jmodel.idx_v() >= 0);
const int joint_nq = jmodel.nq();
const int joint_idx_q = jmodel.idx_q();
const int joint_nv = jmodel.nv();
const int joint_idx_v = jmodel.idx_v();
assert(joint_idx_q >= 0);
assert(joint_idx_v >= 0);
inertias .push_back(Inertia::Zero());
parents .push_back(parent);
jointPlacements.push_back(joint_placement);
names .push_back(joint_name);
nq += joint_model.nq();
nv += joint_model.nv();
nq += joint_nq; nqs.push_back(joint_nq); idx_qs.push_back(joint_idx_q);
nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v);
if(joint_model.nq() > 0 && joint_model.nv() > 0)
if(joint_nq > 0 && joint_nv > 0)
{
effortLimit.conservativeResize(nv);
jmodel.jointVelocitySelector(effortLimit) = max_effort;
......@@ -104,7 +110,7 @@ namespace pinocchio
upperPositionLimit.conservativeResize(nq);
jmodel.jointConfigSelector(upperPositionLimit) = max_config;
/// TODO: remove this pragma when neutralConfiguration will be removed
// TODO: remove this pragma when neutralConfiguration will be removed
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
neutralConfiguration.conservativeResize(nq);
......
......@@ -67,6 +67,24 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
}
}
BOOST_AUTO_TEST_CASE(test_model_subspace_dimensions)
{
Model model;
buildModels::humanoidRandom(model);
// Check that i ends supports[i]
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
{
const Model::JointModel & jmodel = model.joints[joint_id];
BOOST_CHECK(model.nqs[joint_id] == jmodel.nq());
BOOST_CHECK(model.idx_qs[joint_id] == jmodel.idx_q());
BOOST_CHECK(model.nvs[joint_id] == jmodel.nv());
BOOST_CHECK(model.idx_vs[joint_id] == jmodel.idx_v());
}
}
BOOST_AUTO_TEST_CASE(comparison)
{
Model model;
......
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