Commit 1f511197 by Julian Viereck Committed by Andrea Del Prete

### Convert to pinocchio2 namespace (#54)

parent 809abf65
 ... ... @@ -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 m_J_RF_QR; ... ...
 ... ... @@ -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 ... ...
 ... ... @@ -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; ... ...
 ... ... @@ -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 ... ...
 ... ... @@ -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 m_J_RF_QR; ... ...
 ... ... @@ -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; ... ...
 ... ... @@ -35,6 +35,7 @@ /* --------------------------------------------------------------------- */ /* --- INCLUDE --------------------------------------------------------- */ /* --------------------------------------------------------------------- */ #include #include #include ... ...
 ... ... @@ -155,8 +155,8 @@ namespace dynamicgraph vector 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; ... ...
 ... ... @@ -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); ... ...
 ... ... @@ -15,6 +15,7 @@ */ #include #include #include #include #include ... ...
 ... ... @@ -190,7 +190,7 @@ namespace dynamicgraph m_emergency_stop_triggered = false; m_initSucceeded = true; vector 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; ... ...
 ... ... @@ -144,8 +144,8 @@ void DeviceTorqueCtrl::init(const double& dt, const std::string& robotRef) { vector 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); ... ...
 ... ... @@ -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 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(); ... ...
 ... ... @@ -428,7 +428,7 @@ namespace dynamicgraph vector 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; ... ...
 ... ... @@ -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!