Commit 39a3db9c authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

remove stuff dependent on urdf

parent 95f043a4
......@@ -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>
......
......@@ -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);
......
......@@ -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 {
......
......@@ -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
......
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