Skip to content
This repository was archived by the owner on Nov 28, 2025. It is now read-only.

Commit afc5f68

Browse files
nim65snim65s
committed
Compatibility with Pinocchio v2
1 parent b393d94 commit afc5f68

File tree

5 files changed

+47
-48
lines changed

5 files changed

+47
-48
lines changed

‎include/sot-dynamic-pinocchio/dynamic-pinocchio.h‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,6 @@
3333
/* PINOCCHIO */
3434
#include <pinocchio/macros.hpp>
3535
#include <pinocchio/multibody/model.hpp>
36-
#include <pinocchio/multibody/joint/joint-variant.hpp>
3736
#include <pinocchio/algorithm/rnea.hpp>
3837
#include <pinocchio/algorithm/jacobian.hpp>
3938
#include <pinocchio/algorithm/frames.hpp>
@@ -84,8 +83,8 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
8483
DYNAMIC_GRAPH_ENTITY_DECL();
8584

8685
/* --- MODEL ATRIBUTES --- */
87-
se3::Model* m_model;
88-
se3::Data* m_data;
86+
pinocchio::Model* m_model;
87+
pinocchio::Data* m_data;
8988

9089
/* --- MODEL ATRIBUTES --- */
9190

@@ -174,9 +173,9 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
174173
void displayModel() const
175174
{ assert(m_model); std::cout<<(*m_model)<<std::endl; };
176175

177-
void setModel(se3::Model*);
176+
void setModel(pinocchio::Model*);
178177

179-
void setData(se3::Data*);
178+
void setData(pinocchio::Data*);
180179

181180
/* --- GETTERS --- */
182181

‎src/python-module-py.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ namespace dynamicgraph{
1616
PyObject* object = NULL;
1717
PyObject* pyPinocchioObject;
1818
void* pointer1 = NULL;
19-
se3::Model* pointer2 = NULL;
19+
pinocchio::Model* pointer2 = NULL;
2020
if (!PyArg_ParseTuple(args, "OO", &object, &pyPinocchioObject))
2121
return NULL;
2222

@@ -30,8 +30,8 @@ namespace dynamicgraph{
3030
DynamicPinocchio* dyn_entity = (DynamicPinocchio*) pointer1;
3131

3232
try {
33-
boost::python::extract<se3::Model&> cppHandle(pyPinocchioObject);
34-
pointer2 = (se3::Model*) &cppHandle();
33+
boost::python::extract<pinocchio::Model&> cppHandle(pyPinocchioObject);
34+
pointer2 = (pinocchio::Model*) &cppHandle();
3535
dyn_entity->setModel(pointer2);
3636
}
3737
catch (const std::exception& exc) {
@@ -54,7 +54,7 @@ namespace dynamicgraph{
5454
PyObject* object = NULL;
5555
PyObject* pyPinocchioObject;
5656
void* pointer1 = NULL;
57-
se3::Data* pointer2 = NULL;
57+
pinocchio::Data* pointer2 = NULL;
5858
if (!PyArg_ParseTuple(args, "OO", &object, &pyPinocchioObject))
5959
return NULL;
6060

@@ -68,8 +68,8 @@ namespace dynamicgraph{
6868
DynamicPinocchio* dyn_entity = (DynamicPinocchio*) pointer1;
6969

7070
try {
71-
boost::python::extract<se3::Data&> cppHandle(pyPinocchioObject);
72-
pointer2 = (se3::Data*) &cppHandle();
71+
boost::python::extract<pinocchio::Data&> cppHandle(pyPinocchioObject);
72+
pointer2 = (pinocchio::Data*) &cppHandle();
7373
dyn_entity->setData(pointer2);
7474
}
7575
catch (const std::exception& exc) {

‎src/sot-dynamic-pinocchio.cpp‎

Lines changed: 32 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -246,22 +246,22 @@ DynamicPinocchio::~DynamicPinocchio( void ) {
246246
}
247247

248248
void
249-
DynamicPinocchio::setModel(se3::Model* modelPtr){
249+
DynamicPinocchio::setModel(pinocchio::Model* modelPtr){
250250
this->m_model = modelPtr;
251251

252252
if (this->m_model->nq > m_model->nv) {
253-
if (se3::nv(this->m_model->joints[1]) == 6)
253+
if (pinocchio::nv(this->m_model->joints[1]) == 6)
254254
sphericalJoints.push_back(3); //FreeFlyer Orientation
255255

256256
for(int i = 1; i<this->m_model->njoints; i++) //0: universe
257-
if(se3::nq(this->m_model->joints[i]) == 4) //Spherical Joint Only
258-
sphericalJoints.push_back(se3::idx_v(this->m_model->joints[i]));
257+
if(pinocchio::nq(this->m_model->joints[i]) == 4) //Spherical Joint Only
258+
sphericalJoints.push_back(pinocchio::idx_v(this->m_model->joints[i]));
259259
}
260260
}
261261

262262

263263
void
264-
DynamicPinocchio::setData(se3::Data* dataPtr){
264+
DynamicPinocchio::setData(pinocchio::Data* dataPtr){
265265
this->m_data = dataPtr;
266266

267267
}
@@ -696,9 +696,9 @@ dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,const int& time )
696696
res.resize(3);
697697
newtonEulerSINTERN(time);
698698

699-
const se3::Force& ftau = m_data->oMi[1].act(m_data->f[1]);
700-
const se3::Force::Vector3& tau = ftau.angular();
701-
const se3::Force::Vector3& f = ftau.linear();
699+
const pinocchio::Force& ftau = m_data->oMi[1].act(m_data->f[1]);
700+
const pinocchio::Force::Vector3& tau = ftau.angular();
701+
const pinocchio::Force::Vector3& f = ftau.linear();
702702
res(0) = -tau[1]/f[2];
703703
res(1) = tau[0]/f[2];
704704
res(2) = 0;
@@ -715,7 +715,7 @@ dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,const int& time )
715715
int& DynamicPinocchio::computeJacobians(int& dummy, const int& time) {
716716
sotDEBUGIN(25);
717717
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
718-
se3::computeJacobians(*m_model,*m_data, q);
718+
pinocchio::computeJacobians(*m_model,*m_data, q);
719719
sotDEBUG(25)<<"Jacobians updated"<<std::endl;
720720
sotDEBUGOUT(25);
721721
return dummy;
@@ -725,7 +725,7 @@ int& DynamicPinocchio::computeForwardKinematics(int& dummy, const int& time) {
725725
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
726726
const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
727727
const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
728-
se3::forwardKinematics(*m_model, *m_data, q, v, a);
728+
pinocchio::forwardKinematics(*m_model, *m_data, q, v, a);
729729
sotDEBUG(25)<<"Kinematics updated"<<std::endl;
730730
sotDEBUGOUT(25);
731731
return dummy;
@@ -735,7 +735,7 @@ int& DynamicPinocchio::computeCcrba(int& dummy, const int& time) {
735735
sotDEBUGIN(25);
736736
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
737737
const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
738-
se3::ccrba(*m_model,*m_data, q, v);
738+
pinocchio::ccrba(*m_model,*m_data, q, v);
739739
sotDEBUG(25)<<"Inertia and Momentum updated"<<std::endl;
740740
sotDEBUGOUT(25);
741741
return dummy;
@@ -752,15 +752,15 @@ computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res,co
752752
jacobiansSINTERN(time);
753753

754754
//TODO: Find a way to remove tmp object
755-
se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
755+
pinocchio::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
756756

757757
//Computes Jacobian in world coordinates.
758758
if(isFrame){
759-
se3::getJacobian<se3::WORLD>(*m_model,*m_data,
760-
m_model->frames[(se3::Model::Index)jointId].parent,tmp);
759+
pinocchio::getJacobian<pinocchio::WORLD>(*m_model,*m_data,
760+
m_model->frames[(pinocchio::Model::Index)jointId].parent,tmp);
761761
}
762762
else
763-
se3::getJacobian<se3::WORLD>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
763+
pinocchio::getJacobian<pinocchio::WORLD>(*m_model,*m_data,(pinocchio::Model::Index)jointId,tmp);
764764
res = tmp;
765765
sotDEBUGOUT(25);
766766
return res;
@@ -778,22 +778,22 @@ computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int j
778778
forwardKinematicsSINTERN(time);
779779

780780
//TODO: Find a way to remove tmp object
781-
se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
781+
pinocchio::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
782782
//std::string temp;
783783
//Computes Jacobian in end-eff coordinates.
784784
if(isFrame){
785-
se3::framesForwardKinematics(*m_model,*m_data);
786-
se3::getFrameJacobian(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
785+
pinocchio::framesForwardKinematics(*m_model,*m_data);
786+
pinocchio::getFrameJacobian(*m_model,*m_data,(pinocchio::Model::Index)jointId,tmp);
787787
sotDEBUG(25) << "EndEffJacobian for "
788-
<< m_model->frames.at((se3::Model::Index)jointId).name
788+
<< m_model->frames.at((pinocchio::Model::Index)jointId).name
789789
<<" is "<<tmp<<std::endl;
790790
}
791791
else {
792-
//temp = m_model->getJointName((se3::Model::Index)jointId);
793-
se3::getJacobian<se3::LOCAL>
794-
(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
792+
//temp = m_model->getJointName((pinocchio::Model::Index)jointId);
793+
pinocchio::getJacobian<pinocchio::LOCAL>
794+
(*m_model,*m_data,(pinocchio::Model::Index)jointId,tmp);
795795
sotDEBUG(25) << "EndEffJacobian for "
796-
<< m_model->getJointName((se3::Model::Index)jointId)
796+
<< m_model->getJointName((pinocchio::Model::Index)jointId)
797797
<<" is "<<tmp<<std::endl;
798798
}
799799
res = tmp;
@@ -817,13 +817,13 @@ computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous&
817817
std::string temp;
818818
forwardKinematicsSINTERN(time);
819819
if(isFrame){
820-
se3::framesForwardKinematics(*m_model,*m_data);
820+
pinocchio::framesForwardKinematics(*m_model,*m_data);
821821
res.matrix()= m_data->oMf[jointId].toHomogeneousMatrix();
822-
temp = m_model->frames.at((se3::Model::Index)jointId).name;
822+
temp = m_model->frames.at((pinocchio::Model::Index)jointId).name;
823823
}
824824
else{
825825
res.matrix()= m_data->oMi[jointId].toHomogeneousMatrix();
826-
temp = m_model->getJointName((se3::Model::Index)jointId);
826+
temp = m_model->getJointName((pinocchio::Model::Index)jointId);
827827
}
828828
sotDEBUG(25)<<"For "<<temp<<" with id: "<<jointId<<" position is "<<res<<std::endl;
829829
sotDEBUGOUT(25);
@@ -837,7 +837,7 @@ computeGenericVelocity( const int jointId, dg::Vector& res,const int& time )
837837
assert(m_data);
838838
res.resize(6);
839839
forwardKinematicsSINTERN(time);
840-
const se3::Motion& aRV = m_data->v[jointId];
840+
const pinocchio::Motion& aRV = m_data->v[jointId];
841841
res<<aRV.linear(),aRV.angular();
842842
sotDEBUGOUT(25);
843843
return res;
@@ -850,7 +850,7 @@ computeGenericAcceleration( const int jointId ,dg::Vector& res,const int& time )
850850
assert(m_data);
851851
res.resize(6);
852852
forwardKinematicsSINTERN(time);
853-
const se3::Motion& aRA = m_data->a[jointId];
853+
const pinocchio::Motion& aRA = m_data->a[jointId];
854854
res<<aRA.linear(),aRA.angular();
855855
sotDEBUGOUT(25);
856856
return res;
@@ -865,7 +865,7 @@ computeNewtonEuler(int& dummy,const int& time )
865865
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
866866
const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
867867
const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
868-
se3::rnea(*m_model,*m_data,q,v,a);
868+
pinocchio::rnea(*m_model,*m_data,q,v,a);
869869

870870
sotDEBUG(1)<< "pos = " <<q <<std::endl;
871871
sotDEBUG(1)<< "vel = " <<v <<std::endl;
@@ -882,7 +882,7 @@ computeJcom( dg::Matrix& Jcom,const int& time )
882882
sotDEBUGIN(25);
883883

884884
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
885-
Jcom = se3::jacobianCenterOfMass(*m_model, *m_data,
885+
Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data,
886886
q, false);
887887
sotDEBUGOUT(25);
888888
return Jcom;
@@ -894,7 +894,7 @@ computeCom( dg::Vector& com,const int& time )
894894

895895
sotDEBUGIN(25);
896896
forwardKinematicsSINTERN(time);
897-
se3::centerOfMass(*m_model,*m_data,false);
897+
pinocchio::centerOfMass(*m_model,*m_data,false);
898898
com = m_data->com[0];
899899
sotDEBUGOUT(25);
900900
return com;
@@ -905,7 +905,7 @@ computeInertia( dg::Matrix& res,const int& time )
905905
{
906906
sotDEBUGIN(25);
907907
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
908-
res = se3::crba(*m_model, *m_data, q);
908+
res = pinocchio::crba(*m_model, *m_data, q);
909909
res.triangularView<Eigen::StrictlyLower>() =
910910
res.transpose().triangularView<Eigen::StrictlyLower>();
911911
sotDEBUGOUT(25);

‎unitTesting/kineromeo.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -25,12 +25,12 @@
2525

2626
#Taking input from pinocchio
2727
from pinocchio.romeo_wrapper import RomeoWrapper
28-
import pinocchio as se3
28+
import pinocchio as pin
2929

3030
#SET THE PATH TO THE URDF AND MESHES
3131
urdfPath = "~/git/sot/pinocchio/models/romeo.urdf"
3232
urdfDir = ["~/git/sot/pinocchio/models"]
33-
pinocchioRobot = RomeoWrapper(urdfPath, urdfDir, se3.JointModelFreeFlyer())
33+
pinocchioRobot = RomeoWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
3434
robotName = 'romeo'
3535
pinocchioRobot.initDisplay(loadModel=True)
3636
pinocchioRobot.display(pinocchioRobot.q0)
@@ -47,7 +47,7 @@
4747
#-----------------------------------------------------------------------------
4848
#---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
4949
#-----------------------------------------------------------------------------
50-
#pinocchioModel = se3.buildModelFromUrdf(urdfpath, se3.JointModelFreeFlyer())
50+
#pinocchioModel = pin.buildModelFromUrdf(urdfpath, pin.JointModelFreeFlyer())
5151
#pinocchioData = pinocchioModel.createData()
5252

5353
#-----------------------------------------------------------------------------

‎unitTesting/kinetalos.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,9 @@
4242
#---- DYN --------------------------------------------------------------------
4343
#-----------------------------------------------------------------------------
4444
from pinocchio.robot_wrapper import RobotWrapper
45-
import pinocchio as se3
45+
import pinocchio as pin
4646
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
47-
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, se3.JointModelFreeFlyer())
47+
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
4848
pinocchioRobot.initDisplay(loadModel=True)
4949
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
5050

0 commit comments

Comments
 (0)