From e82f73f655a4a14d72ee85e61661a211c79e9f6c Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Wed, 17 Aug 2016 19:23:31 +0200
Subject: [PATCH] [c++] Dealing with Spherical Joints
---
include/sot-dynamic/dynamic.h | 6 ++-
src/python-module-py.cpp | 6 ++-
src/sot-dynamic.cpp | 81 +++++++++++++++++++++++------------
3 files changed, 63 insertions(+), 30 deletions(-)
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index 645575a..175b5e0 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -138,7 +138,7 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN;
dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
-
+
int& computeNewtonEuler( int& dummy,int time );
dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
@@ -177,6 +177,9 @@ class SOTDYNAMIC_EXPORT Dynamic
void displayModel() const
{ assert(m_model); std::cout<<(*m_model)<<std::endl; };
+ void setModel(se3::Model*);
+
+ void setData(se3::Data*);
/* --- GETTERS --- */
@@ -246,6 +249,7 @@ class SOTDYNAMIC_EXPORT Dynamic
dg::Vector getPinocchioPos(int);
dg::Vector getPinocchioVel(int);
dg::Vector getPinocchioAcc(int);
+ std::vector<int> sphericalJoints;
};
diff --git a/src/python-module-py.cpp b/src/python-module-py.cpp
index a333ca8..34b5a5d 100644
--- a/src/python-module-py.cpp
+++ b/src/python-module-py.cpp
@@ -89,7 +89,8 @@ namespace dynamicgraph{
try {
se3::python::ModelHandler cppModelHandle =
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) {
//PyErr_SetString(dgpyError, exc.what());
@@ -128,7 +129,8 @@ namespace dynamicgraph{
try {
se3::python::DataHandler cppDataHandle =
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) {
// PyErr_SetString(dgpyError, exc.what());
diff --git a/src/sot-dynamic.cpp b/src/sot-dynamic.cpp
index 48b6432..ef78d46 100644
--- a/src/sot-dynamic.cpp
+++ b/src/sot-dynamic.cpp
@@ -196,6 +196,9 @@ Dynamic( const std::string & name)
addCommand("createPosition",
makeCommandVoid2(*this,&Dynamic::cmd_createPositionSignal,docstring));
}
+
+
+ sphericalJoints.clear();
sotDEBUG(10)<< "Dynamic class_name address"<<&CLASS_NAME<<std::endl;
sotDEBUGOUT(5);
@@ -218,6 +221,27 @@ Dynamic::~Dynamic( void ) {
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-------------------------------------------*/
dg::Vector& Dynamic::
@@ -283,32 +307,35 @@ dg::Vector Dynamic::getPinocchioPos(int time)
sotDEBUGIN(15);
dg::Vector qJoints=jointPositionSIN.access(time);
dg::Vector q;
- assert(m_model);
-
- if( freeFlyerPositionSIN) {
- dg::Vector qFF=freeFlyerPositionSIN.access(time);
-
- q.resize(qJoints.size() + 7);
-
- Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
- Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())*
- Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX());
-
- q << qFF(0),qFF(1),qFF(2),
- quat.x(),quat.y(),quat.z(),quat.w(),
- qJoints;
- }
- else if (se3::nv(m_model->joints[1]) == 6){
- dg::Vector qFF = qJoints.head<6>();
- q.resize(qJoints.size()+1);
-
- Eigen::Quaternion<double> quat = Eigen::AngleAxisd(qFF(5),Eigen::Vector3d::UnitZ())*
- Eigen::AngleAxisd(qFF(4),Eigen::Vector3d::UnitY())*
- Eigen::AngleAxisd(qFF(3),Eigen::Vector3d::UnitX());
- q << qFF(0),qFF(1),qFF(2),
- quat.x(),quat.y(),quat.z(),quat.w(),
- qJoints.segment(6,qJoints.size()-6);
- assert(q.size() == m_model->nq);
+ if (!sphericalJoints.empty()){
+ if( freeFlyerPositionSIN) {
+ 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()+sphericalJoints.size());
+ int fillingIndex = 0;
+ int origIndex = 0;
+ for (std::vector<int>::const_iterator it = sphericalJoints.begin(); it < sphericalJoints.end(); it++){
+ if(*it-origIndex > 0){
+ q.segment(fillingIndex,*it-origIndex) = qJoints.segment(origIndex,*it-origIndex);
+ fillingIndex += *it-origIndex;
+ origIndex += *it-origIndex;
+ }
+ assert(*it == origIndex);
+ Eigen::Quaternion<double> temp =
+ Eigen::AngleAxisd(qJoints(origIndex+2),Eigen::Vector3d::UnitZ())*
+ Eigen::AngleAxisd(qJoints(origIndex+1),Eigen::Vector3d::UnitY())*
+ Eigen::AngleAxisd(qJoints(origIndex),Eigen::Vector3d::UnitX());
+ q(fillingIndex) = temp.x();
+ q(fillingIndex+1) = temp.y();
+ q(fillingIndex+2) = temp.z();
+ q(fillingIndex+3) = temp.w();
+ it++;
+ fillingIndex +=4;
+ origIndex +=3;
+ }
+ if(qJoints.size()>origIndex) q.segment(fillingIndex, qJoints.size()-origIndex) = qJoints.tail(qJoints.size()-origIndex);
}
else {
q.resize(qJoints.size());
@@ -316,7 +343,7 @@ dg::Vector Dynamic::getPinocchioPos(int time)
}
sotDEBUG(15) <<"Position out"<<q<<std::endl;
sotDEBUGOUT(15);
- return q;
+ return q;
}
Eigen::VectorXd Dynamic::getPinocchioVel(int time)
--
GitLab