// #include "armcontroller.hpp"
// #include <iostream>
// #include <thread>

// #include <termios.h>
// #include <unistd.h>
// #include <sys/select.h>
// #include <cstdio>

// string urdfPath = "/home/unitree/unitreeApp/urdf/d1_description/urdf/d1_description.urdf";

// int main()
// {
//     armController *arm = new armController();

//     double x, y, z, row, pitch, yaw;

//     std::this_thread::sleep_for(std::chrono::seconds(1));

//     arm->trashActionDefault();
//     arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw);
//     // arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//     // arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw);

//     // return 0;
//     std::this_thread::sleep_for(std::chrono::seconds(3));

//     arm->goHome();
//     arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw);

//     std::this_thread::sleep_for(std::chrono::seconds(3));

//     // arm->ikPoseRPY(x, y, z, row, pitch, yaw);

//     std::this_thread::sleep_for(std::chrono::seconds(5));

//     bool flag = true;
//     while (flag)
//     {
//         char ch;
//         cin >> ch;
//         switch (ch)
//         {
//         case 'w':
//             x += 0.05;
//             arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//             break;
//         case 'a':
//             y += 0.05;
//             arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//             break;
//         case 's':
//             x -= 0.05;
//             arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//             break;
//         case 'd':
//             y -= 0.05;
//             arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//             break;
//         case 'i':
//             z += 0.05;
//             arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//             break;
//         case 'k':
//             z -= 0.05;
//             arm->ikPoseRPY(x, y, z, row, pitch, yaw);
//             break;
//         case 'c':
//             arm->goHome();
//             arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw);
//             break;
//         case 'p':
//             arm->ikPoseRPY(0.462213, -0.00559882, -0.0217322, 3.1036, 0.151366, 3.04644);
//             arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw);
//             break;
//         case 'q':
//             flag = false;
//         }
//     }
//     return 0;
// }