feat:Testing the ViSP demo in MuJoCo.
This commit is contained in:
parent
18ed229e92
commit
61bbd33be7
@ -152,12 +152,12 @@ protected:
|
||||
q_home_ = { 0.25, 1.00, M_PI/2 - 0.2, M_PI/2 - 0.2, -M_PI + 0.5, 0.0, 0.0 };
|
||||
|
||||
// 5) 坐标轴对齐:site相机系 -> ViSP相机系
|
||||
// 你现在截图表现是“画面左上”,但 t_co.x,y 是正,说明 x/y 反了
|
||||
// 先试这个:翻转 x,y
|
||||
// ViSP(+X 右、+Y 下、+Z 前)
|
||||
// R_CAM_SITE +X 右、+Y 上、+Z 后
|
||||
R_cv_ = (Eigen::Matrix3d() <<
|
||||
-1, 0, 0,
|
||||
1, 0, 0,
|
||||
0,-1, 0,
|
||||
0, 0, 1).finished();
|
||||
0, 0, -1).finished();
|
||||
|
||||
// 6) ViSP IBVS 参数
|
||||
tag_half_ = 0.06; // 12cm tag -> half 6cm
|
||||
@ -176,7 +176,10 @@ protected:
|
||||
// desired: 正对 + 距离 Z_des
|
||||
{
|
||||
vpTranslationVector t_des(0.0, 0.0, Z_des_);
|
||||
vpRotationMatrix R_des; R_des.eye();
|
||||
|
||||
// tag 在(相机坐标系visp 相机)下的朝向
|
||||
vpRotationMatrix R_des;
|
||||
R_des.buildFrom(M_PI, 0, 0);
|
||||
vpHomogeneousMatrix cMo_des(t_des, R_des);
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
@ -240,6 +243,31 @@ protected:
|
||||
Eigen::Vector3d p_cw(pc[0], pc[1], pc[2]);
|
||||
Eigen::Matrix3d R_cw = xmat_to_R(Rc9); // site(cam)->world
|
||||
Eigen::Matrix3d R_wc = R_cw.transpose(); // world->site(cam)
|
||||
if ((step_count_ % 120) == 0) {
|
||||
const Eigen::Vector3d x_w = R_cw.col(0);
|
||||
const Eigen::Vector3d y_w = R_cw.col(1);
|
||||
const Eigen::Vector3d z_w = R_cw.col(2);
|
||||
std::printf("[IBVS] site axes(world): x=[%.3f %.3f %.3f] y=[%.3f %.3f %.3f] z=[%.3f %.3f %.3f]\n",
|
||||
x_w.x(), x_w.y(), x_w.z(),
|
||||
y_w.x(), y_w.y(), y_w.z(),
|
||||
z_w.x(), z_w.y(), z_w.z());
|
||||
|
||||
const int cam_id = mj_name2id(m, mjOBJ_CAMERA, "hand_cam");
|
||||
if (cam_id >= 0) {
|
||||
const mjtNum* Rc9_cam = d->cam_xmat + 9 * cam_id;
|
||||
Eigen::Matrix3d R_cam_w = xmat_to_R(Rc9_cam); // cam->world
|
||||
Eigen::Matrix3d R_rel = R_cam_w.transpose() * R_cw; // cam <- site
|
||||
std::printf("[IBVS] cam<-site:\n"
|
||||
"%.3f %.3f %.3f\n"
|
||||
"%.3f %.3f %.3f\n"
|
||||
"%.3f %.3f %.3f\n",
|
||||
R_rel(0,0), R_rel(0,1), R_rel(0,2),
|
||||
R_rel(1,0), R_rel(1,1), R_rel(1,2),
|
||||
R_rel(2,0), R_rel(2,1), R_rel(2,2));
|
||||
} else {
|
||||
std::printf("[IBVS] cam<-site: hand_cam not found\n");
|
||||
}
|
||||
}
|
||||
|
||||
// 2) tag_board body pose in world
|
||||
const mjtNum* po = d->xpos + 3 * tag_body_id_;
|
||||
@ -255,6 +283,15 @@ protected:
|
||||
Eigen::Matrix3d R_co = R_cv_ * R_co_site;
|
||||
Eigen::Vector3d t_co = R_cv_ * t_co_site;
|
||||
|
||||
if ((step_count_ % 120) == 0) {
|
||||
const Eigen::Vector3d z_site = R_co_site.col(2); // tag +Z in site(cam) frame
|
||||
const Eigen::Vector3d z_visp = R_co.col(2); // tag +Z in ViSP frame
|
||||
std::printf("[IBVS] tag +Z (site) = [%.3f %.3f %.3f]\n",
|
||||
z_site.x(), z_site.y(), z_site.z());
|
||||
std::printf("[IBVS] tag +Z (visp) = [%.3f %.3f %.3f]\n",
|
||||
z_visp.x(), z_visp.y(), z_visp.z());
|
||||
}
|
||||
|
||||
if ((step_count_ % 60) == 0) {
|
||||
std::printf("[IBVS] t_co = [%.3f %.3f %.3f]\n", t_co.x(), t_co.y(), t_co.z());
|
||||
}
|
||||
@ -481,15 +518,15 @@ protected:
|
||||
// 6) 坐标对齐(site相机系 -> ViSP相机系)
|
||||
// 这就是你之前验证过能收敛的那套:x/y 同时翻转
|
||||
R_cv_ = (Eigen::Matrix3d() <<
|
||||
-1, 0, 0,
|
||||
1, 0, 0,
|
||||
0,-1, 0,
|
||||
0, 0, 1).finished();
|
||||
0, 0, -1).finished();
|
||||
|
||||
// 7) IBVS 任务:用 AprilTag 的 4 个角点特征
|
||||
tag_size_m_ = 0.12; // 12cm
|
||||
tag_half_ = tag_size_m_ * 0.5;
|
||||
Z_des_ = 0.60;
|
||||
lambda_ = 0.7;
|
||||
Z_des_ = 0.28;
|
||||
lambda_ = 1.7;
|
||||
|
||||
task_.setServo(vpServo::EYEINHAND_CAMERA);
|
||||
task_.setInteractionMatrixType(vpServo::CURRENT);
|
||||
@ -504,7 +541,9 @@ protected:
|
||||
// desired: 正对 + 距离 Z_des
|
||||
{
|
||||
vpTranslationVector t_des(0.0, 0.0, Z_des_);
|
||||
vpRotationMatrix R_des; R_des.eye();
|
||||
// tag 在(相机坐标系visp 相机)下的朝向
|
||||
vpRotationMatrix R_des;
|
||||
R_des.buildFrom(M_PI, 0, 0);
|
||||
vpHomogeneousMatrix cMo_des(t_des, R_des);
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
|
||||
@ -751,7 +751,7 @@ TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) {
|
||||
solver.fk(joint_angles, target_pose, true);
|
||||
|
||||
// 直线插补:从 target_pose 出发沿 X 方向 L 米,共 N 段
|
||||
const int N = 200;
|
||||
const int N = 100;
|
||||
const double L = 0.40;
|
||||
Eigen::Vector3d dir = Eigen::Vector3d::UnitZ();
|
||||
dir.normalize();
|
||||
|
||||
@ -1,203 +1,103 @@
|
||||
psi,q1,q2,q3,q4,q5,q6,q7
|
||||
0.000000000,0.293251835,0.758274585,1.296200413,1.570796327,-1.328603785,0.029738090,-0.243184799
|
||||
0.000000000,0.250086584,0.987999944,1.558700421,1.570796327,-1.558796269,0.000072292,-0.012048253
|
||||
0.000000000,0.253465442,1.040302054,1.613445935,1.575333670,-1.611189364,-0.005729065,0.044670600
|
||||
0.000000000,0.258397998,1.073557197,1.648079069,1.579806848,-1.644698758,-0.010356407,0.082276145
|
||||
0.000000000,0.263491802,1.096352790,1.672068598,1.584215943,-1.667852826,-0.014921251,0.109390999
|
||||
0.000000000,0.268378107,1.112398797,1.689344560,1.588561031,-1.684328531,-0.019732652,0.129738884
|
||||
0.000000000,0.272967220,1.123752893,1.702024567,1.592842183,-1.696162824,-0.024866749,0.145384712
|
||||
0.000000000,0.277266039,1.131718414,1.711421556,1.597059459,-1.704645392,-0.030315409,0.157636178
|
||||
0.000000000,0.281315722,1.137186223,1.718418668,1.601212916,-1.710658033,-0.036039713,0.167386979
|
||||
0.000000000,0.285166582,1.140792607,1.723638871,1.605302602,-1.714831096,-0.041992388,0.175275336
|
||||
0.000000000,0.288867440,1.143003561,1.727533812,1.609328561,-1.717626814,-0.048127614,0.181768608
|
||||
0.000000000,0.292461383,1.144165209,1.730436057,1.613290830,-1.719389056,-0.054404990,0.187213917
|
||||
0.000000000,0.295984556,1.144536657,1.732592620,1.617189440,-1.720375699,-0.060790700,0.191871115
|
||||
0.000000000,0.299466337,1.144312696,1.734187838,1.621024417,-1.720780990,-0.067257329,0.195935563
|
||||
0.000000000,0.302930112,1.143640102,1.735359606,1.624795783,-1.720751596,-0.073783125,0.199554486
|
||||
0.000000000,0.306394230,1.142629597,1.736211195,1.628503554,-1.720398402,-0.080351076,0.202838959
|
||||
0.000000000,0.309872939,1.141364756,1.736819969,1.632147741,-1.719805291,-0.086948016,0.205872840
|
||||
0.000000000,0.313377213,1.139908675,1.737243867,1.635728351,-1.719035739,-0.093563822,0.208719454
|
||||
0.000000000,0.316915457,1.138308996,1.737526257,1.639245386,-1.718137787,-0.100190749,0.211426623
|
||||
0.000000000,0.320494077,1.136601686,1.737699559,1.642698847,-1.717147791,-0.106822881,0.214030459
|
||||
0.000000000,0.324117935,1.134813899,1.737787974,1.646088726,-1.716093263,-0.113455706,0.216558228
|
||||
0.000000000,0.327790708,1.132966132,1.737809530,1.649415017,-1.714995021,-0.120085775,0.219030511
|
||||
0.000000000,0.331515163,1.131073854,1.737777615,1.652677706,-1.713868808,-0.126710442,0.221462840
|
||||
0.000000000,0.335293376,1.129148738,1.737702136,1.655876779,-1.712726534,-0.133327671,0.223866927
|
||||
0.000000000,0.339126896,1.127199592,1.737590378,1.659012217,-1.711577198,-0.139935878,0.226251600
|
||||
0.000000000,0.343016867,1.125233058,1.737447665,1.662083998,-1.710427599,-0.146533817,0.228623506
|
||||
0.000000000,0.346964128,1.123254143,1.737277841,1.665092100,-1.709282869,-0.153120499,0.230987644
|
||||
0.000000000,0.350969285,1.121266623,1.737083643,1.668036495,-1.708146877,-0.159695121,0.233347764
|
||||
0.000000000,0.355032765,1.119273341,1.736866975,1.670917156,-1.707022534,-0.166257017,0.235706676
|
||||
0.000000000,0.359154859,1.117276440,1.736629116,1.673734051,-1.705912028,-0.172805626,0.238066471
|
||||
0.000000000,0.363335755,1.115277530,1.736370876,1.676487149,-1.704816995,-0.179340458,0.240428704
|
||||
0.000000000,0.367575558,1.113277822,1.736092714,1.679176414,-1.703738654,-0.185861080,0.242794517
|
||||
0.000000000,0.371874312,1.111278225,1.735794825,1.681801811,-1.702677907,-0.192367094,0.245164741
|
||||
0.000000000,0.376232012,1.109279422,1.735477205,1.684363303,-1.701635416,-0.198858131,0.247539970
|
||||
0.000000000,0.380648616,1.107281921,1.735139706,1.686860851,-1.700611658,-0.205333838,0.249920618
|
||||
0.000000000,0.385124048,1.105286104,1.734782066,1.689294415,-1.699606968,-0.211793876,0.252306963
|
||||
0.000000000,0.389658210,1.103292254,1.734403944,1.691663955,-1.698621577,-0.218237911,0.254699172
|
||||
0.000000000,0.394250983,1.101300579,1.734004936,1.693969430,-1.697655628,-0.224665612,0.257097336
|
||||
0.000000000,0.398902232,1.099311236,1.733584594,1.696210796,-1.696709203,-0.231076652,0.259501477
|
||||
0.000000000,0.403611805,1.097324336,1.733142437,1.698388013,-1.695782333,-0.237470699,0.261911573
|
||||
0.000000000,0.408379539,1.095339961,1.732677959,1.700501036,-1.694875007,-0.243847421,0.264327558
|
||||
0.000000000,0.413205259,1.093358170,1.732190637,1.702549822,-1.693987182,-0.250206482,0.266749336
|
||||
0.000000000,0.418088782,1.091379001,1.731679938,1.704534328,-1.693118789,-0.256547541,0.269176785
|
||||
0.000000000,0.423029913,1.089402483,1.731145317,1.706454512,-1.692269738,-0.262870253,0.271609762
|
||||
0.000000000,0.428028450,1.087428633,1.730586226,1.708310329,-1.691439917,-0.269174269,0.274048106
|
||||
0.000000000,0.433084180,1.085457461,1.730002114,1.710101737,-1.690629200,-0.275459235,0.276491639
|
||||
0.000000000,0.438196886,1.083488969,1.729392427,1.711828693,-1.689837443,-0.281724790,0.278940169
|
||||
0.000000000,0.443366340,1.081523160,1.728756612,1.713491156,-1.689064490,-0.287970569,0.281393494
|
||||
0.000000000,0.448592309,1.079560029,1.728094116,1.715089084,-1.688310171,-0.294196203,0.283851396
|
||||
0.000000000,0.453874551,1.077599570,1.727404387,1.716622436,-1.687574303,-0.300401317,0.286313650
|
||||
0.000000000,0.459212817,1.075641776,1.726686877,1.718091173,-1.686856690,-0.306585531,0.288780018
|
||||
0.000000000,0.464606853,1.073686638,1.725941037,1.719495256,-1.686157123,-0.312748460,0.291250252
|
||||
0.000000000,0.470056396,1.071734146,1.725166323,1.720834646,-1.685475382,-0.318889717,0.293724094
|
||||
0.000000000,0.475561177,1.069784288,1.724362193,1.722109308,-1.684811232,-0.325008908,0.296201277
|
||||
0.000000000,0.481120921,1.067837053,1.723528109,1.723319205,-1.684164426,-0.331105636,0.298681522
|
||||
0.000000000,0.486735346,1.065892426,1.722663535,1.724464304,-1.683534705,-0.337179500,0.301164541
|
||||
0.000000000,0.492404163,1.063950395,1.721767939,1.725544572,-1.682921792,-0.343230095,0.303650037
|
||||
0.000000000,0.498127079,1.062010945,1.720840791,1.726559977,-1.682325401,-0.349257014,0.306137702
|
||||
0.000000000,0.503903791,1.060074059,1.719881567,1.727510489,-1.681745228,-0.355259846,0.308627215
|
||||
0.000000000,0.509733994,1.058139722,1.718889742,1.728396080,-1.681180955,-0.361238176,0.311118249
|
||||
0.000000000,0.515617376,1.056207915,1.717864799,1.729216724,-1.680632250,-0.367191588,0.313610462
|
||||
0.000000000,0.521553619,1.054278621,1.716806222,1.729972396,-1.680098762,-0.373119663,0.316103506
|
||||
0.000000000,0.527542398,1.052351818,1.715713497,1.730663071,-1.679580127,-0.379021979,0.318597016
|
||||
0.000000000,0.533583386,1.050427487,1.714586114,1.731288730,-1.679075963,-0.384898113,0.321090621
|
||||
0.000000000,0.539676249,1.048505603,1.713423568,1.731849352,-1.678585870,-0.390747642,0.323583936
|
||||
0.000000000,0.545820648,1.046586143,1.712225354,1.732344920,-1.678109430,-0.396570137,0.326076563
|
||||
0.000000000,0.552016241,1.044669081,1.710990971,1.732775419,-1.677646210,-0.402365173,0.328568096
|
||||
0.000000000,0.558262679,1.042754388,1.709719921,1.733140834,-1.677195755,-0.408132322,0.331058112
|
||||
0.000000000,0.564559610,1.040842035,1.708411708,1.733441154,-1.676757590,-0.413871153,0.333546180
|
||||
0.000000000,0.570906679,1.038931990,1.707065838,1.733676370,-1.676331224,-0.419581239,0.336031853
|
||||
0.000000000,0.577303526,1.037024216,1.705681819,1.733846474,-1.675916142,-0.425262150,0.338514673
|
||||
0.000000000,0.583749788,1.035118678,1.704259162,1.733951461,-1.675511810,-0.430913457,0.340994167
|
||||
0.000000000,0.590245100,1.033215335,1.702797378,1.733991327,-1.675117672,-0.436534731,0.343469851
|
||||
0.000000000,0.596789092,1.031314144,1.701295980,1.733966070,-1.674733150,-0.442125546,0.345941224
|
||||
0.000000000,0.603381392,1.029415059,1.699754483,1.733875692,-1.674357644,-0.447685473,0.348407774
|
||||
0.000000000,0.610021627,1.027518030,1.698172402,1.733720196,-1.673990530,-0.453214087,0.350868972
|
||||
0.000000000,0.616709421,1.025623002,1.696549251,1.733499587,-1.673631159,-0.458710964,0.353324277
|
||||
0.000000000,0.623444397,1.023729920,1.694884547,1.733213871,-1.673278861,-0.464175682,0.355773130
|
||||
0.000000000,0.630226176,1.021838722,1.693177803,1.732863058,-1.672932937,-0.469607818,0.358214958
|
||||
0.000000000,0.637054378,1.019949341,1.691428536,1.732447159,-1.672592666,-0.475006956,0.360649172
|
||||
0.000000000,0.643928624,1.018061707,1.689636257,1.731966187,-1.672257296,-0.480372679,0.363075168
|
||||
0.000000000,0.650848533,1.016175745,1.687800477,1.731420157,-1.671926053,-0.485704572,0.365492325
|
||||
0.000000000,0.657813725,1.014291372,1.685920707,1.730809087,-1.671598131,-0.491002226,0.367900002
|
||||
0.000000000,0.664823821,1.012408503,1.683996453,1.730132995,-1.671272698,-0.496265233,0.370297546
|
||||
0.000000000,0.671878442,1.010527046,1.682027217,1.729391903,-1.670948890,-0.501493188,0.372684283
|
||||
0.000000000,0.678977213,1.008646900,1.680012499,1.728585833,-1.670625816,-0.506685690,0.375059520
|
||||
0.000000000,0.686119757,1.006767961,1.677951794,1.727714809,-1.670302551,-0.511842342,0.377422548
|
||||
0.000000000,0.693305702,1.004890117,1.675844591,1.726778859,-1.669978140,-0.516962750,0.379772637
|
||||
0.000000000,0.700534679,1.003013246,1.673690374,1.725778009,-1.669651594,-0.522046523,0.382109038
|
||||
0.000000000,0.707806321,1.001137222,1.671488618,1.724712290,-1.669321890,-0.527093278,0.384430981
|
||||
0.000000000,0.715120267,0.999261908,1.669238794,1.723581732,-1.668987973,-0.532102631,0.386737677
|
||||
0.000000000,0.722476156,0.997387159,1.666940361,1.722386368,-1.668648748,-0.537074206,0.389028314
|
||||
0.000000000,0.729873638,0.995512821,1.664592770,1.721126232,-1.668303085,-0.542007631,0.391302058
|
||||
0.000000000,0.737312363,0.993638730,1.662195462,1.719801360,-1.667949817,-0.546902538,0.393558053
|
||||
0.000000000,0.744791990,0.991764712,1.659747866,1.718411787,-1.667587735,-0.551758562,0.395795419
|
||||
0.000000000,0.752312185,0.989890582,1.657249397,1.716957551,-1.667215592,-0.556575347,0.398013252
|
||||
0.000000000,0.759872618,0.988016143,1.654699459,1.715438691,-1.666832097,-0.561352538,0.400210624
|
||||
0.000000000,0.767472971,0.986141187,1.652097439,1.713855246,-1.666435916,-0.566089787,0.402386580
|
||||
0.000000000,0.775112931,0.984265493,1.649442706,1.712207257,-1.666025671,-0.570786749,0.404540139
|
||||
0.000000000,0.782792198,0.982388824,1.646734615,1.710494765,-1.665599935,-0.575443087,0.406670291
|
||||
0.000000000,0.790510478,0.980510933,1.643972498,1.708717813,-1.665157236,-0.580058465,0.408776000
|
||||
0.000000000,0.798267490,0.978631555,1.641155667,1.706876441,-1.664696048,-0.584632556,0.410856197
|
||||
0.000000000,0.806062964,0.976750410,1.638283411,1.704970694,-1.664214794,-0.589165036,0.412909785
|
||||
0.000000000,0.813896643,0.974867201,1.635354995,1.703000613,-1.663711845,-0.593655584,0.414935633
|
||||
0.000000000,0.821768283,0.972981614,1.632369657,1.700966244,-1.663185512,-0.598103886,0.416932576
|
||||
0.000000000,0.829677653,0.971093315,1.629326604,1.698867628,-1.662634049,-0.602509633,0.418899417
|
||||
0.000000000,0.837624538,0.969201951,1.626225016,1.696704808,-1.662055645,-0.606872518,0.420834919
|
||||
0.000000000,0.845608739,0.967307147,1.623064036,1.694477829,-1.661448429,-0.611192241,0.422737809
|
||||
0.000000000,0.853630074,0.965408506,1.619842771,1.692186733,-1.660810459,-0.615468505,0.424606772
|
||||
0.000000000,0.861688380,0.963505608,1.616560291,1.689831562,-1.660139722,-0.619701016,0.426440453
|
||||
0.000000000,0.869783513,0.961598007,1.613215622,1.687412357,-1.659434133,-0.623889484,0.428237452
|
||||
0.000000000,0.877915350,0.959685229,1.609807745,1.684929159,-1.658691525,-0.628033622,0.429996323
|
||||
0.000000000,0.886083789,0.957766774,1.606335594,1.682382009,-1.657909651,-0.632133148,0.431715569
|
||||
0.000000000,0.894288754,0.955842109,1.602798047,1.679770945,-1.657086176,-0.636187780,0.433393644
|
||||
0.000000000,0.902530191,0.953910670,1.599193929,1.677096004,-1.656218672,-0.640197239,0.435028947
|
||||
0.000000000,0.910808076,0.951971856,1.595522001,1.674357224,-1.655304614,-0.644161247,0.436619816
|
||||
0.000000000,0.919122412,0.950025033,1.591780959,1.671554638,-1.654341374,-0.648079528,0.438164532
|
||||
0.000000000,0.927473233,0.948069523,1.587969429,1.668688279,-1.653326213,-0.651951807,0.439661307
|
||||
0.000000000,0.935860606,0.946104607,1.584085957,1.665758179,-1.652256275,-0.655777805,0.441108284
|
||||
0.000000000,0.944284633,0.944129520,1.580129010,1.662764367,-1.651128580,-0.659557247,0.442503533
|
||||
0.000000000,0.952745454,0.942143448,1.576096961,1.659706869,-1.649940014,-0.663289852,0.443845044
|
||||
0.000000000,0.961243247,0.940145523,1.571988088,1.656585710,-1.648687320,-0.666975338,0.445130720
|
||||
0.000000000,0.969778238,0.938134820,1.567800562,1.653400911,-1.647367088,-0.670613419,0.446358373
|
||||
0.000000000,0.978350695,0.936110351,1.563532441,1.650152492,-1.645975743,-0.674203804,0.447525717
|
||||
0.000000000,0.986960938,0.934071063,1.559181656,1.646840468,-1.644509532,-0.677746194,0.448630359
|
||||
0.000000000,0.995609343,0.932015826,1.554746004,1.643464853,-1.642964511,-0.681240283,0.449669791
|
||||
0.000000000,1.004296342,0.929943435,1.550223134,1.640025655,-1.641336528,-0.684685757,0.450641378
|
||||
0.000000000,1.013022433,0.927852594,1.545610532,1.636522881,-1.639621206,-0.688082289,0.451542352
|
||||
0.000000000,1.021788182,0.925741916,1.540905507,1.632956534,-1.637813924,-0.691429538,0.452369793
|
||||
0.000000000,1.030594229,0.923609907,1.536105177,1.629326610,-1.635909791,-0.694727148,0.453120623
|
||||
0.000000000,1.039441301,0.921454961,1.531206443,1.625633104,-1.633903626,-0.697974744,0.453791584
|
||||
0.000000000,1.048330209,0.919275346,1.526205974,1.621876006,-1.631789926,-0.701171930,0.454379224
|
||||
0.000000000,1.057261868,0.917069191,1.521100179,1.618055301,-1.629562837,-0.704318286,0.454879874
|
||||
0.000000000,1.066237300,0.914834471,1.515885179,1.614170968,-1.627216115,-0.707413362,0.455289630
|
||||
0.000000000,1.075257647,0.912568990,1.510556779,1.610222983,-1.624743085,-0.710456674,0.455604322
|
||||
0.000000000,1.084324186,0.910270364,1.505110427,1.606211315,-1.622136594,-0.713447701,0.455819487
|
||||
0.000000000,1.093438342,0.907935992,1.499541176,1.602135929,-1.619388954,-0.716385877,0.455930332
|
||||
0.000000000,1.102601708,0.905563038,1.493843636,1.597996782,-1.616491882,-0.719270586,0.455931699
|
||||
0.000000000,1.111816060,0.903148392,1.488011914,1.593793827,-1.613436423,-0.722101152,0.455818010
|
||||
0.000000000,1.121083389,0.900688640,1.482039553,1.589527011,-1.610212862,-0.724876830,0.455583223
|
||||
0.000000000,1.130405921,0.898180021,1.475919457,1.585196271,-1.606810629,-0.727596795,0.455220757
|
||||
0.000000000,1.139786156,0.895618372,1.469643793,1.580801542,-1.603218169,-0.730260130,0.454723423
|
||||
0.000000000,1.149226904,0.892999077,1.463203891,1.576342747,-1.599422809,-0.732865809,0.454083332
|
||||
0.000000000,1.158731334,0.890316984,1.456590112,1.571819806,-1.595410579,-0.735412677,0.453291783
|
||||
0.000000000,1.168303029,0.887566327,1.449791692,1.567232629,-1.591166005,-0.737899431,0.452339133
|
||||
0.000000000,1.177946062,0.884740615,1.442796549,1.562581117,-1.586671860,-0.740324585,0.451214636
|
||||
0.000000000,1.187665076,0.881832499,1.435591055,1.557865165,-1.581908848,-0.742686439,0.449906241
|
||||
0.000000000,1.197465393,0.878833611,1.428159739,1.553084658,-1.576855214,-0.744983037,0.448400345
|
||||
0.000000000,1.207353153,0.875734349,1.420484920,1.548239473,-1.571486261,-0.747212107,0.446681481
|
||||
0.000000000,1.217335483,0.872523617,1.412546243,1.543329478,-1.565773716,-0.749370990,0.444731914
|
||||
0.000000000,1.227420723,0.869188475,1.404320064,1.538354529,-1.559684927,-0.751456552,0.442531121
|
||||
0.000000000,1.237618725,0.865713677,1.395778654,1.533314474,-1.553181793,-0.753465057,0.440055103
|
||||
0.000000000,1.247941245,0.862081056,1.386889125,1.528209152,-1.546219326,-0.755391999,0.437275462
|
||||
0.000000000,1.258402492,0.858268666,1.377611942,1.523038388,-1.538743676,-0.757231883,0.434158118
|
||||
0.000000000,1.269019899,0.854249568,1.367898848,1.517801999,-1.530689344,-0.758977892,0.430661511
|
||||
0.000000000,1.279815239,0.849990059,1.357689810,1.512499789,-1.521975111,-0.760621426,0.426733960
|
||||
0.000000000,1.290816318,0.845446969,1.346908432,1.507131550,-1.512497877,-0.762151383,0.422309661
|
||||
0.000000000,1.302059644,0.840563355,1.335454644,1.501697063,-1.502122866,-0.763553042,0.417302313
|
||||
0.000000000,1.313594912,0.835261193,1.323192416,1.496196095,-1.490667075,-0.764806167,0.411594330
|
||||
0.000000000,1.325493139,0.829427934,1.309927300,1.490628400,-1.477868981,-0.765881591,0.405017043
|
||||
0.000000000,1.337863158,0.822888802,1.295360576,1.484993719,-1.463326363,-0.766734289,0.397309980
|
||||
0.000000000,1.350891195,0.815338842,1.278978005,1.479291780,-1.446344625,-0.767286876,0.388021250
|
||||
0.000000000,1.364969539,0.806115270,1.259682427,1.473522294,-1.425431514,-0.767376526,0.376174637
|
||||
0.000000000,1.381782668,0.792167888,1.232596031,1.467684957,-1.393828564,-0.766318552,0.357310941
|
||||
0.000000000,1.496738857,0.655528665,0.975714777,1.461779452,-1.087276611,-0.717411730,0.161215074
|
||||
0.000000000,1.619580657,0.580405088,0.764712829,1.455805443,-0.903872998,-0.663906662,0.054054904
|
||||
0.000000000,1.733786448,0.538744420,0.590838219,1.449762579,-0.784342201,-0.618505256,-0.008826976
|
||||
0.000000000,1.831734308,0.516491419,0.451126452,1.443650491,-0.702295232,-0.582619763,-0.047403632
|
||||
0.000000000,1.911675949,0.505553397,0.341647185,1.437468791,-0.644153540,-0.555178245,-0.071601448
|
||||
0.000000000,2.246475828,0.492926835,-0.131130549,1.431217073,-0.419960497,-0.421321518,-0.158303642
|
||||
0.000000000,2.439316930,0.522562260,-0.406637361,1.424894913,-0.295781460,-0.334495533,-0.190080003
|
||||
0.000000000,2.553976572,0.557693680,-0.577991582,1.418501865,-0.213259530,-0.273712152,-0.203041755
|
||||
0.000000000,2.626018321,0.589846442,-0.690935647,1.412037462,-0.154082994,-0.229820806,-0.207514823
|
||||
0.000000000,2.673594313,0.617314616,-0.768699779,1.405501217,-0.109886900,-0.197687229,-0.207677559
|
||||
0.000000000,2.706292962,0.640256583,-0.823903302,1.398892620,-0.076035941,-0.174051034,-0.205514034
|
||||
0.000000000,2.729484722,0.659319135,-0.863922595,1.392211136,-0.049640088,-0.156690461,-0.202051728
|
||||
0.000000000,2.746350478,0.675211234,-0.893349483,1.385456207,-0.028757948,-0.144027064,-0.197849696
|
||||
0.000000000,2.758866194,0.688571143,-0.915188277,1.378627251,-0.012022597,-0.134911138,-0.193220847
|
||||
0.000000000,2.768308511,0.699932947,-0.931480566,1.371723658,0.001557053,-0.128492594,-0.188343034
|
||||
0.000000000,2.775529054,0.709727970,-0.943653570,1.364744791,0.012712751,-0.124137113,-0.183318016
|
||||
0.000000000,2.781110816,0.718297665,-0.952726138,1.357689984,0.021990824,-0.121368696,-0.178203995
|
||||
0.000000000,2.785461299,0.725909037,-0.959436895,1.350558545,0.029801931,-0.119828888,-0.173034015
|
||||
0.000000000,2.788870113,0.732769189,-0.964327376,1.343349746,0.036455968,-0.119247147,-0.167826546
|
||||
0.000000000,2.791545818,0.739037859,-0.967797836,1.336062830,0.042187247,-0.119419018,-0.162591622
|
||||
0.000000000,2.793640136,0.744837741,-0.970145734,1.328697007,0.047172979,-0.120189906,-0.157334421
|
||||
0.000000000,2.795264279,0.750262739,-0.971592824,1.321251451,0.051546999,-0.121442956,-0.152057369
|
||||
0.000000000,2.796500153,0.755384472,-0.972304521,1.313725300,0.055410016,-0.123089972,-0.146761365
|
||||
0.000000000,2.797408187,0.760257319,-0.972403901,1.306117653,0.058837290,-0.125064613,-0.141446493
|
||||
0.000000000,2.798032868,0.764922302,-0.971981875,1.298427571,0.061884369,-0.127317298,-0.136112440
|
||||
0.000000000,2.798406669,0.769410038,-0.971104607,1.290654072,0.064591363,-0.129811408,-0.130758727
|
||||
0.000000000,2.798552850,0.773742953,-0.969818877,1.282796132,0.066986084,-0.132520482,-0.125384851
|
||||
0.000000000,2.798487435,0.777936917,-0.968155901,1.274852680,0.069086291,-0.135426181,-0.119990354
|
||||
0.000000000,2.798220556,0.782002394,-0.966133929,1.266822598,0.070901208,-0.138516863,-0.114574872
|
||||
0.000000000,2.797757309,0.785945198,-0.963759823,1.258704719,0.072432413,-0.141786679,-0.109138168
|
||||
0.000000000,2.797098167,0.789766875,-0.961029705,1.250497823,0.073674114,-0.145235131,-0.103680159
|
||||
0.000000000,2.796238957,0.793464703,-0.957928611,1.242200634,0.074612768,-0.148867120,-0.098200960
|
||||
0.000000000,2.795170316,0.797031226,-0.954428902,1.233811819,0.075225829,-0.152693594,-0.092700939
|
||||
0.000000000,2.793876377,0.800453094,-0.950486827,1.225329985,0.075479184,-0.156733077,-0.087180827
|
||||
0.000000000,2.792332142,0.803708734,-0.946035827,1.216753674,0.075322264,-0.161014721,-0.081641890
|
||||
0.000000000,2.790498181,0.806763593,-0.940973234,1.208081362,0.074678409,-0.165584475,-0.076086289
|
||||
0.000000000,2.788308900,0.809559580,-0.935131048,1.199311452,0.073423810,-0.170518739,-0.070517853
|
||||
0.000000000,2.785641028,0.811986546,-0.928197821,1.190442273,0.071331413,-0.175961122,-0.064944180
|
||||
0.000000000,0.255988373,1.039014972,1.614825493,1.579806848,-1.610006057,-0.012320588,0.047584633
|
||||
0.000000000,0.263738451,1.070978698,1.650416613,1.588561031,-1.642529617,-0.023598925,0.088116949
|
||||
0.000000000,0.271873967,1.092433517,1.675088055,1.597059459,-1.664800242,-0.034833669,0.118132688
|
||||
0.000000000,0.279993201,1.107060286,1.692835178,1.605302602,-1.680434327,-0.046317740,0.141326779
|
||||
0.000000000,0.287989695,1.116906197,1.705818070,1.613290830,-1.691436336,-0.058118576,0.159752608
|
||||
0.000000000,0.295862560,1.123276536,1.715381654,1.621024417,-1.699082052,-0.070221665,0.174718480
|
||||
0.000000000,0.303650316,1.127071072,1.722433967,1.628503554,-1.704250785,-0.082582541,0.187125657
|
||||
0.000000000,0.311403426,1.128938041,1.727617002,1.635728351,-1.707576794,-0.095148894,0.197623074
|
||||
0.000000000,0.319172177,1.129355957,1.731396352,1.642698847,-1.709529321,-0.107870361,0.206689521
|
||||
0.000000000,0.327001568,1.128682663,1.734114224,1.649415017,-1.710460309,-0.120702522,0.214682876
|
||||
0.000000000,0.334929600,1.127187530,1.736023743,1.655876779,-1.710635613,-0.133608026,0.221872417
|
||||
0.000000000,0.342987223,1.125073976,1.737312570,1.662083998,-1.710256762,-0.146556345,0.228461380
|
||||
0.000000000,0.351199005,1.122495816,1.738119795,1.668036495,-1.709476755,-0.159522942,0.234603323
|
||||
0.000000000,0.359584067,1.119569400,1.738548298,1.673734051,-1.708411794,-0.172488257,0.240414269
|
||||
0.000000000,0.368157040,1.116382731,1.738673894,1.679176414,-1.707150108,-0.185436723,0.245981813
|
||||
0.000000000,0.376928930,1.113002351,1.738552158,1.684363303,-1.705758636,-0.198355902,0.251371994
|
||||
0.000000000,0.385907859,1.109478551,1.738223522,1.689294415,-1.704288097,-0.211235758,0.256634498
|
||||
0.000000000,0.395099674,1.105849326,1.737717086,1.693969430,-1.702776843,-0.224068075,0.261806598
|
||||
0.000000000,0.404508435,1.102143365,1.737053485,1.698388013,-1.701253790,-0.236845997,0.266916135
|
||||
0.000000000,0.414136803,1.098382318,1.736247022,1.702549822,-1.699740642,-0.249563673,0.271983784
|
||||
0.000000000,0.423986337,1.094582519,1.735307279,1.706454512,-1.698253586,-0.262215985,0.277024758
|
||||
0.000000000,0.434057733,1.090756279,1.734240314,1.710101737,-1.696804573,-0.274798348,0.282050110
|
||||
0.000000000,0.444351000,1.086912872,1.733049564,1.713491156,-1.695402288,-0.287306549,0.287067707
|
||||
0.000000000,0.454865600,1.083059277,1.731736521,1.716622436,-1.694052891,-0.299736642,0.292082973
|
||||
0.000000000,0.465600556,1.079200740,1.730301233,1.719495256,-1.692760573,-0.312084859,0.297099447
|
||||
0.000000000,0.476554530,1.075341197,1.728742686,1.722109308,-1.691527975,-0.324347551,0.302119203
|
||||
0.000000000,0.487725892,1.071483594,1.727059085,1.724464304,-1.690356506,-0.336521147,0.307143172
|
||||
0.000000000,0.499112764,1.067630128,1.725248068,1.726559977,-1.689246578,-0.348602123,0.312171376
|
||||
0.000000000,0.510713060,1.063782427,1.723306861,1.728396080,-1.688197785,-0.360586981,0.317203110
|
||||
0.000000000,0.522524521,1.059941686,1.721232401,1.729972396,-1.687209035,-0.372472236,0.322237077
|
||||
0.000000000,0.534544733,1.056108769,1.719021417,1.731288730,-1.686278642,-0.384254413,0.327271485
|
||||
0.000000000,0.546771154,1.052284283,1.716670501,1.732344920,-1.685404400,-0.395930036,0.332304123
|
||||
0.000000000,0.559201127,1.048468638,1.714176151,1.733140834,-1.684583632,-0.407495635,0.337332416
|
||||
0.000000000,0.571831898,1.044662081,1.711534803,1.733676370,-1.683813219,-0.418947745,0.342353460
|
||||
0.000000000,0.584660631,1.040864731,1.708742859,1.733951461,-1.683089626,-0.430282909,0.347364055
|
||||
0.000000000,0.597684419,1.037076596,1.705796700,1.733966070,-1.682408914,-0.441497685,0.352360718
|
||||
0.000000000,0.610900294,1.033297589,1.702692691,1.733720196,-1.681766743,-0.452588650,0.357339699
|
||||
0.000000000,0.624305246,1.029527534,1.699427191,1.733213871,-1.681158373,-0.463552406,0.362296987
|
||||
0.000000000,0.637896229,1.025766171,1.695996545,1.732447159,-1.680578657,-0.474385588,0.367228309
|
||||
0.000000000,0.651670177,1.022013159,1.692397084,1.731420157,-1.680022034,-0.485084869,0.372129135
|
||||
0.000000000,0.665624013,1.018268070,1.688625114,1.730132995,-1.679482514,-0.495646969,0.376994668
|
||||
0.000000000,0.679754666,1.014530388,1.684676908,1.728585833,-1.678953666,-0.506068658,0.381819839
|
||||
0.000000000,0.694059081,1.010799499,1.680548687,1.726778859,-1.678428593,-0.516346765,0.386599302
|
||||
0.000000000,0.708534235,1.007074690,1.676236604,1.724712290,-1.677899921,-0.526478180,0.391327420
|
||||
0.000000000,0.723177151,1.003355132,1.671736728,1.722386368,-1.677359766,-0.536459865,0.395998254
|
||||
0.000000000,0.737984914,0.999639870,1.667045013,1.719801360,-1.676799709,-0.546288853,0.400605546
|
||||
0.000000000,0.752954685,0.995927815,1.662157278,1.716957551,-1.676210771,-0.555962255,0.405142707
|
||||
0.000000000,0.768083722,0.992217722,1.657069170,1.713855246,-1.675583372,-0.565477262,0.409602793
|
||||
0.000000000,0.783369395,0.988508177,1.651776134,1.710494765,-1.674907295,-0.574831149,0.413978486
|
||||
0.000000000,0.798809209,0.984797578,1.646273371,1.706876441,-1.674171643,-0.584021275,0.418262065
|
||||
0.000000000,0.814400823,0.981084112,1.640555793,1.703000613,-1.673364789,-0.593045081,0.422445385
|
||||
0.000000000,0.830142072,0.977365730,1.634617972,1.698867628,-1.672474319,-0.601900097,0.426519837
|
||||
0.000000000,0.846030997,0.973640121,1.628454085,1.694477829,-1.671486965,-0.610583929,0.430476311
|
||||
0.000000000,0.862065868,0.969904679,1.622057843,1.689831562,-1.670388532,-0.619094267,0.434305155
|
||||
0.000000000,0.878245218,0.966156464,1.615422418,1.684929159,-1.669163810,-0.627428869,0.437996122
|
||||
0.000000000,0.894567874,0.962392163,1.608540356,1.679770945,-1.667796471,-0.635585563,0.441538309
|
||||
0.000000000,0.911032999,0.958608037,1.601403472,1.674357224,-1.666268948,-0.643562232,0.444920085
|
||||
0.000000000,0.927640138,0.954799864,1.594002738,1.668688279,-1.664562297,-0.651356804,0.448129010
|
||||
0.000000000,0.944389263,0.950962871,1.586328141,1.662764367,-1.662656023,-0.658967240,0.451151730
|
||||
0.000000000,0.961280840,0.947091648,1.578368518,1.656585710,-1.660527881,-0.666391510,0.453973856
|
||||
0.000000000,0.978315892,0.943180054,1.570111366,1.650152492,-1.658153633,-0.673627579,0.456579822
|
||||
0.000000000,0.995496089,0.939221090,1.561542605,1.643464853,-1.655506748,-0.680673369,0.458952695
|
||||
0.000000000,1.012823845,0.935206759,1.552646297,1.636522881,-1.652558040,-0.687526736,0.461073963
|
||||
0.000000000,1.030302440,0.931127878,1.543404295,1.629326610,-1.649275219,-0.694185418,0.462923254
|
||||
0.000000000,1.047936176,0.926973853,1.533795817,1.621876006,-1.645622328,-0.700646988,0.464477991
|
||||
0.000000000,1.065730561,0.922732386,1.523796906,1.614170968,-1.641559029,-0.706908786,0.465712951
|
||||
0.000000000,1.083692554,0.918389107,1.513379738,1.606211315,-1.637039693,-0.712967830,0.466599700
|
||||
0.000000000,1.101830876,0.913927084,1.502511740,1.597996782,-1.632012213,-0.718820707,0.467105851
|
||||
0.000000000,1.120156419,0.909326184,1.491154418,1.589527011,-1.626416436,-0.724463425,0.467194083
|
||||
0.000000000,1.138682798,0.904562192,1.479261792,1.580801542,-1.620182049,-0.729891211,0.466820811
|
||||
0.000000000,1.157427105,0.899605606,1.466778240,1.571819806,-1.613225670,-0.735098239,0.465934343
|
||||
0.000000000,1.176410979,0.894419909,1.453635467,1.562581117,-1.605446716,-0.740077241,0.464472264
|
||||
0.000000000,1.195662151,0.888959033,1.439748081,1.553084658,-1.596721390,-0.744818931,0.462357595
|
||||
0.000000000,1.215216798,0.883163482,1.425006900,1.543329478,-1.586893523,-0.749311146,0.459492942
|
||||
0.000000000,1.235123255,0.876954104,1.409268321,1.533314474,-1.575760011,-0.753537467,0.455751127
|
||||
0.000000000,1.255448263,0.870221463,1.392336377,1.523038388,-1.563046115,-0.757474890,0.450959249
|
||||
0.000000000,1.276288348,0.862806113,1.373929862,1.512499789,-1.548360039,-0.761089599,0.444869241
|
||||
0.000000000,1.297792967,0.854457505,1.353614810,1.501697063,-1.531099115,-0.764328345,0.437096768
|
||||
0.000000000,1.320220447,0.844731574,1.330639088,1.490628400,-1.510218250,-0.767097764,0.426969688
|
||||
0.000000000,1.344122768,0.832637397,1.303373985,1.479291780,-1.483439705,-0.769197089,0.413007932
|
||||
0.000000000,1.372176117,0.812824123,1.263512337,1.467684957,-1.438859775,-0.769683022,0.387351392
|
||||
0.000000000,1.494443255,0.669156817,0.996008310,1.455805443,-1.114600767,-0.725446922,0.181188520
|
||||
0.000000000,1.626547749,0.592950576,0.776782227,1.443650491,-0.922836322,-0.673937176,0.071591346
|
||||
0.000000000,2.100430816,0.491066528,0.077309447,1.431217073,-0.514544679,-0.483150111,-0.123632958
|
||||
0.000000000,2.365350127,0.512877934,-0.291475264,1.418501865,-0.346918544,-0.375976291,-0.172018050
|
||||
0.000000000,2.516288627,0.552648293,-0.509066817,1.405501217,-0.242743156,-0.304586843,-0.188024970
|
||||
0.000000000,2.607176401,0.591806709,-0.646436925,1.392211136,-0.169808290,-0.254827762,-0.191011844
|
||||
0.000000000,2.665131522,0.626316077,-0.737895064,1.378627251,-0.115755150,-0.219629012,-0.187716312
|
||||
0.000000000,2.703785254,0.655790511,-0.800984596,1.364744791,-0.074386128,-0.194777824,-0.181111460
|
||||
0.000000000,2.730411150,0.680800830,-0.845423696,1.350558545,-0.042079013,-0.177514617,-0.172676004
|
||||
0.000000000,2.749143292,0.702104443,-0.877003536,1.336062830,-0.016504464,-0.165929797,-0.163194995
|
||||
0.000000000,2.762462080,0.720412743,-0.899381647,1.321251451,0.003927050,-0.158659225,-0.153100052
|
||||
0.000000000,2.771921523,0.736327095,-0.914968754,1.306117653,0.020338041,-0.154711486,-0.142632297
|
||||
0.000000000,2.778527174,0.750330895,-0.925398526,1.290654072,0.033533142,-0.153361372,-0.131926730
|
||||
0.000000000,2.782942767,0.762799412,-0.931790729,1.274852680,0.044086636,-0.154080707,-0.121058229
|
||||
0.000000000,2.785607142,0.774012973,-0.934903043,1.258704719,0.052394749,-0.156493010,-0.110067545
|
||||
0.000000000,2.786799873,0.784166872,-0.935214655,1.242200634,0.058702859,-0.160346249,-0.098976515
|
||||
0.000000000,2.786671611,0.793372025,-0.932954544,1.225329985,0.063106151,-0.165504603,-0.087797590
|
||||
0.000000000,2.785234452,0.801631739,-0.928048485,1.208081362,0.065499199,-0.171974808,-0.076541380
|
||||
0.000000000,2.782236331,0.808719992,-0.919788415,1.190442273,0.065330290,-0.180061318,-0.065229292
|
||||
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -66,7 +66,7 @@
|
||||
</asset>
|
||||
|
||||
<worldbody>
|
||||
<body name="tag_board" pos="0.7 -0.2 1.1" euler="1.57 1.87 0.26">
|
||||
<body name="tag_board" pos="0.7 -0.2 1.1" euler="1.57 -1.57 0">
|
||||
<!-- 12cm x 12cm,厚1mm;contype=0 防止碰撞干扰 -->
|
||||
<geom name="tag_geom"
|
||||
type="box"
|
||||
@ -168,9 +168,10 @@
|
||||
rgba="0 1 1 1"/>
|
||||
|
||||
<!-- 相机 marker site(URDF: R_CAM_FIXED xyz+rpy) -->
|
||||
<!-- 相机看向前方时,R_CAM_SITE 的 -Z 指向前方 -->
|
||||
<site name="R_CAM_SITE"
|
||||
pos="-0.01212 -0.17655 0.07506"
|
||||
quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
|
||||
quat="3.17467e-11 -3.17467e-11 0.707107 0.707107"
|
||||
size="0.006"
|
||||
rgba="0 1 0 1"/>
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user