Commit 620a4f1c authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add Device::getJointAtConfigRank and getJointAtVelocityRank

parent b0fa5060
......@@ -103,6 +103,12 @@ namespace hpp {
/// Get vector of joints
const JointVector_t& getJointVector () const;
/// Get the joint at configuration rank r
JointPtr_t getJointAtConfigRank (const size_type& r) const;
/// Get the joint at velocity rank r
JointPtr_t getJointAtVelocityRank (const size_type& r) const;
/// Get joint by name
/// \param name name of the joint.
/// \throw runtime_error if device has no joint with this name
......@@ -342,6 +348,8 @@ namespace hpp {
DistanceResults_t distances_;
JointByName_t jointByName_;
JointVector_t jointVector_;
JointVector_t jointByConfigRank_;
JointVector_t jointByVelocityRank_;
JointPtr_t rootJoint_;
size_type numberDof_;
size_type configSize_;
......
......@@ -33,7 +33,8 @@ namespace hpp {
Device::Device(const std::string& name) :
name_ (name), distances_ (),
jointByName_ (),
jointVector_ (), rootJoint_ (0x0), numberDof_ (0),
jointVector_ (), jointByConfigRank_ (), jointByVelocityRank_ (),
rootJoint_ (0x0), numberDof_ (0),
configSize_ (0), currentConfiguration_ (configSize_),
currentVelocity_ (numberDof_), currentAcceleration_ (numberDof_),
com_ (), jacobianCom_ (3, 0), mass_ (0), upToDate_ (false),
......@@ -414,6 +415,10 @@ namespace hpp {
jointVector_.push_back (joint);
joint->rankInConfiguration_ = configSize_;
joint->rankInVelocity_ = numberDof_;
for (size_type i = 0; i < joint->configSize(); ++i)
jointByConfigRank_.push_back (joint);
for (size_type i = 0; i < joint->numberDof(); ++i)
jointByVelocityRank_.push_back (joint);
numberDof_ += joint->numberDof ();
configSize_ += joint->configSize ();
resizeState (joint);
......@@ -472,6 +477,16 @@ namespace hpp {
return jointVector_;
}
JointPtr_t Device::getJointAtConfigRank (const size_type& r) const
{
return jointByConfigRank_[r];
}
JointPtr_t Device::getJointAtVelocityRank (const size_type& r) const
{
return jointByVelocityRank_[r];
}
JointPtr_t Device::getJointByName (const std::string& name) const
{
JointByName_t::const_iterator it = jointByName_.find (name);
......
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