From 39a3db9c6ac34ecb161f8f656ccae5ce3e4be7cc Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Thu, 28 Jul 2016 15:54:40 +0200
Subject: [PATCH] remove stuff dependent on urdf
---
include/sot-dynamic/dynamic.h | 1 -
src/python-module-py.cpp | 4 ----
src/sot-dynamic.cpp | 28 ++++++++++++++++++----------
unitTesting/kineromeo.py | 14 +++++++++++++-
4 files changed, 31 insertions(+), 16 deletions(-)
diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h
index 20406b1..645575a 100644
--- a/include/sot-dynamic/dynamic.h
+++ b/include/sot-dynamic/dynamic.h
@@ -47,7 +47,6 @@
/* PINOCCHIO */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/joint/joint-variant.hpp>
-#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
diff --git a/src/python-module-py.cpp b/src/python-module-py.cpp
index 9df8e96..a333ca8 100644
--- a/src/python-module-py.cpp
+++ b/src/python-module-py.cpp
@@ -86,10 +86,6 @@ namespace dynamicgraph{
pointer1 = PyCObject_AsVoidPtr(object);
Dynamic* dyn_entity = (Dynamic*) pointer1;
- std::string msg("Error in obtaining pinocchio model");
- PyObject* dgpyError =
- PyErr_NewException(const_cast<char*>(msg.c_str()), NULL, NULL);
-
try {
se3::python::ModelHandler cppModelHandle =
boost::python::extract<se3::python::ModelHandler>(pyPinocchioObject);
diff --git a/src/sot-dynamic.cpp b/src/sot-dynamic.cpp
index 13871e6..69b4ad8 100644
--- a/src/sot-dynamic.cpp
+++ b/src/sot-dynamic.cpp
@@ -286,21 +286,29 @@ dg::Vector Dynamic::getPinocchioPos(int time)
if( freeFlyerPositionSIN) {
dg::Vector qFF=freeFlyerPositionSIN.access(time);
+
+
q.resize(qJoints.size() + 7);
- urdf::Rotation rot;
- rot.setFromRPY(qFF(3),qFF(4),qFF(5));
- double x,y,z,w;
- rot.getQuaternion(x,y,z,w);
- q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints;
+
+ 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>();
- urdf::Rotation rot;
- rot.setFromRPY(qFF(3),qFF(4),qFF(5));
- double x,y,z,w;
- rot.getQuaternion(x,y,z,w);
q.resize(qJoints.size()+1);
- q << qFF(0),qFF(1),qFF(2),x,y,z,w,qJoints.segment(6,qJoints.size()-6);
+
+ 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);
}
else {
diff --git a/unitTesting/kineromeo.py b/unitTesting/kineromeo.py
index 9e6304f..e7eac08 100644
--- a/unitTesting/kineromeo.py
+++ b/unitTesting/kineromeo.py
@@ -5,6 +5,7 @@
#
# ______________________________________________________________________________
# ******************************************************************************
+import pinocchio as se3
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.dynamics import *
@@ -24,7 +25,7 @@ set_printoptions(suppress=True, precision=7)
#Define robotName, urdfpath and initialConfig
robotName = 'romeo'
-urdfpath = "/local/rbudhira/git/proyan/sot-pinocchio/unitTesting/romeoNoToes.urdf"
+urdfpath = "romeoNoToes.urdf"
initialConfig = (0, 0, .840252, 0, 0, 0, # FF
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG
@@ -35,13 +36,24 @@ initialConfig = (0, 0, .840252, 0, 0, 0, # FF
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM
)
+
+#-----------------------------------------------------------------------------
+#---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+pinocchioModel = se3.buildModelFromUrdf(urdfpath,se3.JointModelFreeFlyer())
+pinocchioData = pinocchioModel.createData()
+
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
dyn = Dynamic("dyn")
+dyn.setModel(pinocchioModel)
+dyn.setData(pinocchioData)
dyn.displayModel()
+
+robotDim = dyn.getDimension()
inertiaRotor = (0,)*6+(5e-4,)*31
gearRatio = (0,)*6+(200,)*31
dyn.inertiaRotor.value = inertiaRotor
--
GitLab