Franka七自由度运动学正解C++eigen

最新推荐文章于 2024-01-17 06:00:00 发布

weixin_42184478

最新推荐文章于 2024-01-17 06:00:00 发布

阅读量1k

点赞数 1

分类专栏： Franka emika 文章标签： c++

本文链接：https://blog.csdn.net/weixin_42184478/article/details/107779047

版权

Franka emika 专栏收录该内容

3 篇文章 1 订阅

订阅专栏

下面展示一些 内联代码片。

#include <iostream>
#include <Eigen/Dense>
#include <math.h>


using namespace Eigen;
using namespace std;




float PI=3.1415926535897932643323846;

//-0.0182128,-0.751325,-0.0162059,-2.33274,0.0310344,1.59808,0.751991 初始


array <float,7> ori_input_thea={0};
array<float,7> ori_theta= {0};





float alpha1 =0; float alpha2 = -PI/2; float alpha3 = PI/2; float alpha4 = PI/2; float alpha5 = -PI/2; float alpha6 = PI/2; float alpha7 =PI/2;
//连杆扭转角常参

float d1 = 0.333;float d2 = 0; float d3 = 0.316;float d4 = 0;float d5 = 0.384;float d6 = 0;float d7 = 0;
//连杆长cm常参

float a1 =0;float a2 =0;float a3 =0; float a4 = 0.0825;float a5 = -0.0825; float a6 = 0.088;float a7=0;
//连杆长cm常参

int main ()
{




  
    for (size_t i = 0; i < ori_input_thea.size(); i++)
    {
        cin >> ori_theta[i] ;
    }
	float theta1 = ori_theta[0]*180/PI;
	float theta2 = ori_theta[1]*180/PI;  
	float theta3 = ori_theta[2]*180/PI;
	float theta4 = ori_theta[3]*180/PI;
	float theta5 = ori_theta[4]*180/PI;
	float theta6 = ori_theta[5]*180/PI;
	float theta7 = ori_theta[6]*180/PI;
		//关节角
    
       
 	Matrix4d T1; 
	T1 <<  cos(theta1)  ,  -sin(theta1)*cos(alpha1)  , sin(theta1)*sin(alpha1) , a1*cos(theta1),
		 sin(theta1) , cos(theta1)*cos(alpha1) , -cos(theta1)*sin(alpha1), a1*sin(theta1),
		  0  ,  sin(alpha1)  ,  cos(alpha1) ,  d1  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T2; 
	T2 <<  cos(theta2)  ,  -sin(theta2)*cos(alpha2)  , sin(theta2)*sin(alpha2) , a2*cos(theta2),
		sin(theta2) , cos(theta2)*cos(alpha2) , -cos(theta2)*sin(alpha2), a2*sin(theta2),
		0  ,  sin(alpha2)  ,  cos(alpha2) ,  d2  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T3; 
	T3 <<  cos(theta3)  ,  -sin(theta3)*cos(alpha3)  , sin(theta3)*sin(alpha3) , a3*cos(theta3),
		sin(theta3) , cos(theta3)*cos(alpha3) , -cos(theta3)*sin(alpha3), a3*sin(theta3),
		0  ,  sin(alpha3)  ,  cos(alpha3) ,  d3  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T4; 
	T4 <<  cos(theta4)  ,  -sin(theta4)*cos(alpha4)  , sin(theta4)*sin(alpha4) , a4*cos(theta4),
		sin(theta4) , cos(theta4)*cos(alpha4) , -cos(theta4)*sin(alpha4), a4*sin(theta4),
		0  ,  sin(alpha4)  ,  cos(alpha4) ,  d4  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T5; 
	T5 <<  cos(theta5)  ,  -sin(theta5)*cos(alpha5)  , sin(theta5)*sin(alpha5) , a5*cos(theta5),
		sin(theta5) , cos(theta5)*cos(alpha5) , -cos(theta5)*sin(alpha5), a5*sin(theta5),
		0  ,  sin(alpha5)  ,  cos(alpha5) ,  d5  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T6; 
	T6 <<  cos(theta6)  ,  -sin(theta6)*cos(alpha6)  , sin(theta6)*sin(alpha6) , a6*cos(theta6),
		sin(theta6) , cos(theta6)*cos(alpha6) , -cos(theta6)*sin(alpha6), a6*sin(theta6),
		0  ,  sin(alpha6)  ,  cos(alpha6) ,  d6  ,
		0 ,        0   ,      0    ,1.0000;
    Matrix4d T7; 
	T7 <<  cos(theta7)  ,  -sin(theta7)*cos(alpha7)  , sin(theta7)*sin(alpha7) , a7*cos(theta7),
		sin(theta7) , cos(theta7)*cos(alpha7) , -cos(theta7)*sin(alpha7), a7*sin(theta7),
		0  ,  sin(alpha7)  ,  cos(alpha7) ,  d7  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T;
	T = T1*T2*T3*T4*T5*T6*T7;


	float nx;float ny; float nz;
	float ox;float oy;float oz;
	float ax;float ay;float az;
	float px;float py;float pz;

	nx = T(0,0);
	ny = T(1,0);
	nz = T(2,0);
	ox = T(0,1);
	oy = T(1,1);
	oz = T(2,1);
	ax = T(0,2);
	ay = T(1,2);
	az = T(2,2);
	px = T(0,3);
	py = T(1,3);
	pz = T(2,3);
	cout<<ori_theta[0]<< "ori_theta1\t"<<ori_theta[1]<<"ori_theta2\t"<<ori_theta[2]<<"ori_theta3\t"<<ori_theta[3]<<"ori_theta4\t"<<ori_theta[4]<<"ori_theta5\t"<<ori_theta[5]<<"ori_theta6\t"<<ori_theta[6]<<"ori_theta7\t"<<endl;
	cout<<theta1<< "theta1\t"<<theta2<<"theta2\t"<<theta3<<"theta3\t"<<theta4<<"theta4\t"<<theta5<<"theta5\t"<<theta6<<"theta6\t"<<theta7<<"theta7\t"<<endl;

    cout<<nx<< "nx\t"<<ny<<"ny\t"<<nz<<"nz\t\n"<<ox<<"ox\t"<<oy<<"oy\t"<<oz<<"oz\t\n"<<ax<<"ax\t"<<ay<<"ay\t"<<az<<"az\t\n"<<px<<"px\t"<<py<<"py\t"<<pz<<"pz\t\n";
}

//-4.37114e-08 :nx 4.37114e-08:ny    -1  :nz 
 //1 ox  -8.74228e-08oy   -4.37114e-08oz 
 //-8.74228e-08ax  -1ay  -4.37114e-08 az
 //53.9px -425py  33.7pz
 //53.9px-425py33.7
 //475.016px-1.45712py35.2063pz

//"theta": [-0.00903131,-0.484955,-0.0795593,-2.84765,-0.0443225,2.38366,0.766914]
 //475px3.9py33.7pz

// 0.00821319,-0.67407,-0.03377,-1.97815,-0.0443129,1.47291,0.828575
//  475px3.9py33.7pz

// {
//   "O_T_EE": [0.998578,0.0328747,-0.0417381,0,0.0335224,-0.999317,0.0149157,0,-0.04122,-0.016294,
//              -0.999017,0,0.305468,-0.00814133,0.483198,1],
//   "O_T_EE_d": [0.998582,0.0329548,-0.041575,0,0.0336027,-0.999313,0.0149824,0,-0.0410535,
//                -0.0163585,-0.999023,0,0.305444,-0.00810967,0.483251,1],
//   "F_T_EE": [0.7071,-0.7071,0,0,0.7071,0.7071,0,0,0,0,1,0,0,0,0.1034,1],   18wei
//   "EE_T_K": [1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],
//   "m_ee": 0.73, "F_x_Cee": [-0.01,0,0.03], "I_ee": [0.001,0,0,0,0.0025,0,0,0,0.0017],
//   "m_load": 0, "F_x_Cload": [0,0,0], "I_load": [0,0,0,0,0,0,0,0,0],
//   "m_total": 0.73, "F_x_Ctotal": [-0.01,0,0.03], "I_total": [0.001,0,0,0,0.0025,0,0,0,0.0017],
//   "elbow": [-0.0207622,-1], "elbow_d": [-0.0206678,-1],
//   "tau_J": [-0.00359774,-5.08582,0.105732,21.8135,0.63253,2.18121,-0.0481953],
//   "tau_J_d": [0,0,0,0,0,0,0],
//   "dtau_J": [-54.0161,-18.9808,-64.6899,-64.2609,14.1561,28.5654,-11.1858],
//   "q": [0.0167305,-0.762614,-0.0207622,-2.34352,-0.0305686,1.53975,0.753872],
//   "dq": [0.00785939,0.00189343,0.00932415,0.0135431,-0.00220327,-0.00492024,0.00213604],
//   "q_d": [0.0167347,-0.762775,-0.0206678,-2.34352,-0.0305677,1.53975,0.753862],
//   "dq_d": [0,0,0,0,0,0,0],
//   "joint_contact": [0,0,0,0,0,0,0], "cartesian_contact": [0,0,0,0,0,0],
//   "joint_collision": [0,0,0,0,0,0,0], "cartesian_collision": [0,0,0,0,0,0],
//   "tau_ext_hat_filtered": [0.00187271,-0.700316,0.386035,0.0914781,-0.117258,-0.00667777,
//                            -0.0252562],
//   "O_F_ext_hat_K": [-2.06065,0.45889,-0.150951,-0.482791,-1.39347,0.109695],
//   "K_F_ext_hat_K": [-2.03638,-0.529916,0.228266,-0.275938,0.434583,0.0317351],
//   "theta": [0.01673,-0.763341,-0.0207471,-2.34041,-0.0304783,1.54006,0.753865],
//   "dtheta": [0,0,0,0,0,0,0],
//   "current_errors": [], "last_motion_errors": [],
//   "control_command_success_rate": 0, "robot_mode": "Idle", "time": 3781435
// }

自己参照别人的写了一个，注意Franka的原点坐标位于竖直姿态的最顶端。

/**
   * \f${^OdP_{EE}}_{d}\f$
   * Desired end effector twist in base frame.
   * Unit: \f$[\frac{m}{s},\frac{m}{s},\frac{m}{s},\frac{rad}{s},\frac{rad}{s},\frac{rad}{s}]\f$.
   */
  std::array<double, 6> O_dP_EE_d{};  // NOLINT(readability-identifier-naming)