Commit cff464ee authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[c++] Deal with Spherical Joints in JointLimits.

update signal lowerJl and upperJl
parent daade23f
......@@ -249,6 +249,8 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::Vector getPinocchioPos(int);
dg::Vector getPinocchioVel(int);
dg::Vector getPinocchioAcc(int);
//\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
std::vector<int> sphericalJoints;
};
......
......@@ -247,13 +247,38 @@ Dynamic::setData(se3::Data* dataPtr){
dg::Vector& Dynamic::
getLowerPositionLimits(dg::Vector& res, const int&)
{
sotDEBUGIN(15);
assert(m_model);
res.resize(m_model->nq);
res = m_model->lowerPositionLimit;
res.resize(m_model->nv);
if (!sphericalJoints.empty()) {
int fillingIndex = 0; //SoTValue
int origIndex = 0; //PinocchioValue
for (std::vector<int>::const_iterator it = sphericalJoints.begin();
it < sphericalJoints.end(); it++){
if(*it-fillingIndex > 0){
res.segment(fillingIndex, *it-fillingIndex) = m_model->lowerPositionLimit.segment(origIndex, *it-fillingIndex);
//Don't Change this order
origIndex += *it-fillingIndex;
fillingIndex += *it-fillingIndex;
}
//Found a Spherical Joint.
//Assuming that spherical joint limits are unset
res(fillingIndex) = std::numeric_limits<double>::min();
res(fillingIndex+1) = std::numeric_limits<double>::min();
res(fillingIndex+2) = std::numeric_limits<double>::min();
fillingIndex +=3;
origIndex +=4;
}
assert(m_model->nv-fillingIndex == m_model->nq- origIndex);
if(m_model->nv > fillingIndex)
res.segment(fillingIndex, m_model->nv-fillingIndex) =
m_model->lowerPositionLimit.segment(origIndex, m_model->nv-fillingIndex);
}
else {
res = m_model->lowerPositionLimit;
}
sotDEBUG(15) << "lowerLimit (" << res << ")=" << std::endl;
sotDEBUGOUT(15);
return res;
......@@ -265,10 +290,36 @@ getUpperPositionLimits(dg::Vector& res, const int&)
sotDEBUGIN(15);
assert(m_model);
res.resize(m_model->nq);
res = m_model->upperPositionLimit;
sotDEBUG(15) << "upperLimit (" << res << ")=" <<std::endl;
res.resize(m_model->nv);
if (!sphericalJoints.empty()) {
int fillingIndex = 0; //SoTValue
int origIndex = 0; //PinocchioValue
for (std::vector<int>::const_iterator it = sphericalJoints.begin();
it < sphericalJoints.end(); it++){
if(*it-fillingIndex > 0){
res.segment(fillingIndex, *it-fillingIndex) = m_model->upperPositionLimit.segment(origIndex, *it-fillingIndex);
//Don't Change this order
origIndex += *it-fillingIndex;
fillingIndex += *it-fillingIndex;
}
//Found a Spherical Joint.
//Assuming that spherical joint limits are unset
res(fillingIndex) = std::numeric_limits<double>::max();
res(fillingIndex+1) = std::numeric_limits<double>::max();
res(fillingIndex+2) = std::numeric_limits<double>::max();
fillingIndex +=3;
origIndex +=4;
}
assert(m_model->nv-fillingIndex == m_model->nq- origIndex);
if(m_model->nv > fillingIndex)
res.segment(fillingIndex, m_model->nv-fillingIndex) =
m_model->upperPositionLimit.segment(origIndex, m_model->nv-fillingIndex);
}
else {
res = m_model->upperPositionLimit;
}
sotDEBUG(15) << "upperLimit (" << res << ")=" << std::endl;
sotDEBUGOUT(15);
return res;
}
......
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