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

[c++] Dealing with Spherical Joints

parent 9fe5912d
...@@ -138,7 +138,7 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -138,7 +138,7 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN; dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN;
dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN; dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
int& computeNewtonEuler( int& dummy,int time ); int& computeNewtonEuler( int& dummy,int time );
dg::SignalTimeDependent<dg::Vector,int> zmpSOUT; dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
...@@ -177,6 +177,9 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -177,6 +177,9 @@ class SOTDYNAMIC_EXPORT Dynamic
void displayModel() const void displayModel() const
{ assert(m_model); std::cout<<(*m_model)<<std::endl; }; { assert(m_model); std::cout<<(*m_model)<<std::endl; };
void setModel(se3::Model*);
void setData(se3::Data*);
/* --- GETTERS --- */ /* --- GETTERS --- */
...@@ -246,6 +249,7 @@ class SOTDYNAMIC_EXPORT Dynamic ...@@ -246,6 +249,7 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::Vector getPinocchioPos(int); dg::Vector getPinocchioPos(int);
dg::Vector getPinocchioVel(int); dg::Vector getPinocchioVel(int);
dg::Vector getPinocchioAcc(int); dg::Vector getPinocchioAcc(int);
std::vector<int> sphericalJoints;
}; };
......
...@@ -89,7 +89,8 @@ namespace dynamicgraph{ ...@@ -89,7 +89,8 @@ namespace dynamicgraph{
try { try {
se3::python::ModelHandler cppModelHandle = se3::python::ModelHandler cppModelHandle =
boost::python::extract<se3::python::ModelHandler>(pyPinocchioObject); boost::python::extract<se3::python::ModelHandler>(pyPinocchioObject);
dyn_entity->m_model = cppModelHandle.ptr(); dyn_entity->setModel(cppModelHandle.ptr());
//dyn_entity->m_model = cppModelHandle.ptr();
} }
catch (const std::exception& exc) { catch (const std::exception& exc) {
//PyErr_SetString(dgpyError, exc.what()); //PyErr_SetString(dgpyError, exc.what());
...@@ -128,7 +129,8 @@ namespace dynamicgraph{ ...@@ -128,7 +129,8 @@ namespace dynamicgraph{
try { try {
se3::python::DataHandler cppDataHandle = se3::python::DataHandler cppDataHandle =
boost::python::extract<se3::python::DataHandler>(pyPinocchioObject); boost::python::extract<se3::python::DataHandler>(pyPinocchioObject);
dyn_entity->m_data = cppDataHandle.ptr(); dyn_entity->setData(cppDataHandle.ptr());
//dyn_entity->m_data = cppDataHandle.ptr();
} }
catch (const std::exception& exc) { catch (const std::exception& exc) {
// PyErr_SetString(dgpyError, exc.what()); // PyErr_SetString(dgpyError, exc.what());
......
...@@ -196,6 +196,9 @@ Dynamic( const std::string & name) ...@@ -196,6 +196,9 @@ Dynamic( const std::string & name)
addCommand("createPosition", addCommand("createPosition",
makeCommandVoid2(*this,&Dynamic::cmd_createPositionSignal,docstring)); makeCommandVoid2(*this,&Dynamic::cmd_createPositionSignal,docstring));
} }
sphericalJoints.clear();
sotDEBUG(10)<< "Dynamic class_name address"<<&CLASS_NAME<<std::endl; sotDEBUG(10)<< "Dynamic class_name address"<<&CLASS_NAME<<std::endl;
sotDEBUGOUT(5); sotDEBUGOUT(5);
...@@ -218,6 +221,27 @@ Dynamic::~Dynamic( void ) { ...@@ -218,6 +221,27 @@ Dynamic::~Dynamic( void ) {
sotDEBUGOUT(15); sotDEBUGOUT(15);
} }
void
Dynamic::setModel(se3::Model* modelPtr){
this->m_model = modelPtr;
if (this->m_model->nq > m_model->nv) {
if (se3::nv(this->m_model->joints[1]) == 6)
sphericalJoints.push_back(3); //FreeFlyer Orientation
for(int i = 1; i<this->m_model->njoint; i++) //0: universe
if(se3::nq(this->m_model->joints[i]) == 4) //Spherical Joint Only
sphericalJoints.push_back(se3::idx_v(this->m_model->joints[i]));
}
}
void
Dynamic::setData(se3::Data* dataPtr){
this->m_data = dataPtr;
}
/*--------------------------------GETTERS-------------------------------------------*/ /*--------------------------------GETTERS-------------------------------------------*/
dg::Vector& Dynamic:: dg::Vector& Dynamic::
...@@ -283,32 +307,35 @@ dg::Vector Dynamic::getPinocchioPos(int time) ...@@ -283,32 +307,35 @@ dg::Vector Dynamic::getPinocchioPos(int time)
sotDEBUGIN(15); sotDEBUGIN(15);
dg::Vector qJoints=jointPositionSIN.access(time); dg::Vector qJoints=jointPositionSIN.access(time);
dg::Vector q; dg::Vector q;
assert(m_model); if (!sphericalJoints.empty()){
if( freeFlyerPositionSIN) {
if( freeFlyerPositionSIN) { dg::Vector qFF=freeFlyerPositionSIN.access(time);
dg::Vector qFF=freeFlyerPositionSIN.access(time); qJoints.head<6>() = qFF; //Overwrite qJoints ff with ffposition value
assert(sphericalJoints[0] == 3); // FreeFlyer should ideally be present.
q.resize(qJoints.size() + 7); }
q.resize(qJoints.size()+sphericalJoints.size());
Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())* int fillingIndex = 0;
Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())* int origIndex = 0;
Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX()); for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++){
if(*it-origIndex > 0){
q << qFF(0),qFF(1),qFF(2), q.segment(fillingIndex,*it-origIndex) = qJoints.segment(origIndex,*it-origIndex);
quat.x(),quat.y(),quat.z(),quat.w(), fillingIndex += *it-origIndex;
qJoints; origIndex += *it-origIndex;
} }
else if (se3::nv(m_model->joints[1]) == 6){ assert(*it == origIndex);
dg::Vector qFF = qJoints.head<6>(); Eigen::Quaternion<double> temp =
q.resize(qJoints.size()+1); Eigen::AngleAxisd(qJoints(origIndex+2),Eigen::Vector3d::UnitZ())*
Eigen::AngleAxisd(qJoints(origIndex+1),Eigen::Vector3d::UnitY())*
Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(qJoints(origIndex),Eigen::Vector3d::UnitX());
Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())* q(fillingIndex) = temp.x();
Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX()); q(fillingIndex+1) = temp.y();
q << qFF(0),qFF(1),qFF(2), q(fillingIndex+2) = temp.z();
quat.x(),quat.y(),quat.z(),quat.w(), q(fillingIndex+3) = temp.w();
qJoints.segment(6,qJoints.size()-6); it++;
assert(q.size() == m_model->nq); fillingIndex +=4;
origIndex +=3;
}
if(qJoints.size()>origIndex) q.segment(fillingIndex, qJoints.size()-origIndex) = qJoints.tail(qJoints.size()-origIndex);
} }
else { else {
q.resize(qJoints.size()); q.resize(qJoints.size());
...@@ -316,7 +343,7 @@ dg::Vector Dynamic::getPinocchioPos(int time) ...@@ -316,7 +343,7 @@ dg::Vector Dynamic::getPinocchioPos(int time)
} }
sotDEBUG(15) <<"Position out"<<q<<std::endl; sotDEBUG(15) <<"Position out"<<q<<std::endl;
sotDEBUGOUT(15); sotDEBUGOUT(15);
return q; return q;
} }
Eigen::VectorXd Dynamic::getPinocchioVel(int time) Eigen::VectorXd Dynamic::getPinocchioVel(int time)
......
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