81 lines
2.3 KiB
C++
81 lines
2.3 KiB
C++
|
// #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;
|
||
|
|
// }
|