Skip to content
GitLab
Commit 1f511197 authored by Julian Viereck's avatar Julian Viereck Committed by Andrea Del Prete
Browse files

Convert to pinocchio2 namespace (#54)

parent 809abf65
Hide whitespace changes
Inline Side-by-side
include/sot/torque_control/admittance-controller.hh
View file @ 1f511197
......@@ -122,7 +122,7 @@ namespace dynamicgraph {
/// tsid
tsid::robots::RobotWrapper * m_robot;
se3::Data* m_data;
pinocchio::Data* m_data;
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
......@@ -135,7 +135,7 @@ namespace dynamicgraph {
tsid::math::Vector m_dq_fd; /// joint velocities computed with finite differences
tsid::math::Vector m_qPrev; /// previous value of encoders
typedef se3::Data::Matrix6x Matrix6x;
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
......
include/sot/torque_control/base-estimator.hh
View file @ 1f511197
......@@ -60,7 +60,7 @@ namespace dynamicgraph {
/** Compute s12 as an intermediate transform between s1 and s2 SE3 transforms**/
void se3Interp(const se3::SE3 & s1, const se3::SE3 & s2, const double alpha, se3::SE3 & s12);
void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12);
/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d & R);
......@@ -81,7 +81,7 @@ namespace dynamicgraph {
:public::dynamicgraph::Entity
{
typedef BaseEstimator EntityClassName;
typedef se3::SE3 SE3;
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector4d Vector4;
......@@ -217,10 +217,10 @@ namespace dynamicgraph {
double m_w_lf_filtered; /// filtered weight of the estimation coming from the left foot
double m_w_rf_filtered; /// filtered weight of the estimation coming from the right foot
se3::Model m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
se3::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
se3::SE3 m_oMff_rf; /// world-to-base transformation obtained through right foot
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
pinocchio::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
pinocchio::SE3 m_oMff_rf; /// world-to-base transformation obtained through right foot
SE3 m_oMlfs; /// transformation from world to left foot sole
SE3 m_oMrfs; /// transformation from world to right foot sole
Vector7 m_oMlfs_xyzquat;
......@@ -233,9 +233,9 @@ namespace dynamicgraph {
SE3 m_sole_M_ftSens; /// foot sole to F/T sensor transformation
se3::FrameIndex m_right_foot_id;
se3::FrameIndex m_left_foot_id;
se3::FrameIndex m_IMU_body_id;
pinocchio::FrameIndex m_right_foot_id;
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_IMU_body_id;
Eigen::VectorXd m_q_pin; /// robot configuration according to pinocchio convention
Eigen::VectorXd m_q_sot; /// robot configuration according to SoT convention
......
include/sot/torque_control/device-torque-ctrl.hh
View file @ 1f511197
......@@ -142,11 +142,11 @@ namespace dynamicgraph
bool m_isTorqueControlled;
/// robot geometric/inertial data
tsid::robots::RobotWrapper * m_robot;
se3::Data * m_data;
tsid::robots::RobotWrapper * m_robot;
pinocchio::Data * m_data;
tsid::tasks::TaskSE3Equality * m_contactRF;
tsid::tasks::TaskSE3Equality * m_contactLF;
unsigned int m_nk; // number of contact forces
unsigned int m_nk; // number of contact forces
tsid::math::Vector m_q, m_v, m_dv, m_f;
tsid::math::Vector m_q_sot, m_v_sot, m_dv_sot;
......
include/sot/torque_control/free-flyer-locator.hh
View file @ 1f511197
......@@ -99,11 +99,11 @@ namespace dynamicgraph {
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
se3::Model *m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
se3::SE3 m_Mff; /// SE3 Transform from center of feet to base
se3::SE3 m_w_M_lf;
se3::SE3 m_w_M_rf;
pinocchio::Model *m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
pinocchio::SE3 m_Mff; /// SE3 Transform from center of feet to base
pinocchio::SE3 m_w_M_lf;
pinocchio::SE3 m_w_M_rf;
long unsigned int m_right_foot_id;
long unsigned int m_left_foot_id;
Eigen::VectorXd m_q_pin; /// robot configuration according to pinocchio convention
......
include/sot/torque_control/inverse-dynamics-balance-controller.hh
View file @ 1f511197
......@@ -250,7 +250,7 @@ namespace dynamicgraph {
tsid::math::Vector m_q_urdf;
tsid::math::Vector m_v_urdf;
typedef se3::Data::Matrix6x Matrix6x;
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
......
include/sot/torque_control/torque-offset-estimator.hh
View file @ 1f511197
......@@ -89,15 +89,15 @@ namespace dynamicgraph {
protected:
RobotUtil * m_robot_util;
se3::Model m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
int n_iterations; //Number of iterations to consider
double epsilon;
double gyro_epsilon;
int ffIndex, torsoIndex; //Index of the free-flyer and torso frames
Eigen::VectorXd jointTorqueOffsets;
se3::SE3 m_torso_X_imu; // Definition of the imu in the chest frame.
pinocchio::SE3 m_torso_X_imu; // Definition of the imu in the chest frame.
// stdAlignedVector encSignals;
// stdAlignedVector accSignals;
......
include/sot/torque_control/utils/trajectory-generators.hh
View file @ 1f511197
......@@ -35,6 +35,7 @@
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <iostream>
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
......
src/admittance-controller.cpp
View file @ 1f511197
......@@ -155,8 +155,8 @@ namespace dynamicgraph
vector<string> package_dirs;
m_robot = new robots::RobotWrapper(m_robot_util->m_urdf_filename,
package_dirs,
se3::JointModelFreeFlyer());
m_data = new se3::Data(m_robot->model());
pinocchio::JointModelFreeFlyer());
m_data = new pinocchio::Data(m_robot->model());
assert(m_robot->nv()>=6);
m_robot_util->m_nbJoints = m_robot->nv()-6;
......
src/base-estimator.cpp
View file @ 1f511197
......@@ -33,18 +33,18 @@ namespace dynamicgraph
using namespace dg;
using namespace dg::command;
using namespace std;
using namespace se3;
using namespace pinocchio;
using boost::math::normal; // typedef provides default type is double.
void se3Interp(const se3::SE3 & s1, const se3::SE3 & s2, const double alpha, se3::SE3 & s12)
void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12)
{
const Eigen::Vector3d t_( s1.translation() * alpha+
s2.translation() * (1-alpha));
const Eigen::Vector3d w( se3::log3(s1.rotation()) * alpha +
se3::log3(s2.rotation()) * (1-alpha) );
const Eigen::Vector3d w( pinocchio::log3(s1.rotation()) * alpha +
pinocchio::log3(s2.rotation()) * (1-alpha) );
s12 = se3::SE3(se3::exp3(w),t_);
s12 = pinocchio::SE3(pinocchio::exp3(w),t_);
}
void rpyToMatrix(double roll, double pitch, double yaw, Eigen::Matrix3d & R)
{
......@@ -286,8 +286,8 @@ namespace dynamicgraph
return;
}
se3::urdf::buildModel(m_robot_util->m_urdf_filename,
se3::JointModelFreeFlyer(), m_model);
pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
pinocchio::JointModelFreeFlyer(), m_model);
assert(m_model.existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
assert(m_model.existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
......@@ -328,7 +328,7 @@ namespace dynamicgraph
SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
return;
}
m_data = new se3::Data(m_model);
m_data = new pinocchio::Data(m_model);
m_initSucceeded = true;
}
......@@ -452,7 +452,7 @@ namespace dynamicgraph
void BaseEstimator::compute_zmp(const Vector6 & w, Vector2 & zmp)
{
se3::Force f(w);
pinocchio::Force f(w);
f = m_sole_M_ftSens.act(f);
if(f.linear()[2]==0.0)
return;
......@@ -501,8 +501,8 @@ namespace dynamicgraph
rfMff = groundMfoot * rfMff;
// set the world frame in between the feet
const Vector3 w( 0.5*(se3::log3(lfMff.rotation())+se3::log3(rfMff.rotation())) );
SE3 oMff = SE3(se3::exp3(w), 0.5*(lfMff.translation()+rfMff.translation()));
const Vector3 w( 0.5*(pinocchio::log3(lfMff.rotation())+pinocchio::log3(rfMff.rotation())) );
SE3 oMff = SE3(pinocchio::exp3(w), 0.5*(lfMff.translation()+rfMff.translation()));
// add a constant offset to make the world frame match the one in OpenHRP
oMff.translation()(0) += 9.562e-03;
......@@ -583,8 +583,8 @@ namespace dynamicgraph
m_q_pin.head<6>().setZero();
m_q_pin(6) = 1.0;
m_v_pin.head<6>().setZero();
se3::forwardKinematics(m_model, *m_data, m_q_pin, m_v_pin);
se3::framesForwardKinematics(m_model, *m_data);
pinocchio::forwardKinematics(m_model, *m_data, m_q_pin, m_v_pin);
pinocchio::framesForwardKinematics(m_model, *m_data);
getProfiler().stop(PROFILE_BASE_KINEMATICS_COMPUTATION);
......
src/common.cpp
View file @ 1f511197
......@@ -15,6 +15,7 @@
*/
#include <iostream>
#include <iostream>
#include <sot/torque_control/common.hh>
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
......
src/control-manager.cpp
View file @ 1f511197
......@@ -190,7 +190,7 @@ namespace dynamicgraph
m_emergency_stop_triggered = false;
m_initSucceeded = true;
vector<string> package_dirs;
m_robot = new robots::RobotWrapper(urdfFile, package_dirs, se3::JointModelFreeFlyer());
m_robot = new robots::RobotWrapper(urdfFile, package_dirs, pinocchio::JointModelFreeFlyer());
std::string localName(robotRef);
if (!isNameInRobotUtil(localName))
......@@ -659,12 +659,12 @@ namespace dynamicgraph
bool ControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
{
// Check if the joint name exists
se3::Model::JointIndex jid = m_robot_util->get_id_from_name(name);
pinocchio::Model::JointIndex jid = m_robot_util->get_id_from_name(name);
if (jid<0)
{
SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
std::stringstream ss;
for(se3::Model::JointIndex it=0; it< m_robot_util->m_nbJoints;it++)
for(pinocchio::Model::JointIndex it=0; it< m_robot_util->m_nbJoints;it++)
ss<< m_robot_util->get_name_from_id(it) <<", ";
SEND_MSG("Possible joint names are: "+ss.str(), MSG_TYPE_INFO);
return false;
......
src/device-torque-ctrl.cpp
View file @ 1f511197
......@@ -144,8 +144,8 @@ void DeviceTorqueCtrl::init(const double& dt, const std::string& robotRef)
{
vector<string> package_dirs;
m_robot = new robots::RobotWrapper(urdfFile,
package_dirs, se3::JointModelFreeFlyer());
m_data = new se3::Data(m_robot->model());
package_dirs, pinocchio::JointModelFreeFlyer());
m_data = new pinocchio::Data(m_robot->model());
m_robot->rotor_inertias(rotor_inertias);
m_robot->gear_ratios(gear_ratios);
......@@ -214,14 +214,14 @@ void DeviceTorqueCtrl::setState( const dynamicgraph::Vector& q )
m_robot_util->config_sot_to_urdf(m_q_sot, m_q);
m_robot->computeAllTerms(*m_data, m_q, m_v);
se3::SE3 H_lf = m_robot->position(*m_data,
pinocchio::SE3 H_lf = m_robot->position(*m_data,
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
tsid::trajectories::TrajectorySample s(12, 6);
tsid::math::SE3ToVector(H_lf, s.pos);
m_contactLF->setReference(s);
SEND_MSG("Setting left foot reference to "+toString(H_lf), MSG_TYPE_DEBUG);
se3::SE3 H_rf = m_robot->position(*m_data,
pinocchio::SE3 H_rf = m_robot->position(*m_data,
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
tsid::math::SE3ToVector(H_rf, s.pos);
m_contactRF->setReference(s);
......@@ -337,7 +337,7 @@ void DeviceTorqueCtrl::integrate( const double & dt )
{
computeForwardDynamics();
// integrate
m_q = se3::integrate(m_robot->model(), m_q, dt*m_v);
m_q = pinocchio::integrate(m_robot->model(), m_q, dt*m_v);
m_v += dt*m_dv;
m_robot_util->config_urdf_to_sot(m_q, m_q_sot);
......
src/free-flyer-locator.cpp
View file @ 1f511197
......@@ -33,7 +33,7 @@ namespace dynamicgraph
using namespace dynamicgraph;
using namespace dynamicgraph::command;
using namespace std;
using namespace se3;
using namespace pinocchio;
typedef Eigen::Vector6d Vector6;
......@@ -106,11 +106,11 @@ namespace dynamicgraph
return;
}
m_model = new se3::Model();
m_model = new pinocchio::Model();
m_model->name.assign("EmptyRobot");
se3::urdf::buildModel(m_robot_util->m_urdf_filename,
se3::JointModelFreeFlyer(),*m_model);
pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
pinocchio::JointModelFreeFlyer(),*m_model);
assert(m_model->nv == m_robot_util->m_nbJoints+6);
assert(m_model->existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
assert(m_model->existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
......@@ -127,7 +127,7 @@ namespace dynamicgraph
std::cout << e.what();
return SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
}
m_data = new se3::Data(*m_model);
m_data = new pinocchio::Data(*m_model);
m_initSucceeded = true;
}
......@@ -155,8 +155,8 @@ namespace dynamicgraph
m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
/* Compute kinematic and return q with freeflyer */
se3::forwardKinematics(*m_model, *m_data, m_q_pin, m_v_pin);
se3::framesForwardKinematics(*m_model, *m_data);
pinocchio::forwardKinematics(*m_model, *m_data, m_q_pin, m_v_pin);
pinocchio::framesForwardKinematics(*m_model, *m_data);
return s;
}
......@@ -179,11 +179,11 @@ namespace dynamicgraph
assert(q.size()==m_robot_util->m_nbJoints+6 && "Unexpected size of signal base6d_encoder");
/* Compute kinematic and return q with freeflyer */
const se3::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
const se3::SE3 iMo2(m_data->oMf[m_right_foot_id].inverse());
const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
const pinocchio::SE3 iMo2(m_data->oMf[m_right_foot_id].inverse());
// Average in SE3
const se3::SE3::Vector3 w(0.5*(se3::log3(iMo1.rotation())+se3::log3(iMo2.rotation())));
m_Mff = se3::SE3(se3::exp3(w), 0.5 * (iMo1.translation()+iMo2.translation() ));
const pinocchio::SE3::Vector3 w(0.5*(pinocchio::log3(iMo1.rotation())+pinocchio::log3(iMo2.rotation())));
m_Mff = pinocchio::SE3(pinocchio::exp3(w), 0.5 * (iMo1.translation()+iMo2.translation() ));
// due to distance from ankle to ground
Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
......@@ -212,7 +212,7 @@ namespace dynamicgraph
//just read the data, convert to axis angle
if(s.size()!=6)
s.resize(6);
//~ const se3::SE3 & iMo = m_data->oMi[31].inverse();
//~ const pinocchio::SE3 & iMo = m_data->oMi[31].inverse();
const Eigen::AngleAxisd aa(m_Mff.rotation());
Eigen::Vector6d freeflyer;
freeflyer << m_Mff.translation(), aa.axis() * aa.angle();
......
src/inverse-dynamics-balance-controller.cpp
View file @ 1f511197
......@@ -428,7 +428,7 @@ namespace dynamicgraph
vector<string> package_dirs;
m_robot = new robots::RobotWrapper(m_robot_util->m_urdf_filename,
package_dirs,
se3::JointModelFreeFlyer());
pinocchio::JointModelFreeFlyer());
assert(m_robot->nv()>=6);
m_robot_util->m_nbJoints = m_robot->nv()-6;
......@@ -726,12 +726,12 @@ namespace dynamicgraph
m_firstTime = false;
m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
// m_robot->computeAllTerms(m_invDyn->data(), q, v);
se3::SE3 H_lf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
m_contactLF->setReference(H_lf);
SEND_MSG("Setting left foot reference to "+toString(H_lf), MSG_TYPE_DEBUG);
se3::SE3 H_rf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
m_contactRF->setReference(H_rf);
SEND_MSG("Setting right foot reference to "+toString(H_rf), MSG_TYPE_DEBUG);
......@@ -954,7 +954,7 @@ namespace dynamicgraph
if(s.size()!=2)
s.resize(2);
m_f_des_right_footSOUT(iter);
se3::SE3 H_rf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
if(fabs(m_f_RF(2)>1.0))
{
......@@ -980,7 +980,7 @@ namespace dynamicgraph
if(s.size()!=2)
s.resize(2);
m_f_des_left_footSOUT(iter);
se3::SE3 H_lf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
if(fabs(m_f_LF(2)>1.0))
{
......@@ -1025,7 +1025,7 @@ namespace dynamicgraph
const Vector6 & f_LF = m_f_ref_left_footSIN(iter);
const Vector6 & f_RF = m_f_ref_right_footSIN(iter);
se3::SE3 H_lf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
Vector3 zmp_LF, zmp_RF;
if(fabs(f_LF(2)>1.0))
......@@ -1038,7 +1038,7 @@ namespace dynamicgraph
zmp_LF.setZero();
zmp_LF = H_lf.act(zmp_LF);
se3::SE3 H_rf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
if(fabs(f_RF(2)>1.0))
{
......@@ -1067,7 +1067,7 @@ namespace dynamicgraph
s.resize(2);
const Vector6& f = m_wrench_right_footSIN(iter);
assert(f.size()==6);
se3::SE3 H_rf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_rf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
if(fabs(f(2)>1.0))
{
......@@ -1093,7 +1093,7 @@ namespace dynamicgraph
if(s.size()!=2)
s.resize(2);
const Vector6& f = m_wrench_left_footSIN(iter);
se3::SE3 H_lf = m_robot->position(m_invDyn->data(),
pinocchio::SE3 H_lf = m_robot->position(m_invDyn->data(),
m_robot->model().getJointId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
if(fabs(f(2)>1.0))
{
......@@ -1180,7 +1180,7 @@ namespace dynamicgraph
return s;
}
m_tau_desSOUT(iter);
se3::SE3 oMi;
pinocchio::SE3 oMi;
s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_lf, oMi);
tsid::math::SE3ToVector(oMi, s);
......@@ -1195,7 +1195,7 @@ namespace dynamicgraph
return s;
}
m_tau_desSOUT(iter);
se3::SE3 oMi;
pinocchio::SE3 oMi;
s.resize(12);
m_robot->framePosition(m_invDyn->data(), m_frame_id_rf, oMi);
tsid::math::SE3ToVector(oMi, s);
......@@ -1209,7 +1209,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal left_foot_vel before initialization!");
return s;
}
se3::Motion v;
pinocchio::Motion v;
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_lf, v);
s = v.toVector();
return s;
......@@ -1222,7 +1222,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_vel before initialization!");
return s;
}
se3::Motion v;
pinocchio::Motion v;
m_robot->frameVelocity(m_invDyn->data(), m_frame_id_rf, v);
s = v.toVector();
return s;
......
src/torque-offset-estimator.cpp
View file @ 1f511197
......@@ -114,8 +114,8 @@ namespace torque_control
return;
}
se3::urdf::buildModel(m_robot_util->m_urdf_filename,
se3::JointModelFreeFlyer(), m_model);
pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
pinocchio::JointModelFreeFlyer(), m_model);
// assert(m_model.nq == N_JOINTS+7);
// assert(m_model.nv == N_JOINTS+6);
......@@ -130,7 +130,7 @@ namespace torque_control
SEND_MSG("Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
return;
}
m_data = new se3::Data(m_model);
m_data = new pinocchio::Data(m_model);
m_torso_X_imu.rotation() = m_torso_X_imu_.block<3,3>(0,0);
m_torso_X_imu.translation() = m_torso_X_imu_.block<3,1>(0,3);
gyro_epsilon = gyro_epsilon_;
......@@ -195,9 +195,9 @@ namespace torque_control
//Get the transformation from ff(f) to torso (t) to IMU(i) frame:
// fMi = oMf^-1 * fMt * tMi
se3::forwardKinematics(m_model,*m_data,enc);
//se3::SE3 fMi = m_data->oMi[ffIndex].inverse()*m_data->oMi[torsoIndex]*m_torso_X_imu;
se3::SE3 oMimu = m_data->oMi[torsoIndex]*m_torso_X_imu;
pinocchio::forwardKinematics(m_model,*m_data,enc);
//pinocchio::SE3 fMi = m_data->oMi[ffIndex].inverse()*m_data->oMi[torsoIndex]*m_torso_X_imu;
pinocchio::SE3 oMimu = m_data->oMi[torsoIndex]*m_torso_X_imu;
//Move the IMU signal to the base frame.
//angularAcceleration is zero. Intermediate frame acc and velocities are zero
......@@ -208,7 +208,7 @@ namespace torque_control
//Set fixed for DEBUG
//m_model.gravity.linear() = m_model.gravity981;
const Eigen::VectorXd& tau_rnea = se3::rnea(m_model, *m_data, enc,
const Eigen::VectorXd& tau_rnea = pinocchio::rnea(m_model, *m_data, enc,
Eigen::VectorXd::Zero(m_model.nv),
Eigen::VectorXd::Zero(m_model.nv));
const Eigen::VectorXd current_offset = tau - tau_rnea.tail(m_model.nv-6);
......
Supports Markdown
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