├── .gitignore ├── example ├── interfaceboarddemo.h ├── test.h ├── example_toolio.h ├── conveyortrackexample.h ├── example_8.h ├── example_3.h ├── example_0.h ├── example_5.h ├── example_9.h ├── example_6.h ├── example_4.h ├── example.pri ├── util.h ├── example_1.h ├── robottooliotestdemo.h ├── test.cpp ├── example_0.cpp ├── example_9.cpp ├── example_8.cpp ├── example_toolio.cpp ├── conveyortrackexample.cpp ├── AuboSdkExample.h ├── interfaceboarddemo.cpp ├── example_1.cpp ├── example_3.cpp ├── util.cpp ├── robottooliotestdemo.cpp ├── example_5.cpp ├── example_4.cpp └── example_6.cpp ├── src ├── 03_TestCamera2Base.cpp ├── 07_test_paw.cpp ├── 05_DetectSquare.cpp ├── 10_RobWork.cpp ├── paw │ ├── Paw.h │ ├── rs232.h │ ├── rs232.cpp │ └── Paw.cpp ├── Robot.h ├── 11_DrawCircle.cpp ├── utils │ ├── RPYUtils.h │ ├── 03_Camera2Base.h │ └── 05_DetectSquare.h ├── CMakeLists.txt ├── 08_Painter.cpp ├── 01_TestRobotSDK.cpp ├── 04_VerifyCalibration.cpp ├── 12_AutoGrab.cpp ├── 06_AutoGrab.cpp ├── 02_TestDoubleRobot.cpp ├── Robot.cpp └── TestDoubleRobot备份.cpp ├── cmake ├── FindFreenect.cmake ├── FindAubo.cmake ├── FindRobWork.cmake ├── FindQt.cmake └── FindOpenCV.cmake ├── main.cpp ├── readme.md └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | /.idea/ 2 | /cmake-build-debug/ -------------------------------------------------------------------------------- /example/interfaceboarddemo.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERFACEBOARDDEMO_H 2 | #define INTERFACEBOARDDEMO_H 3 | 4 | class InterfaceBoardDemo 5 | { 6 | public: 7 | InterfaceBoardDemo(); 8 | 9 | void example(); 10 | }; 11 | 12 | #endif // INTERFACEBOARDDEMO_H 13 | -------------------------------------------------------------------------------- /example/test.h: -------------------------------------------------------------------------------- 1 | #ifndef TEST_H 2 | #define TEST_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | 7 | class Test 8 | { 9 | public: 10 | Test(); 11 | 12 | 13 | static void demo(); 14 | }; 15 | 16 | #endif // TEST_H 17 | -------------------------------------------------------------------------------- /example/example_toolio.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_TOOLIO_H 2 | #define EXAMPLE_TOOLIO_H 3 | 4 | class Example_ToolIO 5 | { 6 | public: 7 | Example_ToolIO(); 8 | 9 | public: 10 | 11 | static void demo(); 12 | 13 | }; 14 | 15 | #endif // EXAMPLE_TOOLIO_H 16 | -------------------------------------------------------------------------------- /example/conveyortrackexample.h: -------------------------------------------------------------------------------- 1 | #ifndef CONVEYORTRACKEXAMPLE_H 2 | #define CONVEYORTRACKEXAMPLE_H 3 | 4 | class ConveyorTrackExample 5 | { 6 | public: 7 | ConveyorTrackExample(); 8 | 9 | static void demo(); 10 | }; 11 | 12 | #endif // CONVEYORTRACKEXAMPLE_H 13 | -------------------------------------------------------------------------------- /src/03_TestCamera2Base.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "utils/03_Camera2Base.h" 4 | /** 5 | * 测试封装的相机工具类 6 | */ 7 | int main(int argc, char **argv) 8 | { 9 | 10 | double cameraPos[3]={0.01,0.01,0.9}; 11 | double basePos[3]={0}; 12 | camera2base(cameraPos, basePos); 13 | 14 | 15 | return 0; 16 | } 17 | 18 | -------------------------------------------------------------------------------- /example/example_8.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_8_H 2 | #define EXAMPLE_8_H 3 | 4 | class Example_8 5 | { 6 | public: 7 | Example_8(); 8 | 9 | /** 10 | * @brief demo 11 | * 12 | * 正逆解接口 13 | */ 14 | static void demo(); 15 | 16 | 17 | static void FK(); 18 | 19 | static void IK(); 20 | }; 21 | 22 | #endif // EXAMPLE_8_H 23 | -------------------------------------------------------------------------------- /example/example_3.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_3_H 2 | #define EXAMPLE_3_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | 7 | 8 | class Example_3 9 | { 10 | public: 11 | Example_3(); 12 | 13 | /** 14 | * @brief demo 15 | * 16 | * 关节运动 17 | */ 18 | static void demo(); 19 | }; 20 | 21 | #endif // EXAMPLE_3_H 22 | -------------------------------------------------------------------------------- /example/example_0.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_0_H 2 | #define EXAMPLE_0_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | 7 | class Example_0 8 | { 9 | public: 10 | Example_0(); 11 | 12 | /** 13 | * @brief demo 14 | * 15 | * 使用SDK构建一个最简单的机械臂的控制工程 16 | */ 17 | static void demo(); 18 | }; 19 | 20 | #endif // EXAMPLE_0_H 21 | -------------------------------------------------------------------------------- /example/example_5.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_5_H 2 | #define EXAMPLE_5_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | 7 | 8 | class Example_5 9 | { 10 | public: 11 | Example_5(); 12 | 13 | /** 14 | * @brief demo 15 | * 16 | * 轨迹运动 17 | * 18 | */ 19 | static void demo(); 20 | }; 21 | 22 | #endif // EXAMPLE_5_H 23 | -------------------------------------------------------------------------------- /example/example_9.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_9_H 2 | #define EXAMPLE_9_H 3 | 4 | /** 5 | * @brief The Example_9 class 6 | * 7 | * 8 | * 关于io的使用案例 9 | * 10 | */ 11 | 12 | 13 | class Example_9 14 | { 15 | public: 16 | Example_9(); 17 | 18 | /** 19 | * @brief demo 20 | * 21 | * IO控制 22 | */ 23 | static void demo(); 24 | }; 25 | 26 | #endif // EXAMPLE_9_H 27 | -------------------------------------------------------------------------------- /example/example_6.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_6_H 2 | #define EXAMPLE_6_H 3 | 4 | class Example_6 5 | { 6 | public: 7 | Example_6(); 8 | 9 | /** 10 | * @brief demo 11 | * 12 | * 以关节或者直线运动的形式运动至目标位置 13 | */ 14 | static void demo(); 15 | 16 | static void example_MoveLtoPosition(); 17 | 18 | static void example_MoveJtoPosition(); 19 | }; 20 | 21 | #endif // EXAMPLE_6_H 22 | -------------------------------------------------------------------------------- /src/07_test_paw.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include "paw/Paw.h" 6 | #include 7 | 8 | using namespace std; 9 | /** 10 | * 测试因时机械爪的使用 11 | * @return 12 | */ 13 | int main() { 14 | Paw paw("/dev/ttyUSB0"); 15 | // 关闭机械抓 16 | paw.catchXG(); 17 | // 打开机械抓 18 | paw.release(); 19 | // 断开与机械抓的连接 20 | paw.close(); 21 | 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /cmake/FindFreenect.cmake: -------------------------------------------------------------------------------- 1 | #指定libfreenect2库所在的目录 2 | set(FREENECT_ROOT /home/kaijun/devtool/robotdependents/libs/libfreenect2) 3 | 4 | 5 | # 引入kenect2相关的依赖 6 | set(FREENECT_INCLUDE_DIRS ${FREENECT_ROOT}/include) 7 | set(FREENECT_LIBRARIES ${FREENECT_ROOT}/lib) 8 | 9 | include_directories(${FREENECT_INCLUDE_DIRS}) 10 | 11 | FIND_LIBRARY(libfreenect NAMES freenect2 PATHS ${FREENECT_ROOT}/lib NO_DEFAULT_PATH) 12 | set(FREENECT_LIBRARIES ${libfreenect}) 13 | message(AAA ${FREENECT_LIBRARIES}) -------------------------------------------------------------------------------- /example/example_4.h: -------------------------------------------------------------------------------- 1 | #ifndef EXAMPLE_4_H 2 | #define EXAMPLE_4_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | 7 | class Example_4 8 | { 9 | public: 10 | Example_4(); 11 | 12 | /** 13 | * @brief demo 14 | * 15 | * 直线运动 16 | */ 17 | static void demo(); 18 | 19 | static void demo_relativeOri(); 20 | 21 | static void RealTimeWaypointCallback(const aubo_robot_namespace::wayPoint_S *wayPointPtr, void *arg); 22 | 23 | static aubo_robot_namespace::wayPoint_S s_currentWayPoing; 24 | 25 | }; 26 | 27 | #endif // EXAMPLE_4_H 28 | -------------------------------------------------------------------------------- /src/05_DetectSquare.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by kaijun on 7/20/19. 3 | // 4 | 5 | #include 6 | #include 7 | #include "utils/05_DetectSquare.h" 8 | 9 | using namespace cv; 10 | using namespace std; 11 | /** 12 | * 小方块识别案例 13 | */ 14 | 15 | 16 | int main(){ 17 | // 获取原始彩色图像 18 | Mat src = imread("./assets/square.jpg",IMREAD_COLOR); 19 | 20 | vector squares; 21 | findSquare(src, squares); 22 | 23 | imshow("src",src); 24 | 25 | cout<<"最终小方块的数量："< 5 | #include "Robot.h" 6 | /** 7 | * 测试RobWork函数库是否已经正确导入 8 | * 9 | * 1. 项目问题 10 | * 2. 姿态问题 11 | */ 12 | int main(){ 13 | Robot robot; 14 | 15 | vector norm = {0,0,-1}; 16 | double alpha = 5; 17 | 18 | 19 | vector ref = {cos(alpha),sin(alpha),0}; 20 | // 测试根据RobWork封装的RPY工具类的可靠性 21 | vector result = calcRPY(norm,ref); 22 | 23 | cout< 2 | 3 | #include 4 | #include 5 | 6 | #include "example_0.h" 7 | #include "example_1.h" 8 | #include "example_3.h" 9 | #include "example_4.h" 10 | #include "example_5.h" 11 | #include "example_6.h" 12 | #include "example_8.h" 13 | #include "example_9.h" 14 | 15 | 16 | int main() 17 | { 18 | 19 | 20 | //案例0:使用SDK构建一个最简单的机械臂的控制工程 21 | //Example_0::demo(); 22 | 23 | //案例1:回调函数的方式获取实时路点，末端速度，机械臂的事件 关于机械臂相关状态的获取 24 | // Example_1::demo(); 25 | // Example_1::getJointStatus(); 26 | 27 | //案例3:关节运动 28 | Example_3::demo(); 29 | 30 | //案例4:直线运动 31 | // Example_4::demo_relativeOri(); 32 | 33 | //案例5:轨迹运动 34 | // Example_5::demo(); 35 | 36 | //案例6:运动至目标位置 37 | // Example_6::demo(); 38 | 39 | //案例8:正逆解 40 | // Example_8::demo(); 41 | 42 | //关于io的使用案例 43 | // Example_9::demo(); 44 | 45 | 46 | return 0; 47 | } 48 | 49 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # 机械臂示例工程 2 | 3 | 该项目正在不断更新优化过程中 4 | 5 | 本示例工程包含如下内容: 6 | 1. 机械臂基本操作 7 | 2. 机械抓串口通讯 8 | 3. 相片平面坐标转相机三维坐标 9 | 4. 相机坐标系转机械臂坐标系 10 | 5. 手眼标定 11 | 6. 自动抓取小方块 12 | 7. 机械臂绘画 13 | 9. 通过调整姿态画圆 14 | 15 | 16 | ## 1.环境搭建 17 | 进行代码调试之前,请确认您是否有以下硬件设备 18 | ### 1.1 测试条件 19 | 1. 系统环境: ubuntu14.04 20 | 2. 开发工具: clion 21 | 3. 机械臂:Auboi3 22 | 4. 因时机械抓 23 | 5. Kinect2相机 24 | 25 | ### 1.2 下载依赖 26 | 当您下载完该项目之后该项目之后,您还需要下载这个项目的依赖库. 27 | 28 | 下载robotdependents文件夹,这个文件大概1G,所以大家可以去百度网盘中下载. 29 | 30 | 在工程的cmake文件夹中有很多.cmake文件,您需要打开这些文件,将robotdependents的路径修改正确 31 | 32 | 33 | ## 2. 代码测试 34 | 每个文件都是一个独立的程序,直接进去运行main函数即可! 35 | ## 3. 注意 36 | 如果出现std::__cxx11::baisc_string 相关的错误.请按如下步骤进行修改: 37 | ``` 38 | cd /usr/lib/x86_64-linux-gnu/ 39 | 40 | ls -ll libstdc* 41 | 42 | 查看是否列出 libstdc++.so.6.0.25 ,如果没有,就去别人的电脑上拷贝一个放到这个路径下面 43 | 创建6.0.25的软链接 44 | sudo -sdf libstdc++.so.6.0.25 libstdc++.so.6 45 | ``` 46 | 47 | ## 4.参考文档 48 | ```$xslt 49 | https://blog.csdn.net/zhangrelay/article/details/81177851 50 | 51 | https://blog.csdn.net/UAV_FRESHMAN/article/details/52832874 52 | ``` -------------------------------------------------------------------------------- /cmake/FindAubo.cmake: -------------------------------------------------------------------------------- 1 | #指定傲博依赖库所在的目录 2 | set(AUBO_ROOT /home/kaijun/devtool/robotdependents/libs) 3 | 4 | 5 | # include 路径 6 | set(PROJECT_INCLUDE_DIRS 7 | ${AUBO_ROOT}/libconfig/linux_x64/inc; 8 | ${AUBO_ROOT}/log4cplus/linux_x64/inc; 9 | ${AUBO_ROOT}/protobuf/linux_x64/google/protobuf/inc; 10 | ${AUBO_ROOT}/robotController/Header; 11 | ${AUBO_ROOT}/robotSDK/inc; 12 | ) 13 | 14 | include_directories(${PROJECT_INCLUDE_DIRS}) 15 | 16 | 17 | # 设置查找的库组件 18 | SET(PROJECT_LIB_COMPONENTS config;log4cplus;protobuf;protobuf-lite;protoc;auborobotcontroller) 19 | 20 | FOREACH(aubocomponent ${PROJECT_LIB_COMPONENTS}) 21 | find_library(lib_${aubocomponent} NAMES ${aubocomponent} PATHS 22 | ${AUBO_ROOT}/protobuf/linux-x64/lib 23 | ${AUBO_ROOT}/libconfig/linux_x64/lib 24 | ${AUBO_ROOT}/log4cplus/linux_x64/lib 25 | ${AUBO_ROOT}/robotController/linux_x64 26 | ${AUBO_ROOT}/robotSDK/lib/linux_x64 27 | NO_DEFAULT_PATH) 28 | set(AUBO_LIBRARIES ${AUBO_LIBRARIES};${lib_${aubocomponent}}) 29 | ENDFOREACH() 30 | 31 | MESSAGE("AUBO_LIBRARIES====" ${AUBO_LIBRARIES}) -------------------------------------------------------------------------------- /src/paw/Paw.h: -------------------------------------------------------------------------------- 1 | #ifndef PAW_DEMO_PAW_PAW_H_ 2 | #define PAW_DEMO_PAW_PAW_H_ 3 | 4 | #include "rs232.h" 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | class PawData { 11 | public: 12 | virtual void toData(u_char *data) = 0; 13 | 14 | virtual int getDataLength() = 0; 15 | }; 16 | 17 | class CatchXgData : public PawData { 18 | public: 19 | CatchXgData(int speed = 500, int power = 100); 20 | 21 | private: 22 | int speed; 23 | int power; 24 | 25 | public: 26 | void toData(u_char *data); 27 | 28 | int getDataLength(); 29 | }; 30 | 31 | class ReleaseData : public PawData { 32 | 33 | public: 34 | ReleaseData(int speed = 300); 35 | 36 | private: 37 | int speed; 38 | 39 | public: 40 | void toData(u_char *data); 41 | 42 | int getDataLength(); 43 | }; 44 | 45 | 46 | class Paw { 47 | 48 | public: 49 | Paw(const char *port); 50 | ~Paw(); 51 | 52 | private: 53 | const char *port; 54 | kfx::RS232 *rs232; 55 | 56 | public: 57 | bool catchXG(int speed = 500, int power = 100); 58 | bool release(int speed = 500); 59 | void close(); 60 | 61 | private: 62 | int parseData(u_char *out, u_char cmd, PawData* data = nullptr); 63 | 64 | }; 65 | 66 | #endif //PAW_DEMO_PAW_PAW_H_ 67 | -------------------------------------------------------------------------------- /cmake/FindRobWork.cmake: -------------------------------------------------------------------------------- 1 | set(ROOT_RW_DIR "/home/kaijun/devtool/robotdependents/libs/robwork") 2 | 3 | 4 | 5 | include_directories(${ROOT_RW_DIR}/src) 6 | 7 | 8 | # 设置查找的库组件 9 | SET(ROBWORK_LIB_COMPONENTS rw_algorithms; 10 | rw_pathplanners; 11 | rw_pathoptimization; 12 | rw_simulation; 13 | rw_opengl; 14 | rw_task; 15 | rw_calibration; 16 | rw_csg; 17 | rw_control; 18 | rw_proximitystrategies; 19 | yaobi; 20 | pqp; 21 | fcl; 22 | rw; 23 | boost_system; 24 | rw_assembly) 25 | 26 | FOREACH(component_name ${ROBWORK_LIB_COMPONENTS}) 27 | find_library(lib${component_name} NAMES ${component_name} PATHS 28 | ${ROOT_RW_DIR}/libs 29 | /usr/lib/x86_64-linux-gnu/ 30 | NO_DEFAULT_PATH) 31 | set(ROBWORK_LIBRARIES ${ROBWORK_LIBRARIES};${lib${component_name}}) 32 | ENDFOREACH() 33 | 34 | MESSAGE(${ROBWORK_LIBRARIES}) -------------------------------------------------------------------------------- /cmake/FindQt.cmake: -------------------------------------------------------------------------------- 1 | #指定QT库所在的目录 2 | set(Qt_ROOT /home/kaijun/devtool/robotdependents/libs) 3 | 4 | 5 | 6 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/bin" CACHE PATH "Runtime directory" FORCE) 7 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/libs" CACHE PATH "Library directory" FORCE) 8 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/libs" CACHE PATH "Archive directory" FORCE) 9 | 10 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") 11 | set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${Qt_ROOT}/qt) 12 | 13 | file( 14 | WRITE 15 | ${CMAKE_CURRENT_SOURCE_DIR}/build/bin/qt.conf 16 | "[Paths] 17 | Plugins=${Qt_ROOT}/qt/plugins" 18 | ) 19 | 20 | 21 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 22 | set(CMAKE_AUTOMOC ON) 23 | set(CMAKE_AUTOUIC ON) 24 | set(CMAKE_AUTORCC ON) 25 | 26 | 27 | find_package(Qt5Core REQUIRED) 28 | find_package(Qt5Gui REQUIRED) 29 | find_package(Qt5Widgets REQUIRED) 30 | find_package(Qt5Network REQUIRED) 31 | 32 | SET(QT_LIBRARIES ${Qt5Core_LIBRARIES} ${Qt5Gui_LIBRARIES} ${Qt5Widgets_LIBRARIES}) 33 | 34 | 35 | # 头文件和链接库的路径 36 | include_directories(${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} 37 | ${Qt5Widgets_INCLUDE_DIRS} ${Qt5Network_INCLUDE_DIRS} ) -------------------------------------------------------------------------------- /example/robottooliotestdemo.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTTOOLIOTESTDEMO_H 2 | #define ROBOTTOOLIOTESTDEMO_H 3 | 4 | #include "serviceinterface.h" 5 | using namespace aubo_robot_namespace; 6 | 7 | class RobotToolIoTestDemo 8 | { 9 | public: 10 | RobotToolIoTestDemo(); 11 | 12 | private: 13 | /** 登录 **/ 14 | int loginAPI(); 15 | 16 | /** 退出登录 **/ 17 | int logoutAPI(); 18 | 19 | /** 设置工具端电源电压类型 接口 **/ 20 | int setToolPowerVoltageTypeAPI(ToolPowerType type); 21 | 22 | /** 获取工具端电源电压类型 接口 **/ 23 | int getToolPowerVoltageTypeAPI(); 24 | 25 | /** 获取工具端电源电压状态 接口 **/ 26 | int getToolPowerVoltageStatusAPI(); 27 | 28 | /** 获取工具端数字量IO的状态 接口 **/ 29 | int getAllToolDigitalIOStatusAPI(); 30 | 31 | /** 设置工具端数字量IO的状态 接口 **/ 32 | int setToolDigitalIOTypeAPI(aubo_robot_namespace::ToolDigitalIOAddr addr, RobotIoType value); 33 | 34 | /** 设置工具端数字量IO的状态 接口 **/ 35 | int setToolDigitalIOStatusAPI(std::string name, aubo_robot_namespace::IO_STATUS value); 36 | 37 | /** 设置工具端数字量IO的状态 接口 **/ 38 | int setToolDigitalIOStatusAPI(aubo_robot_namespace::ToolDigitalIOAddr addr, aubo_robot_namespace::IO_STATUS value); 39 | 40 | /** 获取工具段所有AI的状态 接口 **/ 41 | int robotServiceGetAllToolAIStatusAPI(); 42 | 43 | 44 | public: 45 | void example(); 46 | 47 | 48 | private: 49 | ServiceInterface *m_robotService; 50 | 51 | }; 52 | 53 | #endif // ROBOTTOOLIOTESTDEMO_H 54 | -------------------------------------------------------------------------------- /src/Robot.h: -------------------------------------------------------------------------------- 1 | #ifndef AUBOINTERFACEEXAMPLE_ROBOT_H 2 | #define AUBOINTERFACEEXAMPLE_ROBOT_H 3 | #include "AuboRobotMetaType.h" 4 | #include "serviceinterface.h" 5 | //#include "hand/Hand.h" 6 | #include "paw/Paw.h" 7 | #define DE2RA M_PI/180 8 | class Robot { 9 | public: 10 | // 创建机器人服务 11 | ServiceInterface robotService; 12 | private: 13 | const char* ipaddr="192.168.1.101"; 14 | int port = 8899; 15 | const char* username="admin"; 16 | const char* password="1"; 17 | const char* deviceName="/dev/ttyUSB0"; 18 | // Hand* hand = nullptr; 19 | Paw* paw=nullptr; 20 | public: 21 | Robot(); 22 | Robot(const char* ipaddr,int port,const char* username,const char* password,const char* deviceName); 23 | /*登录机械臂*/ 24 | void loginRobot(); 25 | /*初始化机械臂*/ 26 | void initRobot(); 27 | /*移动机械臂*/ 28 | void movej(double* joints); 29 | void movel(double* position); 30 | void movel(double *xyz,double* rpy); 31 | /*退出机械臂*/ 32 | ~Robot(); 33 | /*打开机械抓*/ 34 | void openHand(); 35 | /*关闭机械抓*/ 36 | void closeHand(); 37 | 38 | void IK(double* xyz,double* rpy,double* resultJoints); 39 | void FK(double* jointAngle,double* resultPoint); 40 | 41 | ServiceInterface getService(); 42 | }; 43 | 44 | #endif //AUBOINTERFACEEXAMPLE_ROBOT_H 45 | -------------------------------------------------------------------------------- /example/test.cpp: -------------------------------------------------------------------------------- 1 | #include "test.h" 2 | 3 | #define SERVER_HOST "127.0.0.1" 4 | #define SERVER_PORT 8899 5 | 6 | Test::Test() 7 | { 8 | } 9 | 10 | void Test::demo() 11 | { 12 | ServiceInterface robotService; 13 | 14 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 15 | 16 | /** 接口调用: 登录 ***/ 17 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 18 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 19 | { 20 | std::cerr<<"登录成功."< 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace kfx { 16 | 17 | const char Comports[22][13] = {"/dev/ttyACM0", 18 | "/dev/ttyS1", "/dev/ttyS2", "/dev/ttyS3", 19 | "/dev/ttyS4", "/dev/ttyS5", "/dev/ttyS6", 20 | "/dev/ttyS7", "/dev/ttyS8", "/dev/ttyS9", 21 | "/dev/ttyS10", "/dev/ttyS11", "/dev/ttyS12", 22 | "/dev/ttyS13", "/dev/ttyS14", "/dev/ttyS15", 23 | "/dev/ttyUSB0", "/dev/ttyUSB1", "/dev/ttyUSB2", 24 | "/dev/ttyUSB3", "/dev/ttyUSB4", "/dev/ttyUSB5"}; 25 | 26 | class RS232 { 27 | char devname[13]; // Device Name 28 | int baudr, port; // Baudrate and Port Number 29 | bool available; 30 | struct termios ops; // old port settings 31 | public: 32 | RS232(const char *, int); 33 | int IsAvailable() { return available; } 34 | char *GetDeviceName() { return devname; } 35 | int Read(unsigned char); 36 | int Read(unsigned char *, int); 37 | int Write(unsigned char); 38 | int Write(unsigned char *, int); 39 | void Print(const char *); 40 | void Close(); 41 | int IsCTSEnabled(); 42 | }; 43 | 44 | } 45 | 46 | #endif // giihub_com_kranfix_rs232_rs232_h -------------------------------------------------------------------------------- /src/11_DrawCircle.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by kaijun on 7/31/19. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "Robot.h" 11 | /** 12 | * 画圆: 保持末端xyz坐标值不变,通过调整姿态进行画圆 13 | */ 14 | using namespace std; 15 | 16 | Robot robot; 17 | 18 | void drawCircle(){ 19 | // 获取当前tcp的坐标 20 | const std::vector &tcp = {0.07,-0.32,0.175}; 21 | // 工具的长度 22 | double T = 0.180; 23 | // 根据工具的长途和Z的值计算圆的半径 24 | double R = sqrt(pow(T,2) - pow(tcp[2],2)); 25 | // 轨迹的偏移量 26 | double i = M_PI/12; 27 | for (double alpha = 0; alpha< 2*M_PI ; alpha+=i) { 28 | 29 | std::vector norm{R*cos(alpha),R*sin(alpha),-tcp[2]}; 30 | 31 | std::vector result = calcRPY(norm); 32 | cout<<"result0:"< poseTarget{tcp[0], tcp[1], tcp[2], result[2], result[1], result[0]}; 38 | 39 | // 这里加上的0.033只是为了让机械臂不撞上桌面 40 | double xyz[]={tcp[0], tcp[1], tcp[2]+0.033}; 41 | double rpy[]={result[2], result[1], result[0]}; 42 | robot.movel(xyz,rpy); 43 | } 44 | } 45 | 46 | 47 | int main(int argc,char** argv){ 48 | QApplication app(argc,argv); 49 | 50 | QWidget widget; 51 | widget.setWindowTitle("画圆案例"); 52 | 53 | QVBoxLayout boxlayout(&widget); 54 | 55 | QPushButton btn("开始画圆",&widget); 56 | QObject::connect(&btn,&QPushButton::clicked,drawCircle); 57 | 58 | boxlayout.addWidget(&btn); 59 | widget.show(); 60 | 61 | 62 | return app.exec(); 63 | } -------------------------------------------------------------------------------- /cmake/FindOpenCV.cmake: -------------------------------------------------------------------------------- 1 | 2 | # once done, this will define 3 | #OpenCV_FOUND - whether there is OpenCV in the prebuilt libraries 4 | #OpenCV_INCLUDE_DIRS - include directories for OpenCV 5 | #OpenCV_LIBRARY_DIRS - library directories for OpenCV 6 | #OpenCV_LIBRARIES - link this to use OpenCV 7 | 8 | unset(OpenCV_FOUND) 9 | 10 | set(PREBUILT_DIR /home/kaijun/devtool/robotdependents/libs) 11 | 12 | 13 | set(OpenCV_DIR ${PREBUILT_DIR}/opencv-3.4.1) 14 | 15 | MESSAGE(STATUS "TEST ${PREBUILT_DIR}") 16 | 17 | set(OpenCV_INCLUDE_DIRS ${OpenCV_DIR}/include) 18 | SET(OpenCV_LIB_COMPONENTS opencv_videostab;opencv_video;opencv_superres;opencv_stitching;opencv_photo; 19 | opencv_objdetect;opencv_ml;opencv_imgproc;opencv_highgui;opencv_flann;opencv_features2d; 20 | opencv_core;opencv_calib3d;opencv_xfeatures2d;opencv_imgcodecs) 21 | find_path(OpenCV_INCLUDE_DIRS NAMES opencv.h HINTS ${OpenCV_DIR}/include NO_SYSTEM_ENVIRONMENT_PATH) 22 | 23 | set(OpenCV_LIBRARY_DIRS ${OpenCV_DIR}/lib) 24 | 25 | FOREACH(cvcomponent ${OpenCV_LIB_COMPONENTS}) 26 | find_library(lib_${cvcomponent} NAMES ${cvcomponent} HINTS ${OpenCV_DIR}/lib NO_DEFAULT_PATH) 27 | 28 | set(OpenCV_LIBRARIES ${OpenCV_LIBRARIES};${lib_${cvcomponent}}) 29 | ENDFOREACH() 30 | 31 | set(OpenCV_LIBS ${OpenCV_LIBRARIES}) 32 | 33 | set(OpenCV_INCLUDE_DIRS ${OpenCV_INCLUDE_DIRS};${OpenCV_INCLUDE_DIRS}/opencv) 34 | 35 | if (OpenCV_INCLUDE_DIRS AND OpenCV_LIBRARIES) 36 | set(OpenCV_FOUND TRUE) 37 | endif (OpenCV_INCLUDE_DIRS AND OpenCV_LIBRARIES) 38 | 39 | if (OpenCV_FOUND) 40 | if (NOT OpenCV_FIND_QUIETLY) 41 | message(STATUS "Found OpenCV: ${OpenCV_LIBRARIES}") 42 | endif (NOT OpenCV_FIND_QUIETLY) 43 | else(OpenCV_FOUND) 44 | if (OpenCV_FIND_REQUIRED) 45 | message(FATAL_ERROR "Could not find the OpenCV library") 46 | endif () 47 | endif (OpenCV_FOUND) 48 | 49 | # 导入opencv相关的头文件 50 | include_directories(${OpenCV_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /example/example_0.cpp: -------------------------------------------------------------------------------- 1 | #include "example_0.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | #define SERVER_HOST "127.0.0.1" 12 | #define SERVER_PORT 8899 13 | 14 | 15 | Example_0::Example_0() 16 | { 17 | } 18 | 19 | 20 | void Example_0::demo() 21 | { 22 | ServiceInterface robotService; 23 | 24 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 25 | 26 | /** 接口调用: 登录 ***/ 27 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 28 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 29 | { 30 | std::cerr<<"登录成功."< 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | using namespace rw::math; 14 | 15 | 16 | /** 17 | * 18 | * @param norm 法线方向 19 | * @param ref 参考向量 20 | * @return 21 | */ 22 | static vector calcRPY(vector norm,vector ref) 23 | { 24 | 25 | /// 1.法线方向(normal) 26 | Vector3D norVec( norm[0], norm[1], norm[2] ); 27 | norVec = normalize(norVec); 28 | 29 | /// 2.给定参考方向，此方向根据实际情况指定 30 | // Vector3D refVec( norm[0], norm[1], 0.0 ); 31 | Vector3D refVec(ref[0],ref[1], ref[2] ); 32 | 33 | /// 3.通过叉乘，计算x轴向量 34 | Vector3D xVec = cross( norVec, refVec ); 35 | // std::cout< yVec = cross( norVec, xVec ); 41 | yVec = normalize( yVec ); 42 | 43 | /// 5.根据三个坐标轴，可以构建一个旋转矩阵， 44 | /// 输入依次是 45 | /// x轴向量 46 | /// y轴向量 47 | /// z轴向量(法向量) 48 | Rotation3D rot( xVec, yVec, norVec ); 49 | 50 | 51 | /// 通过旋转矩阵，可以直接构建出RPY对象 52 | RPY rpy( rot ); 53 | 54 | ///最后，需要注意的是，此处的RPY，是绕动坐标系变换的，顺序是z,y,x 55 | ///而UR驱动中,moveL的输入里，RPY是按x,y,z顺序输入的， 56 | ///因此，实际使用时，需要逆转输出 57 | std::cout << rpy[0] << "," << rpy[1] << "," << rpy[2]< result{rpy[0],rpy[1],rpy[2]}; 62 | // result = normalize(rpy); 63 | return result; 64 | } 65 | 66 | 67 | /** 68 | * 给定法线方向，需要构建出一个坐标系 69 | * @return 70 | */ 71 | static vector calcRPY(vector norm) 72 | { 73 | 74 | vector ref={1,0,0}; 75 | 76 | vector result = calcRPY(norm,ref); 77 | return result; 78 | } 79 | 80 | #endif //STUDY_ROBOT_RPYUTILS_H 81 | -------------------------------------------------------------------------------- /example/example_9.cpp: -------------------------------------------------------------------------------- 1 | #include "example_9.h" 2 | 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | #include 13 | 14 | #include "AuboRobotMetaType.h" //机械臂的元数据类型 15 | #include "serviceinterface.h" //机械臂接口 16 | 17 | 18 | //#define SERVER_HOST "127.0.0.1" 19 | #define SERVER_HOST "192.168.1.123" 20 | #define SERVER_PORT 8899 21 | 22 | 23 | Example_9::Example_9() 24 | { 25 | } 26 | 27 | void Example_9::demo() 28 | { 29 | ServiceInterface robotService; 30 | 31 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 32 | 33 | /** 接口调用: 登录 ***/ 34 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 35 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 36 | { 37 | std::cerr<<"登录成功."< ioType; 50 | // std::vector statusVector; 51 | 52 | // ioType.push_back(aubo_robot_namespace::RobotBoardUserAI); 53 | // robotService.robotServiceGetBoardIOStatus(ioType, statusVector); 54 | 55 | // for(int i=0;i 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "util.h" 11 | 12 | #define SERVER_HOST "127.0.0.1" 13 | #define SERVER_PORT 8899 14 | 15 | 16 | Example_8::Example_8() 17 | { 18 | } 19 | 20 | void Example_8::demo() 21 | { 22 | FK(); 23 | IK(); 24 | } 25 | 26 | void Example_8::FK() 27 | { 28 | ServiceInterface robotService; 29 | 30 | aubo_robot_namespace::wayPoint_S wayPoint; 31 | 32 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 33 | 34 | double jointAngle[aubo_robot_namespace::ARM_DOF] = {0}; 35 | 36 | Util::initJointAngleArray(jointAngle, 0.0/180.0*M_PI, 0.0/180.0*M_PI, 90.0/180.0*M_PI, 0.0/180.0*M_PI, 90.0/180.0*M_PI, 0.0/180.0*M_PI); 37 | 38 | ret = robotService.robotServiceRobotFk(jointAngle, aubo_robot_namespace::ARM_DOF, wayPoint); 39 | 40 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 41 | { 42 | Util::printWaypoint(wayPoint); 43 | } 44 | else 45 | { 46 | std::cerr<<"调用正解函数失败"< 4 | #include 5 | #include 6 | #include 7 | #include "AuboRobotMetaType.h" //机械臂的元数据类型 8 | #include "serviceinterface.h" //机械臂接口 9 | 10 | #define SERVER_HOST "127.0.0.1" 11 | //#define SERVER_HOST "192.168.1.123" 12 | #define SERVER_PORT 8899 13 | 14 | 15 | 16 | Example_ToolIO::Example_ToolIO() 17 | { 18 | } 19 | 20 | void Example_ToolIO::demo() 21 | { 22 | ServiceInterface robotService; 23 | 24 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 25 | 26 | /** 接口调用: 登录 ***/ 27 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 28 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 29 | { 30 | std::cerr<<"登录成功."< statusVector; 58 | ret = robotService.robotServiceGetAllToolDigitalIOStatus(statusVector); 59 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 60 | { 61 | std::cout<<"Digital IO Count:"< 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "Robot.h" 15 | #include "utils/03_Camera2Base.h" 16 | 17 | /* 18 | 画图案例 19 | */ 20 | using namespace std; 21 | using namespace cv; 22 | 23 | 24 | // 初始姿态 25 | double defaultPos[6]={-0.307233,-0.119000,0.421263,175*DE2RA,0*DE2RA,-90*DE2RA}; 26 | 27 | //Robot* robot = nullptr; 28 | Robot robot; 29 | 30 | 31 | void draw(Mat& binary) { 32 | 33 | Mat dst(binary.rows, binary.cols, CV_8UC1); 34 | 35 | double defaultDepth = 800; 36 | 37 | double xoffset = 950; 38 | double yoffset = 500; 39 | 40 | // 固定的z值 41 | double constZ = 0.2125; 42 | 43 | // 查找轮廓 44 | vector> contours; 45 | vector hierachy; 46 | // 查找轮廓 47 | findContours(binary, contours, hierachy, RETR_LIST, CHAIN_APPROX_SIMPLE); 48 | 49 | 50 | double lastX = 0; 51 | double lastY = 0; 52 | double lastZ = 0; 53 | 54 | for (int i = 0; i < contours.size(); ++i) { 55 | // 获得轮廓 56 | vector c = contours[i]; 57 | // 遍历每一个轮廓中的每一个像素点 58 | for (int j = 0; j < c.size(); ++j) { 59 | Point2d p = c[j]; 60 | // 将相机坐标点转成base坐标点 61 | double picturePos[3] = {xoffset + p.x, yoffset + p.y, defaultDepth}; 62 | double cameraPos[3] = {0}; 63 | 64 | // 将像素坐标系中点转成相机坐标系中的点 65 | piexl2camera(picturePos, cameraPos); 66 | 67 | // 将相机坐标系中的点转成base坐标系中的点 68 | double basePos[3] = {0}; 69 | camera2base(cameraPos, basePos); 70 | 71 | // double toolLength = 0.177; 72 | double x = basePos[0] + 0.01; // 标定结果的偏移量 73 | double y = basePos[1]; 74 | // double z = basePos[2] + toolLength; 75 | 76 | double pos[6] = {x, y, constZ, 180 * DE2RA, 0, -90 * DE2RA}; 77 | // cout << "机械臂将要移动到x=" << x << " y=" << y << " z=" << z << endl; 78 | lastX = x; 79 | lastY = y; 80 | // lastZ = z; 81 | robot.movel(pos); 82 | 83 | } 84 | 85 | // 每画完一个轮廓，机械臂要抬起来一点 86 | double uppos[6] = {lastX, lastY, constZ + 0.05, 180 * DE2RA, 0, -90 * DE2RA}; 87 | robot.movel(uppos); 88 | } 89 | 90 | } 91 | 92 | 93 | 94 | int main(int argc, char** argv) { 95 | 96 | Mat src = imread("./assets/logo.png",IMREAD_GRAYSCALE); 97 | imshow("src",src); 98 | // 将原图缩小到250 99 | Mat src2; 100 | resize(src,src2, Size(),0.4,0.4); 101 | imshow("src2",src2); 102 | 103 | Mat binary; 104 | threshold(src2,binary,100,255,THRESH_BINARY|THRESH_OTSU); 105 | 106 | // imshow("binary",binary); 107 | 108 | draw(binary); 109 | 110 | 111 | 112 | return waitKey(0); 113 | } -------------------------------------------------------------------------------- /example/AuboSdkExample.h: -------------------------------------------------------------------------------- 1 | #ifndef AUBO_SDK_EXAMPLE_H 2 | #define AUBO_SDK_EXAMPLE_H 3 | 4 | #include "AuboRobotMetaType.h" 5 | #include "serviceinterface.h" 6 | 7 | 8 | /** 要写清楚demo运行步骤 9 | * step1: robotServiceOfflineTrackWaypointClear 10 | * step2: robotServiceOfflineTrackWaypointAppend 11 | * step3: robotServiceOfflineTrackMoveStartup 12 | */ 13 | 14 | 15 | 16 | class AuboSdkExample 17 | { 18 | public: 19 | AuboSdkExample(); 20 | 21 | public: 22 | /** 打印路点信息 **/ 23 | static void printWaypoint(aubo_robot_namespace::wayPoint_S &wayPoint); 24 | 25 | /** 打印关节状态信息 **/ 26 | static void printJointStatus(const aubo_robot_namespace::JointStatus *jointStatus, int len); 27 | 28 | /** 打印事件信息 **/ 29 | static void printEventInfo(const aubo_robot_namespace::RobotEventInfo &eventInfo); 30 | 31 | /** 打印诊断信息 **/ 32 | static void printRobotDiagnosis(const aubo_robot_namespace::RobotDiagnosis &robotDiagnosis); 33 | 34 | 35 | private: 36 | 37 | 38 | /** 实时关节状态回调函数　**/ 39 | static void RealTimeJointStatusCallback (const aubo_robot_namespace::JointStatus *jointStatus, int size, void *arg); 40 | 41 | 42 | 43 | 44 | private: 45 | static void initJointAngleArray(double *array, double joint0,double joint1,double joint2,double joint3,double joint4,double joint5); 46 | 47 | 48 | public: 49 | /** 关于一些信息获取的示例　**/ 50 | void example_0(); 51 | 52 | void example_JointMove(); 53 | 54 | void example_LineMove(); 55 | 56 | void example_TrackMove(); 57 | 58 | void example_MoveLtoPosition(); 59 | 60 | void example_MoveJtoPosition(); 61 | 62 | public: 63 | /** 四元素和欧拉角相互转化示例 **/ 64 | void example_quaternion_RPY_Conversion(); 65 | 66 | 67 | public: //关于工具端的实例函数 68 | void example_ToolIO(); 69 | 70 | public: 71 | 72 | void testOfflineTrack(); //测试离线轨迹 73 | 74 | void testWorkMode(); //机械臂仿真模式,真实模式 75 | 76 | void testArriveAhead(); //跟随模式测试 77 | //类似于交融半径，几个点之间运动到某一个为至点后不停留 78 | 79 | void testIo(); //IO状态测试（需要连接实际的机器人） 80 | 81 | 82 | 83 | 84 | public: 85 | 86 | /** 87 | * @brief Example_0 88 | * 89 | * 使用SDK构建一个最简单的机械臂的控制工程 90 | * 91 | */ 92 | void Example_0(); 93 | 94 | 95 | 96 | static void RealTimeWaypointCallback (const aubo_robot_namespace::wayPoint_S *wayPointPtr, void *arg); //用于获取实时路点回调函数 97 | 98 | static void RealTimeEndSpeedCallback (double speed, void *arg); //获取实时末端速度回调函数 99 | 100 | static void RealTimeEventInfoCallback(const aubo_robot_namespace::RobotEventInfo *pEventInfo, void *arg); //获取实时机械臂事件回调函数 101 | 102 | /** 103 | * @brief Example_1 104 | * 105 | * 回调函数的方式获取实时路点，末端速度，机械臂的事件 106 | * 107 | * 108 | */ 109 | void Example_1(); 110 | 111 | 112 | /** 113 | * @brief Example_３ 114 | * 115 | * 关节运动 116 | */ 117 | void Example_3(); 118 | 119 | /** 120 | * @brief Example_4 121 | * 122 | * 直线运动 123 | */ 124 | void Example_4(); 125 | 126 | /** 127 | * @brief Example_5 128 | * 129 | * 轨迹运动 130 | */ 131 | void Example_5(); 132 | 133 | 134 | 135 | 136 | }; 137 | 138 | #endif // AUBO_SDK_EXAMPLE_H 139 | -------------------------------------------------------------------------------- /src/01_TestRobotSDK.cpp: -------------------------------------------------------------------------------- 1 | #include "Robot.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #define DE2RA M_PI/180 12 | 13 | // 姿态1: 14 | double joint1[] = {18.819843*DE2RA, -17.610569*DE2RA, -109.448857*DE2RA,-3.861306*DE2RA, -88.872687*DE2RA, -160.007877*DE2RA}; 15 | double point1[] = {-0.227602, -0.205245, 0.312001,-178.483566*DE2RA, 1.75*DE2RA, 88.870956*DE2RA}; 16 | // 17 | //// 姿态2: 从点1的位置到点2的位置，逆解的时候会出现大翻转 18 | double joint2[] = {87.401334*DE2RA, -9.414021*DE2RA, -106.291800*DE2RA,-11.284257*DE2RA, -88.872687*DE2RA, -160.009120*DE2RA}; 19 | double point2[] = {0.106190, -0.325713, 0.302872,-176.938768*DE2RA, 2.561946*DE2RA, 157.537857*DE2RA}; 20 | 21 | 22 | // 创建一个机器人对象 23 | Robot robot; 24 | 25 | //void testOpen(){ 26 | // robot.openHand(); 27 | //} 28 | 29 | void doPutUp(){ 30 | //1. 移动到抓盒子的点的上方 31 | robot.movej(joint1); 32 | //2. 打开机械抓 33 | robot.openHand(); 34 | sleep(1); 35 | //3. 向下移动到物品 36 | double downPos[aubo_robot_namespace::ARM_DOF]; 37 | memcpy(downPos,point1,aubo_robot_namespace::ARM_DOF); 38 | downPos[2] -= 0.05; 39 | robot.movel(downPos); 40 | 41 | //4. 关闭机械抓 42 | robot.closeHand(); 43 | sleep(1); 44 | //5. 返回到盒子上方的位置 45 | robot.movel(point1); 46 | } 47 | 48 | void doPutDown(){ 49 | //1. 移动到放物品的点的上方 50 | robot.movej(joint2); 51 | //2. 向下移动到放物品的地方 52 | double downPos[aubo_robot_namespace::ARM_DOF]; 53 | memcpy(downPos,point2,aubo_robot_namespace::ARM_DOF); 54 | downPos[2] -= 0.05; 55 | robot.movel(point2); 56 | //3. 打开机械抓。等待1s 57 | robot.openHand(); 58 | sleep(1); 59 | //4. 向上移动到放物品的点 60 | robot.movel(point2); 61 | //5. 关闭机械抓 62 | robot.closeHand(); 63 | sleep(1); 64 | } 65 | 66 | 67 | 68 | int main(int argc,char **argv){ 69 | // 创建Qt应用 70 | QApplication app(argc,argv); 71 | // 创建QT窗口 72 | QWidget widget; 73 | widget.setFixedWidth(300); 74 | // 创建布局 75 | QVBoxLayout layout; 76 | // 设置窗口布局为layout 77 | widget.setLayout(&layout); 78 | 79 | //打开机械爪 80 | QPushButton openHand0("连续操机械抓"); 81 | layout.addWidget(&openHand0); 82 | QObject::connect(&openHand0,&QPushButton::clicked,[&](){ 83 | for (int i = 0; i < 5; ++i) { 84 | robot.openHand(); 85 | sleep(1); 86 | robot.closeHand(); 87 | sleep(1); 88 | } 89 | 90 | }); 91 | //打开机械爪 92 | QPushButton openHand("打开机械爪"); 93 | layout.addWidget(&openHand); 94 | QObject::connect(&openHand,&QPushButton::clicked,[&](){robot.openHand();}); 95 | 96 | //关闭机械爪 97 | QPushButton closeHand("关闭机械爪"); 98 | layout.addWidget(&closeHand); 99 | QObject::connect(&closeHand,&QPushButton::clicked,[&](){robot.closeHand();}); 100 | 101 | //抓取的按钮 102 | QPushButton putUpBtn("抓取盒子"); 103 | layout.addWidget(&putUpBtn); 104 | QObject::connect(&putUpBtn,&QPushButton::clicked,doPutUp); 105 | 106 | //放下的按钮 107 | QPushButton putDownBtn("放下盒子"); 108 | layout.addWidget(&putDownBtn); 109 | QObject::connect(&putDownBtn,&QPushButton::clicked,doPutDown); 110 | 111 | // 将窗口显示出来 112 | widget.show(); 113 | 114 | int exec_code = app.exec(); 115 | 116 | return exec_code; 117 | } -------------------------------------------------------------------------------- /src/utils/03_Camera2Base.h: -------------------------------------------------------------------------------- 1 | #ifndef AUBOIUTILR_ROBOT_H 2 | #define AUBOIUTILR_ROBOT_H 3 | 4 | #include 5 | #include 6 | 7 | // Eigen 部分 8 | #include 9 | // 稠密矩阵的代数运算（逆，特征值等） 10 | #include 11 | //Eigen 几何模块 12 | #include 13 | 14 | #include 15 | 16 | /** 17 | * 本文件包含: 18 | * 1.像素坐标系转相机坐标系 19 | * 2.相机坐标系转Base坐标系 20 | */ 21 | 22 | using namespace std; 23 | using namespace Eigen; 24 | using namespace cv; 25 | 26 | // 内参文件路径 27 | static cv::String inCailFilePath="./assets/3DCameraInCailResult.xml"; 28 | // 外参文件路径 29 | static cv::String exCailFilePath="./assets/3DCameraExCailResult.xml"; 30 | 31 | 32 | /** 33 | * 将行列坐标，转成相机坐标系中的x,y,z 34 | * @param piexld 输入 {列, 行, 深度} 35 | * @param cameraPos 输出 {x,y,z} 36 | */ 37 | static void piexl2camera(double *piexld,double *cameraPos){ 38 | Mat cameraMatrix=cv::Mat_(3, 3);; 39 | // 读取内参 40 | FileStorage paramFs(inCailFilePath,FileStorage::READ); 41 | paramFs["cameraMatrix"]>>cameraMatrix; 42 | 43 | cout<<"图像平面：行："<(0,0); 51 | double camera_fy = cameraMatrix.at(1,1); 52 | double camera_cx = cameraMatrix.at(0,2); 53 | double camera_cy = cameraMatrix.at(1,2); 54 | // 计算相机坐标系中的值 55 | double z = double(d)/1000; //单位是米 56 | double x = (u - camera_cx) * z / camera_fx; 57 | double y = (v - camera_cy) * z / camera_fy; 58 | 59 | cameraPos[0]=-x; 60 | cameraPos[1]=y; 61 | cameraPos[2]=z; 62 | 63 | cout<<"ub空间：x:"<> angle; 83 | fs["AxisX"] >> axisX; 84 | fs["AxisY"] >> axisY; 85 | fs["AxisZ"] >> axisZ; 86 | fs["TranslationX"] >> translationX; 87 | fs["TranslationY"] >> translationY; 88 | fs["TranslationZ"] >> translationZ; 89 | // 轴角对 90 | Vector3d axisMatrix(axisX, axisY, axisZ); 91 | AngleAxisd angleAxisd(angle, axisMatrix); 92 | // 获取旋转矩阵 93 | Matrix3d ratationMatrix = angleAxisd.toRotationMatrix(); 94 | cout << "旋转矩阵：" << angleAxisd.toRotationMatrix() << endl; 95 | // 获取平移矩阵 96 | Vector3d translationMatrix(translationX, translationY, translationZ); 97 | 98 | // 获取相机拍摄到的坐标 99 | Vector3d cameraPosition(cameraPos[0], cameraPos[1], cameraPos[2]); 100 | 101 | // 获取输出结果 102 | cout << "旋转之后的结果:" << ratationMatrix * cameraPosition << endl; 103 | cout << "平移向量:" << translationMatrix / 1000 << endl; 104 | 105 | //进行转换 3*3 3*1 + 平移矩阵 106 | Vector3d resultBase = ratationMatrix * cameraPosition + translationMatrix / 1000; 107 | 108 | cout<<"最终结果："< 4 | #include 5 | #include 6 | #include "serviceinterface.h" 7 | 8 | //#define SERVER_HOST "127.0.0.1" 9 | #define SERVER_HOST "192.168.1.5" 10 | #define SERVER_PORT 8899 11 | 12 | InterfaceBoardDemo::InterfaceBoardDemo() 13 | { 14 | 15 | } 16 | 17 | 18 | 19 | void InterfaceBoardDemo::example() 20 | { 21 | ServiceInterface robotService; 22 | 23 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 24 | 25 | /** 接口调用: 登录 ***/ 26 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 27 | 28 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 29 | { 30 | std::cerr<<"登录成功."< ioType; 46 | std::vector configVector; 47 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerDI); 48 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerDO); 49 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerAI); 50 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerAO); 51 | ioType.push_back(aubo_robot_namespace::RobotBoardUserDI); 52 | ioType.push_back(aubo_robot_namespace::RobotBoardUserDO); 53 | ioType.push_back(aubo_robot_namespace::RobotBoardUserAI); 54 | ioType.push_back(aubo_robot_namespace::RobotBoardUserAO); 55 | 56 | ret = robotService.robotServiceGetBoardIOConfig(ioType, configVector); 57 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 58 | { 59 | std::cout<<"获取BoardIOConfig成功"< ioType; 73 | std::vector statusVector; 74 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerDI); 75 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerDO); 76 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerAI); 77 | ioType.push_back(aubo_robot_namespace::RobotBoardControllerAO); 78 | ioType.push_back(aubo_robot_namespace::RobotBoardUserDI); 79 | ioType.push_back(aubo_robot_namespace::RobotBoardUserDO); 80 | ioType.push_back(aubo_robot_namespace::RobotBoardUserAI); 81 | ioType.push_back(aubo_robot_namespace::RobotBoardUserAO); 82 | 83 | ret = robotService.robotServiceGetBoardIOStatus(ioType, statusVector); 84 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 85 | { 86 | std::cout<<"获取BoardIOStatus成功"< SSIZE_MAX) size = (int) SSIZE_MAX; /* SSIZE_MAX is defined in limits.h */ 102 | #else 103 | if(size>4096) size = 4096; 104 | #endif 105 | 106 | return read(port, buf, size); 107 | } 108 | 109 | int kfx::RS232::Write(unsigned char byte) { 110 | return write(port, &byte, 1); 111 | } 112 | 113 | int kfx::RS232::Write(unsigned char *buf, int size) { 114 | return write(port, buf, size); 115 | } 116 | 117 | void kfx::RS232::Close() { 118 | close(port); 119 | tcsetattr(port, TCSANOW, &ops); 120 | } 121 | 122 | /* 123 | Constant Description 124 | -------------------------------------------- 125 | TIOCM_LE DSR (data set ready/line enable) 126 | TIOCM_DTR DTR (data terminal ready) 127 | TIOCM_RTS RTS (request to send) 128 | TIOCM_ST Secondary TXD (transmit) 129 | TIOCM_SR Secondary RXD (receive) 130 | TIOCM_CTS CTS (clear to send) 131 | TIOCM_CAR DCD (data carrier detect) 132 | TIOCM_CD Synonym for TIOCM_CAR 133 | TIOCM_RNG RNG (ring) 134 | TIOCM_RI Synonym for TIOCM_RNG 135 | TIOCM_DSR DSR (data set ready) 136 | */ 137 | int kfx::RS232::IsCTSEnabled() { 138 | int status; 139 | status = ioctl(port, TIOCMGET, &status); 140 | return (status & TIOCM_CTS) ? 1 : 0; 141 | } 142 | 143 | // Sends a string to serial port till finding a '\0' 144 | void kfx::RS232::Print(const char *text) { 145 | while (*text != 0) Write(*(text++)); 146 | } 147 | 148 | #endif // kranfix_rs232_rs232_cc -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(RobotExample) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | 6 | # 设置example的路径 7 | set(EXAMPLE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/example) 8 | 9 | 10 | 11 | # 设置findpackage查找工程的目录 12 | # 1. 在工程中创建一个cmake文件夹 13 | # 2. 创建一个FindXXX.cmake的文件 14 | # 3. find_package(XXX REQUIRED) 15 | 16 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/cmake) 17 | 18 | 19 | 20 | 21 | find_package(Aubo REQUIRED) 22 | 23 | ## 设置以来的路径 24 | #set(DEPENDENTS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/dependents) 25 | # 26 | ## include 路径 27 | #set(PROJECT_INCLUDE_DIRS 28 | # ${DEPENDENTS_DIR}/libconfig/linux_x64/inc; 29 | # ${DEPENDENTS_DIR}/log4cplus/linux_x64/inc; 30 | # ${DEPENDENTS_DIR}/protobuf/linux_x64/google/protobuf/inc; 31 | # ${DEPENDENTS_DIR}/robotController/Header; 32 | # ${DEPENDENTS_DIR}/robotSDK/inc; 33 | # ${EXAMPLE_DIR} 34 | # ) 35 | # 36 | #include_directories(${PROJECT_INCLUDE_DIRS}) 37 | # 38 | # 39 | ## 设置查找的库组件 40 | #SET(PROJECT_LIB_COMPONENTS config;log4cplus;protobuf;protobuf-lite;protoc;auborobotcontroller) 41 | # 42 | #FOREACH(aubocomponent ${PROJECT_LIB_COMPONENTS}) 43 | # find_library(lib_${aubocomponent} NAMES ${aubocomponent} PATHS 44 | # ${DEPENDENTS_DIR}/protobuf/linux-x64/lib 45 | # ${DEPENDENTS_DIR}/libconfig/linux_x64/lib 46 | # ${DEPENDENTS_DIR}/log4cplus/linux_x64/lib 47 | # ${DEPENDENTS_DIR}/robotController/linux_x64 48 | # ${DEPENDENTS_DIR}/robotSDK/lib/linux_x64 49 | # ${EXAMPLE_DIR} 50 | # NO_DEFAULT_PATH) 51 | # set(PROJECT_LIBRARIES ${PROJECT_LIBRARIES};${lib_${aubocomponent}}) 52 | #ENDFOREACH() 53 | # 54 | 55 | 56 | # 指定example文件夹的源码位置 57 | aux_source_directory(example DIR_SRCS) 58 | include_directories(example) 59 | # 让example文件夹下的源文件和main.cpp一起编译 60 | add_executable(RobotExample main.cpp ${DIR_SRCS}) 61 | 62 | 63 | #link_directories(${DEPENDENTS_DIR}/robotSDK/lib/linux_x64) 64 | 65 | # 指定链接库.. 66 | target_link_libraries(RobotExample ${AUBO_LIBRARIES}) 67 | 68 | 69 | #add_executable(DrawCircle DrawCircle.cpp) 70 | #target_link_libraries(DrawCircle ${PROJECT_LIBRARIES}) 71 | 72 | # 73 | #SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/bin" CACHE PATH "Runtime directory" FORCE) 74 | #SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/libs" CACHE PATH "Library directory" FORCE) 75 | #SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/build/libs" CACHE PATH "Archive directory" FORCE) 76 | # 77 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") 78 | #set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/dependents/qt) 79 | # 80 | #file( 81 | # WRITE 82 | # ${CMAKE_CURRENT_SOURCE_DIR}/build/bin/qt.conf 83 | # "[Paths] 84 | #Plugins=${CMAKE_CURRENT_SOURCE_DIR}/dependents/qt/plugins" 85 | #) 86 | # 87 | # 88 | #set(CMAKE_INCLUDE_CURRENT_DIR ON) 89 | #set(CMAKE_AUTOMOC ON) 90 | #set(CMAKE_AUTOUIC ON) 91 | #set(CMAKE_AUTORCC ON) 92 | # 93 | # 94 | #find_package(Qt5Core REQUIRED) 95 | #find_package(Qt5Gui REQUIRED) 96 | #find_package(Qt5Widgets REQUIRED) 97 | #find_package(Qt5Network REQUIRED) 98 | # 99 | #SET(QT_LIBRARIES ${Qt5Core_LIBRARIES} ${Qt5Gui_LIBRARIES} ${Qt5Widgets_LIBRARIES}) 100 | # 101 | # 102 | ## 头文件和链接库的路径 103 | #include_directories(${Qt5Core_INCLUDE_DIRS} ${Qt5Gui_INCLUDE_DIRS} 104 | # ${Qt5Widgets_INCLUDE_DIRS} ${Qt5Network_INCLUDE_DIRS} ) 105 | 106 | # 导入QT相关的依赖 107 | find_package(Qt REQUIRED) 108 | 109 | # 引入Eigen3相关的库 110 | include_directories("/usr/include/eigen3") 111 | 112 | 113 | 114 | 115 | # 引入opencv相关以来 116 | #set(PREBUILT_DIR ${CMAKE_SOURCE_DIR}/dependents) 117 | #set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/cmake) 118 | #message(STATUS "test:${CMAKE_MODULE_PATH}") 119 | find_package(OpenCV REQUIRED) 120 | #include_directories(${OpenCV_INCLUDE_DIRS}) 121 | 122 | #find_package(PCL REQUIRED) 123 | 124 | # 引入kenect2相关的依赖 125 | #set(FREENECT_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/denpendents/include) 126 | #set(FREENECT_LIBRARIES ${PROJECT_SOURCE_DIR}/denpendents/lib) 127 | # 128 | #include_directories(${FREENECT_INCLUDE_DIRS}) 129 | #link_directories(${FREENECT_LIBRARIES}) 130 | FIND_PACKAGE(Freenect REQUIRED) 131 | 132 | 133 | # 引入robwork相关的依赖 134 | #set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/home/kaijun/workspace/RobWork/RobWork/cmake") 135 | #set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/home/kaijun/workspace/RobWork/RobWork/build") 136 | #message("CMAKE_MODULE_PATH:${CMAKE_MODULE_PATH}") 137 | # 138 | #set(CMAKE_PREFIX_PATH "/home/kaijun/workspace/RobWork/RobWork/build" ) 139 | 140 | 141 | FIND_PACKAGE(RobWork REQUIRED) 142 | message("ROBWORK_LIBRARIES:${ROBWORK_LIBRARIES}") 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | add_subdirectory(src) -------------------------------------------------------------------------------- /example/example_1.cpp: -------------------------------------------------------------------------------- 1 | #include "example_1.h" 2 | 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "util.h" 12 | 13 | 14 | #define SERVER_HOST "127.0.0.1" 15 | #define SERVER_PORT 8899 16 | 17 | 18 | Example_1::Example_1() 19 | { 20 | } 21 | 22 | 23 | void Example_1::RealTimeWaypointCallback(const aubo_robot_namespace::wayPoint_S *wayPointPtr, void *arg) 24 | { 25 | (void)arg; 26 | aubo_robot_namespace::wayPoint_S waypoint = *wayPointPtr; 27 | Util::printWaypoint(waypoint); 28 | } 29 | 30 | void Example_1::RealTimeEndSpeedCallback(double speed, void *arg) 31 | { 32 | (void)arg; 33 | std::cout<<"实时末端速度:"< 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "util.h" 10 | 11 | 12 | #define SERVER_HOST "192.168.17.250" 13 | #define SERVER_PORT 8899 14 | 15 | Example_3::Example_3() 16 | { 17 | } 18 | 19 | void Example_3::demo() 20 | { 21 | ServiceInterface robotService; 22 | 23 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 24 | 25 | /** 接口调用: 登录 ***/ 26 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 27 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 28 | { 29 | std::cerr<<"登录成功."< 3 | #include "Paw.h" 4 | 5 | struct HexCS { 6 | u_char *c; 7 | int len; 8 | 9 | HexCS(u_char *_c, int len) : c(_c), len(len) {} 10 | }; 11 | 12 | inline std::ostream &operator<<(std::ostream &o, const HexCS &hs) { 13 | for (int i = 0; i < hs.len; ++i) { 14 | o << "\\x" << setw(2) << setfill('0') << std::hex << (int) hs.c[i]; 15 | } 16 | return o; 17 | } 18 | 19 | inline HexCS hex(u_char *c, int len = 1) { 20 | return HexCS(c, len); 21 | } 22 | 23 | Paw::Paw(const char *port) { 24 | this->port = port; 25 | int baudrate = 115200; 26 | 27 | rs232 = new kfx::RS232(port, baudrate); 28 | if (!rs232->IsAvailable()) { 29 | cout << "Serial port %s is not available" << endl; 30 | return; 31 | } 32 | } 33 | 34 | Paw::~Paw() { 35 | delete (rs232); 36 | } 37 | 38 | int Paw::parseData(u_char *out, u_char cmd, PawData *data) { 39 | 40 | //帧头 41 | u_char header[] = {0xEB, 0x90}; 42 | 43 | //ID 44 | u_char id = 0x01; 45 | 46 | //data1 47 | u_char data1 = cmd; 48 | 49 | //data2 50 | u_char *data2 = nullptr; 51 | int data2_len = 0; 52 | 53 | // len 54 | u_char len; 55 | 56 | //如果有数据 57 | if (data) { 58 | data2_len = data->getDataLength(); 59 | data2 = new u_char[data2_len]; 60 | data->toData(data2); 61 | 62 | len = 1 + data2_len; 63 | } else { 64 | len = 1; 65 | } 66 | 67 | //check sum 68 | u_char check_sum = id + len + data1; 69 | if (data2) { 70 | for (int i = 0; i < data2_len; ++i) { 71 | check_sum += data2[i]; 72 | } 73 | } 74 | 75 | cout << "====================================" << endl; 76 | cout << "header: " << hex(header, 2) << endl; 77 | cout << "ID: " << hex(&id) << endl; 78 | cout << "len: " << hex(&len) << endl; 79 | cout << "data1: " << hex(&data1) << endl; 80 | cout << "data2: " << hex(data2, data2_len) << endl; 81 | cout << "check sum: " << hex(&check_sum) << endl; 82 | 83 | // out = new u_char[len + 5]; 84 | // out = (u_char *) alloca(len + 5); 85 | out[0] = header[0]; 86 | out[1] = header[1]; 87 | out[2] = id; 88 | out[3] = len; 89 | out[4] = data1; 90 | memcpy(out + 5, data2, data2_len); 91 | out[len + 4] = check_sum; 92 | 93 | return len + 5; 94 | 95 | // cout << "---------------- write ------------------" << endl; 96 | // cout << "write: " << hex(buf, len + 5) << endl; 97 | // long int writeLen = write(this->_fd, buf, len + 5); 98 | // cout << "writeLen: " << writeLen << endl; 99 | // 100 | // if (cmd == 0x01 || cmd == 0x12) { 101 | // usleep(1000000); 102 | // } else { 103 | // usleep(100000); 104 | // } 105 | // 106 | // u_char readBuffer[1024]; 107 | // int readLen = read(this->_fd, readBuffer, 1024); 108 | // 109 | // u_char readData[readLen]; 110 | // memcpy(readData, readBuffer, readLen); 111 | // 112 | // cout << "---------------- read -------------------" << endl; 113 | // cout << "readLen: " << readLen << endl; 114 | // cout << "read: " << hex(readData, readLen) << endl; 115 | // if (readLen < 0) { 116 | // perror("read STDIN_FILENO"); 117 | // exit(1); 118 | // } 119 | // 120 | // cout << "====================================" << endl; 121 | 122 | 123 | } 124 | 125 | bool Paw::catchXG(int speed, int power) { 126 | u_char cmd = 0x10; 127 | CatchXgData *data = new CatchXgData(speed, power); 128 | int size = 6 + data->getDataLength(); 129 | 130 | u_char *sendData = new u_char[size]; 131 | parseData(sendData, cmd, data); 132 | 133 | rs232->Write(sendData, size); 134 | 135 | return true; 136 | } 137 | 138 | bool Paw::release(int speed) { 139 | u_char cmd = 0x11; 140 | ReleaseData *data = new ReleaseData(speed); 141 | int size = 6 + data->getDataLength(); 142 | 143 | u_char *sendData = new u_char[size]; 144 | parseData(sendData, cmd, data); 145 | 146 | rs232->Write(sendData, size); 147 | 148 | return false; 149 | } 150 | 151 | void Paw::close() { 152 | rs232->Close(); 153 | } 154 | 155 | CatchXgData::CatchXgData(int speed, int power) : PawData() { 156 | if (speed > 1000) { 157 | speed = 1000; 158 | } 159 | 160 | if (speed < 1) { 161 | speed = 1; 162 | } 163 | this->speed = speed; 164 | 165 | if (power > 1000) { 166 | power = 1000; 167 | } 168 | 169 | if (power < 30) { 170 | power = 30; 171 | } 172 | this->power = power; 173 | } 174 | 175 | int CatchXgData::getDataLength() { 176 | return 4; 177 | } 178 | 179 | void CatchXgData::toData(u_char *data) { 180 | int speedLow = this->speed & 0x00ff; 181 | int speedHigh = this->speed >> 8; 182 | 183 | int powerLow = this->power & 0x00ff; 184 | int powerHigh = this->power >> 8; 185 | 186 | data[0] = speedLow; 187 | data[1] = speedHigh; 188 | data[2] = powerLow; 189 | data[3] = powerHigh; 190 | } 191 | 192 | ReleaseData::ReleaseData(int speed) : PawData() { 193 | if (speed > 1000) { 194 | speed = 1000; 195 | } 196 | 197 | if (speed < 1) { 198 | speed = 1; 199 | } 200 | this->speed = speed; 201 | } 202 | 203 | void ReleaseData::toData(u_char *data) { 204 | int low = this->speed & 0x00ff; 205 | int high = this->speed >> 8; 206 | 207 | data[0] = low; 208 | data[1] = high; 209 | } 210 | 211 | int ReleaseData::getDataLength() { 212 | return 2; 213 | } -------------------------------------------------------------------------------- /example/util.cpp: -------------------------------------------------------------------------------- 1 | #include "util.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | Util::Util() 11 | { 12 | } 13 | 14 | 15 | //打印路点信息 16 | void Util::printWaypoint(aubo_robot_namespace::wayPoint_S &wayPoint) 17 | { 18 | std::cout< 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "Robot.h" 14 | 15 | #include "utils/03_Camera2Base.h" 16 | /* 17 | 验证手眼标定 18 | */ 19 | using namespace std; 20 | using namespace cv; 21 | 22 | const Size patternSize(7,5); 23 | const int squareSize=30; 24 | 25 | bool findCorners(Mat &img,vector &corners) { 26 | Mat gray; 27 | // 将图片转成灰度图像 28 | cvtColor(img, gray, COLOR_BGR2GRAY); 29 | // 查找所有的角点 30 | bool patternfound = findChessboardCorners(gray, patternSize, corners); 31 | 32 | if (patternfound) { 33 | // 提高角点的精确度 34 | cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1)); 35 | // 将所有的焦点在原图中绘制出来 36 | drawChessboardCorners(img, patternSize, corners, patternfound); 37 | // 绘制完角点之后，显示原图 38 | 39 | circle(img,corners[0],5,Scalar(0,0,255),2); 40 | 41 | imshow("src", img); 42 | return true; 43 | }else{ 44 | cout<<"角点检测失败！"< corners,Mat depth){ 53 | for (int i = 0; i < corners.size(); ++i) { 54 | Point2f point1 = corners[i]; 55 | 56 | float d = depth.at(int(point1.y),int(point1.x)); 57 | if(d==0){ 58 | cout<<"行:"<setColorFrameListener(&listener); 104 | dev->setIrAndDepthFrameListener(&listener); 105 | //! [listeners] 106 | 107 | 108 | //! [start] 109 | dev->start(); 110 | std::cout << "device serial: " << dev->getSerialNumber() << std::endl; 111 | std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl; 112 | //! [start] 113 | 114 | 115 | libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams()); 116 | libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4), bigdepth(1920, 1082, 4); 117 | 118 | 119 | 120 | int keyCode=0; 121 | 122 | // while((keyCode=waitKey(100))!=27) { 123 | listener.waitForNewFrame(frames); 124 | libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color]; 125 | libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth]; 126 | 127 | registration->apply(rgb, depth, &undistorted, ®istered,true,&bigdepth); 128 | 129 | Mat rgbmat = Mat(rgb->height, rgb->width, CV_8UC4, rgb->data); 130 | Mat deptmat = Mat(depth->height, depth->width, CV_32FC1, depth->data); 131 | // Mat undistortedmat = Mat(undistorted.height, undistorted.width, CV_8UC4, undistorted.data); 132 | // Mat registeredmat = Mat(registered.height, registered.width, CV_8UC4, registered.data); 133 | Mat bigdepthmat = Mat(bigdepth.height, bigdepth.width, CV_32FC1 , bigdepth.data); 134 | 135 | 136 | 137 | 138 | for (int row = 0; row < bigdepthmat.rows; ++row) { 139 | for (int col = 0; col < bigdepthmat.cols; ++col) { 140 | float32 d = bigdepthmat.at(row, col); 141 | 142 | if(fpclassify(d) == FP_INFINITE || fpclassify(d) == NAN){ 143 | bigdepthmat.at(row, col)=0; 144 | } 145 | 146 | } 147 | } 148 | 149 | 150 | imwrite("./assets/color0.jpg",rgbmat); 151 | // 152 | // FileStorage fs("./assets/depth1.xml",FileStorage::WRITE); 153 | // fs<<"depth"< corners ; 163 | findCorners(rgbmat,corners); 164 | 165 | verifyCalibration(corners,bigdepthmat); 166 | 167 | imshow("src",rgbmat); 168 | // imshow("bigdepthmat",bigdepthmat); 169 | // 170 | listener.release(frames); 171 | 172 | // } 173 | 174 | waitKey(0); 175 | dev->stop(); 176 | dev->close(); 177 | 178 | return 0; 179 | } -------------------------------------------------------------------------------- /src/utils/05_DetectSquare.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by kaijun on 7/20/19. 3 | // 4 | #ifndef AUBOIUTILR_05_DetectSquare_H 5 | #define AUBOIUTILR_05_DetectSquare_H 6 | #include 7 | #include 8 | 9 | using namespace cv; 10 | using namespace std; 11 | /** 12 | * 小方块识别案例 13 | */ 14 | /** 15 | * 小方块颜色枚举类型 : 只会检测 红绿紫 三种颜色的小方块 16 | */ 17 | typedef enum{ 18 | RED = 0, 19 | GREEN = 1, 20 | PURPLE = 2 21 | }EnumColor; 22 | 23 | struct Square{ 24 | Rect rect; 25 | Point2d center; 26 | EnumColor color; 27 | }; 28 | 29 | 30 | /** 31 | * 计算三个点之间的cos值 32 | * @param p0 33 | * @param p1 34 | * @param p2 35 | * @return 36 | */ 37 | double angle_cos(Point p0,Point p1,Point p2){ 38 | Mat m0(p0); 39 | Mat m1(p1); 40 | Mat m2(p2); 41 | 42 | Mat d1 = m0-m1; 43 | Mat d2 = m2-m1; 44 | 45 | double result = d1.dot(d2)/sqrt(d1.dot(d1)*(d2.dot(d2))); 46 | 47 | // cout<<"result:"< points){ 56 | double maxCos=0; 57 | for (int i = 0; i < points.size(); ++i) { 58 | Point p0 = points[i]; 59 | Point p1 = points[(i+1)%4]; 60 | Point p2 = points[(i+2)%4]; 61 | 62 | double cos = angle_cos(p0,p1,p2); 63 | if(cos>maxCos){ 64 | maxCos=cos; 65 | } 66 | } 67 | return maxCos; 68 | } 69 | 70 | /** 71 | * 判定颜色是否在某个范围内 72 | * @param roi0 图片 73 | * @param lowerb 颜色最小值 74 | * @param upperb 颜色最大值 75 | * @return 是否在范围内 76 | */ 77 | bool detectColor(const Mat &roi0, const Scalar &lowerb, const Scalar &upperb) { 78 | Mat binaryResult; 79 | inRange(roi0, lowerb, upperb, binaryResult); 80 | uchar centerColor = binaryResult.at(int(binaryResult.rows / 2), int(binaryResult.cols / 2)); 81 | if (centerColor == 255) { 82 | 83 | return true; 84 | } 85 | return false; 86 | } 87 | /** 88 | * 判断方块的颜色 89 | */ 90 | EnumColor detectColor(const vector &c0, Mat &roi) { 91 | 92 | Rect rect0 = boundingRect(c0); 93 | rectangle(roi, rect0, Scalar(0, 0, 255), 2); 94 | 95 | Mat roi_hsv; 96 | cvtColor(roi, roi_hsv, COLOR_BGR2HSV); 97 | // 截取出小方块 98 | Mat roi0 = roi_hsv(rect0); 99 | 100 | // 紫色判定 101 | if(detectColor(roi0, Scalar(125, 43, 46), Scalar(155, 255, 255))){ 102 | cout << "颜色判断成功: 紫色"<< endl; 103 | return EnumColor::PURPLE; 104 | } 105 | // 绿色判定 106 | if(detectColor(roi0, Scalar(35, 43, 46), Scalar(77, 255, 255))){ 107 | cout << "颜色判断成功: 绿色"<< endl; 108 | return EnumColor::GREEN; 109 | } 110 | // 红色判定 111 | if(detectColor(roi0, Scalar(0, 43, 40), Scalar(10, 255, 255)) || detectColor(roi0, Scalar(156, 43, 40), Scalar(180, 255, 255))){ 112 | cout << "颜色判断成功: 红色"<< endl; 113 | return EnumColor::RED; 114 | } 115 | 116 | } 117 | 118 | void findSquareContours(const Mat &roi,vector> &squareContours) {// 对图像进行高斯滤波 119 | Mat gaussianImg; 120 | GaussianBlur(roi, gaussianImg, Size(5, 5), 0); 121 | 122 | // 获取颜色的三通道 123 | vector channels; 124 | split(gaussianImg, channels); 125 | 126 | // 在一个通道上面进行查找可能会失败，所以需要三通道 127 | for (int i = 0; i < 3; ++i) { 128 | // 取出其中某个通道的值 129 | Mat gray = channels[i]; 130 | // 对图像进行处理 131 | Mat binary; 132 | threshold(gray, binary, 100, 255, THRESH_BINARY); 133 | // 查找轮廓 134 | vector> contours; 135 | vector hierachy; 136 | // 查找轮廓 137 | findContours(binary, contours, hierachy, RETR_LIST, CHAIN_APPROX_SIMPLE); 138 | 139 | for (int j = 0; j < contours.size(); ++j) { 140 | vector contour = contours[j]; 141 | // 弧长 142 | double arclength = arcLength(contour, true); 143 | // 多边形你和 144 | vector newcontour; 145 | approxPolyDP(contour, newcontour, 0.02 * arclength, true); 146 | 147 | if (newcontour.size() == 4 && contourArea(newcontour) > 800 && isContourConvex(newcontour)) { 148 | // 计算cos值 cos90=0 149 | double maxCos = calculateMaxCos(newcontour); 150 | if (maxCos < 0.1) { 151 | 152 | 153 | // 由于是在3通道中找，所以会出现重复的方块 154 | Rect rect = boundingRect(newcontour); 155 | cout << "找到了一个矩形：" << rect << endl; 156 | bool isExist = false; 157 | for (int k = 0; k < squareContours.size(); ++k) { 158 | Rect oldRect = boundingRect(squareContours[k]); 159 | if (abs(oldRect.x - rect.x) < 10 && abs(oldRect.y - rect.y) < 10) { 160 | cout << "已经保存了这个轮廓" << endl; 161 | isExist = true; 162 | break; 163 | } 164 | } 165 | // 将查找到矩形轮廓保存起来 166 | if(!isExist){ 167 | squareContours.push_back(newcontour); 168 | } 169 | 170 | } 171 | } 172 | } 173 | } 174 | 175 | 176 | } 177 | 178 | 179 | 180 | 181 | void findSquare(Mat &src, vector &squares) {// 定义截取的位置 182 | int x = 700; 183 | int y = 250; 184 | int w = 700; 185 | int h = 500; 186 | Rect rect(x, y, w, h); 187 | // ROI截取 188 | Mat roi = src(rect); 189 | // imshow("roi", roi); 190 | 191 | // 用于保存查找出来满足条件的轮廓 192 | vector> squareContours; 193 | 194 | findSquareContours(roi, squareContours);// 判断轮廓的颜色类型 195 | for (int i = 0; i < squareContours.size(); ++i) { 196 | vector c0 = squareContours[i]; 197 | EnumColor colorIndex = detectColor(c0, roi); 198 | 199 | Rect rect = boundingRect(c0); 200 | // 保存找到的图形，注意坐标要对应原图中 201 | Square square; 202 | square.rect = Rect(x + rect.x, y + rect.y, rect.width, rect.height); 203 | square.color = colorIndex; 204 | square.center = Point2i(int(square.rect.x + rect.width/2),int(square.rect.y+rect.height/2)); 205 | 206 | squares.push_back(square); 207 | } 208 | 209 | // imshow("roi", roi); 210 | 211 | 212 | } 213 | 214 | 215 | 216 | #endif 217 | 218 | -------------------------------------------------------------------------------- /src/12_AutoGrab.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "Robot.h" 15 | #include "utils/03_Camera2Base.h" 16 | #include "utils/05_DetectSquare.h" 17 | /* 18 | 自动抓取小方块,该示例代码还未完成 19 | */ 20 | using namespace std; 21 | using namespace cv; 22 | 23 | // 垃圾桶的位置 24 | double rubbish[3][6] = { 25 | {-0.267778, -0.397378, 0.40, 175 * DE2RA, 0 * DE2RA, -90 * DE2RA}, 26 | {-0.267778, -0.269837, 0.40, 175 * DE2RA, 0 * DE2RA, -90 * DE2RA}, 27 | {-0.267778, -0.169321, 0.40, 175 * DE2RA, 0 * DE2RA, -90 * DE2RA}, 28 | }; 29 | 30 | // 吃屎姿态 31 | double defaultPos[6] = {-0.307233, -0.119000, 0.421263, 175 * DE2RA, 0 * DE2RA, -90 * DE2RA}; 32 | 33 | //Robot* robot = nullptr; 34 | Robot robot; 35 | 36 | void grabOneBox(Mat &depth, double defaultDepth, const Square &square); 37 | 38 | void grabOne(double *position, EnumColor colorIndex) { 39 | double zoffset = 0.15; 40 | // 1. 移动到物品上方的位置 41 | double upPos[6] = {0}; 42 | memcpy(upPos, position, sizeof(double) * 6); 43 | upPos[2] = upPos[2] + zoffset; 44 | robot.movel(upPos); 45 | // 2. 打开机械抓 46 | robot.openHand(); 47 | sleep(1); 48 | // 3. 向下移动到方块的位置 49 | robot.movel(position); 50 | // 4. 关闭机械抓 51 | robot.closeHand(); 52 | sleep(1); 53 | // 5. 移动到物品上方0.08 的位置 54 | robot.movel(upPos); 55 | 56 | // 6. 移动到对应颜色的垃圾桶上方 57 | double *rPos = rubbish[colorIndex]; 58 | robot.movel(rPos); 59 | // 7. 向下移动 60 | // double upRPos[6]={0}; 61 | // memcpy(upRPos,rPos, sizeof(double)*6); 62 | // upRPos[2]=upRPos[2]-0.02; 63 | // robot->movel(upRPos); 64 | // 8. 打开机械抓 65 | robot.openHand(); 66 | sleep(1); 67 | // 9. 移动到对应颜色的垃圾桶上方 68 | // robot->movel(upRPos); 69 | // 10. 关闭机械抓 70 | // robot->closeHand(); 71 | 72 | // 11. 休息0.5秒 73 | sleep(1); 74 | } 75 | 76 | 77 | void grabSquareToBox(Mat depth, vector squares) { 78 | 79 | double defaultDepth = 0.931642; 80 | 81 | for (int i = 0; i < squares.size(); ++i) { 82 | Square &square = squares[i]; 83 | 84 | grabOneBox(depth, defaultDepth, square); 85 | 86 | // break; 87 | } 88 | 89 | // 干完之后回到初始姿态 90 | robot.movel(defaultPos); 91 | } 92 | 93 | void grabOneBox(Mat &depth, double defaultDepth, const Square &square) { 94 | Point2d center = square.center; 95 | // 获取深度信息 96 | float d = depth.at(int(center.y), int(center.x)); 97 | 98 | if (d == 0) { 99 | cout << "深度信息丢失--->行:" << int(center.y) << "列：" << int(center.x) << endl; 100 | d = defaultDepth; 101 | // continue; 102 | } 103 | 104 | double p[3] = {center.x, center.y, d}; 105 | double cameraPos[3] = {0}; 106 | piexl2camera(p, cameraPos); 107 | 108 | cout << "相机坐标系：x:" << cameraPos[0] << " y:" << cameraPos[1] << " z:" << cameraPos[2] << endl; 109 | 110 | double basePos[3] = {0}; 111 | camera2base(cameraPos, basePos); 112 | 113 | cout << "Base坐标系：x:" << basePos[0] << " y:" << basePos[1] << " z:" << basePos[2] << endl; 114 | 115 | double toolLength = 0.177; 116 | double x = basePos[0] + 0.01; // 标定结果的偏移量 117 | double y = basePos[1]; 118 | double z = basePos[2] + toolLength; 119 | 120 | if (z > 0.005) { 121 | cout << "z值的求解：" << z << endl; 122 | } 123 | 124 | double pos[6] = {x, y, z, 180 * DE2RA, 0, -90 * DE2RA}; 125 | 126 | grabOne(pos, square.color); 127 | } 128 | 129 | 130 | int main(int argc, char **argv) { 131 | // robot = new Robot(); 132 | double defaultDepth = 0.931642; 133 | //! [context] 134 | libfreenect2::Freenect2 freenect2; 135 | libfreenect2::Freenect2Device *dev = nullptr; 136 | libfreenect2::PacketPipeline *pipeline = nullptr; 137 | //! [context] 138 | //! [discovery] 139 | if (freenect2.enumerateDevices() == 0) { 140 | std::cout << "no device connected!" << std::endl; 141 | return -1; 142 | } 143 | std::string serial = freenect2.getDefaultDeviceSerialNumber(); 144 | if (serial == "") return -1; 145 | cout << "The serial number is :" << serial << endl; 146 | //! [discovery] 147 | 148 | pipeline = new libfreenect2::OpenGLPacketPipeline(); 149 | 150 | dev = freenect2.openDevice(serial, pipeline); 151 | 152 | 153 | //! [listeners] 154 | libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Depth); 155 | libfreenect2::FrameMap frames; 156 | dev->setColorFrameListener(&listener); 157 | dev->setIrAndDepthFrameListener(&listener); 158 | //! [listeners] 159 | 160 | 161 | //! [start] 162 | dev->start(); 163 | std::cout << "device serial: " << dev->getSerialNumber() << std::endl; 164 | std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl; 165 | //! [start] 166 | 167 | // 内参文件路径 168 | cv::String inCailFilePath = "./assets/3DCameraInCailResult.xml"; 169 | FileStorage paramFs(inCailFilePath, FileStorage::READ); 170 | Mat cameraMatrix = cv::Mat_(3, 3); 171 | paramFs["cameraMatrix"] >> cameraMatrix; 172 | double camera_fx = cameraMatrix.at(0, 0); 173 | double camera_fy = cameraMatrix.at(1, 1); 174 | double camera_cx = cameraMatrix.at(0, 2); 175 | double camera_cy = cameraMatrix.at(1, 2); 176 | 177 | libfreenect2::Registration *registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams()); 178 | libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4), bigdepth(1920, 1082, 4); 179 | 180 | int keyCode = 0; 181 | while (true) { 182 | 183 | cout<<"执行代码"<apply(rgb, depth, &undistorted, ®istered, true, &bigdepth); 194 | 195 | Mat rgbmat = Mat(rgb->height, rgb->width, CV_8UC4, rgb->data); 196 | Mat deptmat = Mat(depth->height, depth->width, CV_32FC1, depth->data); 197 | Mat bigdepthmat = Mat(bigdepth.height, bigdepth.width, CV_32FC1, bigdepth.data); 198 | 199 | 200 | for (int row = 0; row < bigdepthmat.rows; ++row) { 201 | for (int col = 0; col < bigdepthmat.cols; ++col) { 202 | float32 d = bigdepthmat.at(row, col); 203 | if (fpclassify(d) == FP_INFINITE || fpclassify(d) == NAN) { 204 | bigdepthmat.at(row, col) = 0; 205 | } 206 | } 207 | } 208 | 209 | // 查找图中的小方块 210 | vector squares; 211 | findSquare(rgbmat, squares); 212 | 213 | // if (squares.size() == 0) { 214 | // continue; 215 | // } 216 | 217 | 218 | // 在原图中绘制方块 219 | for (int j = 0; j < squares.size(); ++j) { 220 | Square square = squares[j]; 221 | rectangle(rgbmat, square.rect, Scalar(0, 255, 255), 2); 222 | } 223 | 224 | imshow("srcrgb", rgbmat); 225 | keyCode = waitKey(100); 226 | 227 | 228 | if(keyCode==27){ 229 | break; 230 | } 231 | if(squares.size()>0) 232 | cout<<"机械臂要开始移动啦!"<stop(); 245 | dev->close(); 246 | 247 | // delete robot; 248 | return 0; 249 | } -------------------------------------------------------------------------------- /src/06_AutoGrab.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "Robot.h" 15 | #include "utils/03_Camera2Base.h" 16 | #include "utils/05_DetectSquare.h" 17 | // 导入方块识别的代码 18 | /* 19 | 自动抓取小方块 20 | 21 | 1. 图像识别： 识别出小方块 22 | 1. 截取工作台 23 | 2. 将彩色图转成灰度图 --> 24 | R ---> 检测方块 2 25 | G ---> 检测方块 1 26 | B ---> 检测方块 4 27 | 删除重复检测出来的方块 28 | 2.1 检测方块 29 | LSD 30 | 霍夫直线 31 | 多边形拟合方式 32 | 33 | 3. 判断方块的颜色 ： 34 | 截取方块彩图 28*28 35 | 将彩图转成HSV 36 | 二值图 inrange()判定颜色 14*14 255==> 判定成功 37 | 28*28*255 38 | 39 | 4. 保存所有方块的位置信息（相对于相机原图）， 保存方块的颜色信息 40 | 41 | 2. 操作机械臂去抓取 42 | 1.将相片坐标系（x,y）中的值 转成 相机坐标系中（X,Y,Z）真实值 43 | 内存标定： 张氏棋盘法 ----》 findCheshBoard ---> calibrationCamera 44 | 2.将 相机坐标系XYZ 转成 Base坐标系 45 | 外参标定： AX=XB 46 | 47 | 3. 遍历所有的方块 48 | 将方块坐标转成Base 49 | 3.1 movel到方块上方 50 | 3.2 打开机械抓 51 | 3.3 向下移动机械臂 52 | 3.4 关闭机械抓 53 | 3.5 movel到方块上方 54 | 3.6 移动到对应垃圾桶上方 55 | 3.7 打开机械抓 56 | 57 | 58 | */ 59 | using namespace std; 60 | using namespace cv; 61 | 62 | // 垃圾桶的位置 63 | double rubbish[3][6]={ 64 | {-0.267778,-0.397378,0.40,175*DE2RA,0*DE2RA,-90*DE2RA}, 65 | {-0.267778,-0.269837,0.40,175*DE2RA,0*DE2RA,-90*DE2RA}, 66 | {-0.267778,-0.169321,0.40,175*DE2RA,0*DE2RA,-90*DE2RA}, 67 | }; 68 | 69 | // 吃屎姿态 70 | double defaultPos[6]={-0.307233,-0.119000,0.421263,175*DE2RA,0*DE2RA,-90*DE2RA}; 71 | 72 | //Robot* robot = nullptr; 73 | Robot robot; 74 | 75 | void grabOne(double* position,EnumColor colorIndex){ 76 | double zoffset=0.15; 77 | // 1. 移动到物品上方的位置 78 | double upPos[6]={0}; 79 | memcpy(upPos,position, sizeof(double)*6); 80 | upPos[2]=upPos[2]+zoffset; 81 | robot.movel(upPos); 82 | // 2. 打开机械抓 83 | robot.openHand(); 84 | sleep(1); 85 | // 3. 向下移动到方块的位置 86 | robot.movel(position); 87 | // 4. 关闭机械抓 88 | robot.closeHand(); 89 | sleep(1); 90 | // 5. 移动到物品上方0.08 的位置 91 | robot.movel(upPos); 92 | 93 | // 6. 移动到对应颜色的垃圾桶上方 94 | double* rPos = rubbish[colorIndex]; 95 | robot.movel(rPos); 96 | // 7. 向下移动 97 | // double upRPos[6]={0}; 98 | // memcpy(upRPos,rPos, sizeof(double)*6); 99 | // upRPos[2]=upRPos[2]-0.02; 100 | // robot->movel(upRPos); 101 | // 8. 打开机械抓 102 | robot.openHand(); 103 | sleep(1); 104 | // 9. 移动到对应颜色的垃圾桶上方 105 | // robot->movel(upRPos); 106 | // 10. 关闭机械抓 107 | // robot->closeHand(); 108 | 109 | // 11. 休息0.5秒 110 | sleep(1); 111 | } 112 | 113 | 114 | 115 | 116 | void grabSquareToBox(Mat depth,vector squares) { 117 | 118 | double defaultDepth=0.931642; 119 | 120 | for (int i = 0; i < squares.size(); ++i) { 121 | Point2d center = squares[i].center; 122 | // 获取深度信息 123 | float d = depth.at(int(center.y),int(center.x)); 124 | 125 | if(d==0){ 126 | cout<<"深度信息丢失--->行:"<0.005){ 148 | cout<<"z值的求解："<setColorFrameListener(&listener); 191 | dev->setIrAndDepthFrameListener(&listener); 192 | //! [listeners] 193 | 194 | 195 | //! [start] 196 | dev->start(); 197 | std::cout << "device serial: " << dev->getSerialNumber() << std::endl; 198 | std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl; 199 | //! [start] 200 | 201 | // 内参文件路径 202 | cv::String inCailFilePath="./assets/3DCameraInCailResult.xml"; 203 | FileStorage paramFs(inCailFilePath,FileStorage::READ); 204 | Mat cameraMatrix=cv::Mat_(3, 3); 205 | paramFs["cameraMatrix"]>>cameraMatrix; 206 | double camera_fx = cameraMatrix.at(0,0); 207 | double camera_fy = cameraMatrix.at(1,1); 208 | double camera_cx = cameraMatrix.at(0,2); 209 | double camera_cy = cameraMatrix.at(1,2); 210 | 211 | libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams()); 212 | 213 | libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4), bigdepth(1920, 1082, 4); 214 | 215 | 216 | 217 | 218 | // while((keyCode=waitKey(100))!=27) { 219 | listener.waitForNewFrame(frames); 220 | libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color]; 221 | libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth]; 222 | 223 | registration->apply(rgb, depth, &undistorted, ®istered,true,&bigdepth); 224 | 225 | Mat rgbmat = Mat(rgb->height, rgb->width, CV_8UC4, rgb->data); 226 | Mat deptmat = Mat(depth->height, depth->width, CV_32FC1, depth->data); 227 | // Mat undistortedmat = Mat(undistorted.height, undistorted.width, CV_8UC4, undistorted.data); 228 | // Mat registeredmat = Mat(registered.height, registered.width, CV_8UC4, registered.data); 229 | Mat bigdepthmat = Mat(bigdepth.height, bigdepth.width, CV_32FC1 , bigdepth.data); 230 | 231 | 232 | 233 | 234 | for (int row = 0; row < bigdepthmat.rows; ++row) { 235 | for (int col = 0; col < bigdepthmat.cols; ++col) { 236 | float32 d = bigdepthmat.at(row, col); 237 | 238 | if(fpclassify(d) == FP_INFINITE || fpclassify(d) == NAN){ 239 | bigdepthmat.at(row, col)=0; 240 | } 241 | 242 | } 243 | } 244 | 245 | 246 | 247 | // imwrite("./assets/color0.jpg",rgbmat); 248 | 249 | 250 | listener.release(frames); 251 | dev->stop(); 252 | dev->close(); 253 | 254 | // 查找图中的小方块 255 | vector squares; 256 | findSquare(rgbmat,squares); 257 | 258 | // 在原图中绘制方块 259 | for (int j = 0; j < squares.size(); ++j) { 260 | Square square = squares[j]; 261 | rectangle(rgbmat, square.rect, Scalar(0, 255, 255), 2); 262 | } 263 | 264 | imshow("srcrgb",rgbmat); 265 | 266 | // 为了防止QT界面被阻塞,所以开启子线程 267 | std::thread t1([=](){ 268 | grabSquareToBox(bigdepthmat,squares); 269 | }); 270 | t1.detach(); 271 | 272 | waitKey(0); 273 | 274 | 275 | // delete robot; 276 | return 0; 277 | } -------------------------------------------------------------------------------- /example/robottooliotestdemo.cpp: -------------------------------------------------------------------------------- 1 | #include "robottooliotestdemo.h" 2 | #include 3 | #include 4 | 5 | #define SERVER_HOST "127.0.0.1" 6 | //#define SERVER_HOST "192.168.1.5" 7 | #define SERVER_PORT 8899 8 | 9 | 10 | 11 | 12 | RobotToolIoTestDemo::RobotToolIoTestDemo() 13 | { 14 | m_robotService = new ServiceInterface(); 15 | } 16 | 17 | 18 | int RobotToolIoTestDemo::loginAPI() 19 | { 20 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 21 | 22 | /** 登录 ***/ 23 | ret = m_robotService->robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 24 | 25 | if(ret != aubo_robot_namespace::InterfaceCallSuccCode) 26 | { 27 | std::cerr<<"ERROR:登录失败."<robotServiceLogout(); 40 | 41 | if(ret != aubo_robot_namespace::InterfaceCallSuccCode) 42 | { 43 | std::cerr<<"ERROR:退出登录失败."<robotServiceSetToolPowerVoltageType(type); 55 | 56 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 57 | { 58 | std::cout<<"INFO:设置工具端电压类型成功. 当前设置的类型:"<robotServiceGetToolPowerVoltageType(type); 77 | 78 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 79 | { 80 | std::cout<<"INFO:获取电源电压类型成功. 结果为:"<robotServiceGetToolPowerVoltageStatus(ToolPowerVoltageStatus); 97 | 98 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 99 | { 100 | std::cout<<"INFO:获取工具端电源电压成功 电源电压:"< statusVector; 116 | 117 | ret = m_robotService->robotServiceGetAllToolDigitalIOStatus(statusVector); 118 | 119 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 120 | { 121 | std::cout<<"INFO:工具端数字量IO的状态:"<robotServiceSetToolDigitalIOType(addr, value)) == aubo_robot_namespace::InterfaceCallSuccCode) 144 | // { 145 | // std::cout<<"INFO:设置工具端数字量IO的类型成功. 当前设置 addr:"<robotServiceSetToolDOStatus(name, value)) == aubo_robot_namespace::InterfaceCallSuccCode) 160 | // { 161 | // std::cout<<"INFO:设置工具端数字量IO的状态成功. 当前设置 name:"<robotServiceSetToolDOStatus(addr, value)) == aubo_robot_namespace::InterfaceCallSuccCode) 176 | { 177 | std::cout<<"INFO:设置工具端数字量IO的状态成功. 当前设置 addr:"< toolAIStatusVector; 192 | ret = m_robotService->robotServiceGetAllToolAIStatus(toolAIStatusVector); 193 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 194 | { 195 | for(int i=0;i<(int)toolAIStatusVector.size();i++) 196 | { 197 | std::cout <<"名称:"< 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | //#include 9 | #include 10 | #include 11 | 12 | #define DE2RA M_PI/180 13 | /** 14 | * 双臂机器人的基本操作 15 | */ 16 | using namespace std; 17 | // 姿态1: 18 | double joint1[] = {18.819843*DE2RA, -17.610569*DE2RA, -109.448857*DE2RA,-3.861306*DE2RA, -88.872687*DE2RA, -160.007877*DE2RA}; 19 | double point1[] = {-0.227602, -0.205245, 0.312001,-178.483566*DE2RA, 1.75*DE2RA, 88.870956*DE2RA}; 20 | // 21 | //// 姿态2: 从点1的位置到点2的位置，逆解的时候会出现大翻转 22 | double joint2[] = {87.401334*DE2RA, -9.414021*DE2RA, -106.291800*DE2RA,-11.284257*DE2RA, -88.872687*DE2RA, -160.009120*DE2RA}; 23 | double point2[] = {0.106190, -0.325713, 0.302872,-176.938768*DE2RA, 2.561946*DE2RA, 157.537857*DE2RA}; 24 | 25 | 26 | // 右臂待机点 27 | double rightStandByJoints[]={-95.951385*DE2RA,-70.584759*DE2RA,52.387425*DE2RA,-29.676421*DE2RA,133.899589*DE2RA,174.029901*DE2RA}; 28 | double rightStandByPoints[]={-0.100499,-0.444972,-0.016334,111.274101*DE2RA,24.156603*DE2RA,135.808929*DE2RA}; 29 | 30 | 31 | 32 | 33 | // 右臂下移动到瓶子位置 34 | double rightJoint1[]={-130.533207*DE2RA,-58.556953*DE2RA,50.121663*DE2RA,22.039323*DE2RA,86.159913*DE2RA,136.317503*DE2RA}; 35 | double rightPoint1[]={-0.470703,-0.357681,0.125994,179.782471*DE2RA,-5.096696*DE2RA,-176.953720*DE2RA}; 36 | 37 | 38 | 39 | // 左臂待机点 40 | double leftStandByJoints[]={67.199472*DE2RA,68.128173*DE2RA,-53.337565*DE2RA,1.482907*DE2RA,-71.974150*DE2RA,174.005954*DE2RA}; 41 | double leftStandByPoints[]={-0.058421,-0.521114,-0.008554,147.223282*DE2RA,18.363787*DE2RA,152.379044*DE2RA}; 42 | 43 | 44 | // 创建一个机器人对象 45 | //Robot leftRobot; 46 | Robot* rightRobot = new Robot("192.168.17.250",8899,"admin","1","/dev/ttyUSB0"); 47 | Robot* leftRobot = new Robot("192.168.17.251",8899,"admin","1","/dev/ttyUSB1"); 48 | 49 | // 获取随机数 0.01~0.09之间的随机数 50 | double getRandom(){ 51 | double v = random()%10*1.0/100; 52 | return v; 53 | } 54 | 55 | void doTest(){ 56 | leftRobot->movej(leftStandByJoints); 57 | rightRobot->movej(rightStandByJoints); 58 | } 59 | 60 | bool standFlag = true; 61 | 62 | 63 | 64 | 65 | void rightHand(){ 66 | //1. 移动到要抓取水杯的位置点1 67 | rightRobot->movej(rightJoint1); 68 | //2. 调整姿态 69 | 70 | //3. 打开机械爪 71 | rightRobot->openHand(); 72 | 73 | //4. 移动到水杯的位置 74 | // double joint2[]={-132.473636*DE2RA,-77.131960*DE2RA,49.483187*DE2RA,43.471070*DE2RA,89.686859*DE2RA,136.333485*DE2RA}; 75 | double joint2[]={-125.779955*DE2RA,-72.321130*DE2RA,60.970462*DE2RA,46.974896*DE2RA,87.546564*DE2RA,140.727988*DE2RA}; 76 | rightRobot->movej(joint2); 77 | // double rightPoint2[aubo_robot_namespace::ARM_DOF] = {0}; 78 | // memcpy(rightPoint2,rightJoint1,6); 79 | // rightPoint2[2]=-0.023191; 80 | // rightRobot->movel(rightPoint2); 81 | 82 | 83 | //5. 关闭机械爪 84 | } 85 | 86 | void rightHandClose(){ 87 | rightRobot->closeHand(); 88 | sleep(1); 89 | //6. 保持当前姿态抬起机械爪 90 | // double joint3[]={-113.827179*DE2RA,-56.857370*DE2RA,118.371741*DE2RA,87.633779*DE2RA,89.724896*DE2RA,158.083040*DE2RA}; 91 | // rightRobot->movej(joint3); 92 | // 93 | // sleep(1); 94 | 95 | // 向外移动一点 96 | // double joint4[]={-120.287991*DE2RA,-71.269684*DE2RA,86.387522*DE2RA,79.171585*DE2RA,89.792153*DE2RA,148.444517*DE2RA}; 97 | double joint4[]={-121.919901*DE2RA,-71.295113*DE2RA,87.380324*DE2RA,78.732137*DE2RA,91.891043*DE2RA,148.444517*DE2RA}; 98 | rightRobot->movej(joint4); 99 | } 100 | 101 | 102 | 103 | void leftHand(){ 104 | //7. 移动到瓶盖上方 105 | double joint4[]={61.348752*DE2RA,91.817693*DE2RA,-61.696989*DE2RA,29.160892*DE2RA,-31.649215*DE2RA,175*DE2RA}; 106 | 107 | leftRobot->movej(joint4); 108 | //8. 调整姿态 109 | cout<<"左臂操作："<openHand(); 112 | //10. 向下移动机械爪 113 | sleep(1); 114 | 115 | //12. 调整姿态，开始扭瓶盖 116 | // double newJoints[]={67.196105*DE2RA,85.900201*DE2RA,-82.755904*DE2RA,4.626307*DE2RA,-18.818792*DE2RA,174*DE2RA}; 117 | double joint5[6]={76.707642*DE2RA,85.768379*DE2RA,-81.199024*DE2RA,3.780824*DE2RA,-12.012680*DE2RA,174*DE2RA}; 118 | 119 | leftRobot->movej(joint5); 120 | } 121 | 122 | void leftHandClose(){ 123 | 124 | //11. 关闭机械爪 125 | leftRobot->closeHand(); 126 | 127 | } 128 | 129 | 130 | 131 | //double newJoints[]={67.196105*DE2RA,85.900201*DE2RA,-82.755904*DE2RA,4.626307*DE2RA,-18.818792*DE2RA,174*DE2RA}; 132 | double joint5[]={76.707642*DE2RA,85.768379*DE2RA,-81.199024*DE2RA,3.780824*DE2RA,-12.012680*DE2RA,174*DE2RA}; 133 | void leftHandOpenCap(){ 134 | //13. 转动瓶盖 -175 - 175 135 | joint5[5]-=10*DE2RA; 136 | cout<movej(joint5); 138 | } 139 | 140 | 141 | void shougong(){ 142 | double newJoints[]={-111.990145*DE2RA,-76.038481*DE2RA,18.184523*DE2RA,86.308074*DE2RA,91.949268*DE2RA,174*DE2RA}; 143 | rightRobot->movej(newJoints); 144 | rightRobot->openHand(); 145 | 146 | // 左臂 移动到垃圾1 正上方 147 | double laji1[]={90.167923*DE2RA,39.253316*DE2RA,-125.998316*DE2RA,2.935550*DE2RA,1.631492*DE2RA,174.009519*DE2RA}; 148 | leftRobot->movej(laji1); 149 | 150 | // 向下稍微移动一点 151 | double laji2[]={103.767956*DE2RA,42.563587*DE2RA,-120.275806*DE2RA,15.148304*DE2RA,21.464110*DE2RA,174.009519*DE2RA}; 152 | leftRobot->movej(laji2); 153 | 154 | // 扔掉瓶盖 155 | leftRobot->openHand(); 156 | sleep(1); 157 | leftRobot->closeHand(); 158 | // 左臂回到待机状态 159 | leftRobot->movej(leftStandByJoints); 160 | 161 | 162 | } 163 | 164 | /* 自动打开盖子*/ 165 | void autoOpenGap(){ 166 | rightHand(); 167 | rightHandClose(); 168 | leftHand(); 169 | leftHandClose(); 170 | sleep(2); 171 | for (int i = 0; i < 35; ++i) { 172 | leftHandOpenCap(); 173 | } 174 | 175 | shougong(); 176 | } 177 | 178 | 179 | 180 | int main(int argc,char **argv){ 181 | // 创建Qt应用 182 | QApplication app(argc,argv); 183 | // 创建QT窗口 184 | QWidget widget; 185 | widget.setFixedWidth(300); 186 | // 创建布局 187 | QVBoxLayout layout; 188 | // 设置窗口布局为layout 189 | widget.setLayout(&layout); 190 | 191 | QPushButton testBtn("回到待机状态"); 192 | layout.addWidget(&testBtn); 193 | QObject::connect(&testBtn,&QPushButton::clicked,doTest); 194 | 195 | //抓取的按钮 196 | QPushButton putUpBtn("右臂准备抓取"); 197 | layout.addWidget(&putUpBtn); 198 | QObject::connect(&putUpBtn,&QPushButton::clicked,rightHand); 199 | 200 | QPushButton closeBtn("右臂抓取"); 201 | layout.addWidget(&closeBtn); 202 | QObject::connect(&closeBtn,&QPushButton::clicked,rightHandClose); 203 | 204 | 205 | 206 | 207 | 208 | //放下的按钮 209 | QPushButton putDownBtn("左臂准备扭瓶盖"); 210 | layout.addWidget(&putDownBtn); 211 | QObject::connect(&putDownBtn,&QPushButton::clicked,leftHand); 212 | //放下的按钮 213 | QPushButton leftHandCloseBtn("左臂夹住瓶盖"); 214 | layout.addWidget(&leftHandCloseBtn); 215 | QObject::connect(&leftHandCloseBtn,&QPushButton::clicked,leftHandClose); 216 | 217 | //左臂扭瓶盖 218 | QPushButton leftOpenBtn("扭瓶盖"); 219 | layout.addWidget(&leftOpenBtn); 220 | QObject::connect(&leftOpenBtn,&QPushButton::clicked,leftHandOpenCap); 221 | 222 | //左臂扭瓶盖 223 | QPushButton shougongBtn("收工"); 224 | layout.addWidget(&shougongBtn); 225 | QObject::connect(&shougongBtn,&QPushButton::clicked,shougong); 226 | 227 | //左臂扭瓶盖 228 | QPushButton autoBtn("人工智能"); 229 | layout.addWidget(&autoBtn); 230 | QObject::connect(&autoBtn,&QPushButton::clicked,autoOpenGap); 231 | 232 | 233 | //打开机械爪 234 | QPushButton openHand("打开右臂机械爪"); 235 | layout.addWidget(&openHand); 236 | QObject::connect(&openHand,&QPushButton::clicked,[&](){rightRobot->openHand();}); 237 | 238 | //关闭机械爪 239 | QPushButton closeHand("关闭右臂机械爪"); 240 | layout.addWidget(&closeHand); 241 | QObject::connect(&closeHand,&QPushButton::clicked,[&](){rightRobot->closeHand();}); 242 | 243 | //打开机械爪 244 | QPushButton openLeftHand("打开左臂机械爪"); 245 | layout.addWidget(&openLeftHand); 246 | QObject::connect(&openLeftHand,&QPushButton::clicked,[&](){leftRobot->openHand();}); 247 | 248 | //关闭机械爪 249 | QPushButton closeLeftHand("关闭左臂臂机械爪"); 250 | layout.addWidget(&closeLeftHand); 251 | QObject::connect(&closeLeftHand,&QPushButton::clicked,[&](){leftRobot->closeHand();}); 252 | 253 | 254 | // //右臂待机状态 255 | QPushButton standByBtn("测试反解"); 256 | layout.addWidget(&standByBtn); 257 | QObject::connect(&standByBtn,&QPushButton::clicked,[&](){ 258 | 259 | double joint11[6] = {18.819843*DE2RA, -17.610569*DE2RA, -109.448857*DE2RA,-3.861306*DE2RA, -88.872687*DE2RA, -160.007877*DE2RA}; 260 | double point11[6] = {-0.227602, -0.205245, 0.312001,-178.483566*DE2RA, 1.75*DE2RA, 88.870956*DE2RA}; 261 | 262 | double resultJoints[6]={0}; 263 | double xyz[]={-0.227602, -0.205245, 0.312001}; 264 | double rpy[]={-178.483566*DE2RA, 1.75*DE2RA, 88.870956*DE2RA}; 265 | rightRobot->IK(xyz,rpy,resultJoints); 266 | for (int i = 0; i < 6; ++i) { 267 | cout<<"原值："<FK(resultJoints,resultPoint1); 273 | for (int i = 0; i < 6; ++i) { 274 | cout<<"原值："< 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "util.h" 10 | 11 | #define SERVER_HOST "127.0.0.1" 12 | #define SERVER_PORT 8899 13 | 14 | Example_5::Example_5() 15 | { 16 | } 17 | 18 | void Example_5::demo() 19 | { 20 | ServiceInterface robotService; 21 | 22 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 23 | 24 | /** 接口调用: 登录 ***/ 25 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 26 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 27 | { 28 | std::cerr<<"登录成功."< 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | //#include 11 | //#include 12 | #include 13 | 14 | using namespace std; 15 | 16 | 17 | 18 | Robot::Robot() { 19 | loginRobot(); 20 | initRobot(); 21 | cout<<"创建了Robot实例"<ipaddr = ipaddr; 26 | this->port = port; 27 | this->username = username; 28 | this->password = password; 29 | this->deviceName = deviceName; 30 | 31 | loginRobot(); 32 | initRobot(); 33 | } 34 | 35 | 36 | 37 | void Robot::loginRobot() { 38 | int ret = robotService.robotServiceLogin(ipaddr,port,username,password); 39 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode){ 40 | cout<<"登录成功!"<hand = new Hand(deviceName); 50 | 51 | /** 如果是连接真实机械臂，需要对机械臂进行初始化　**/ 52 | aubo_robot_namespace::ROBOT_SERVICE_STATE result; 53 | 54 | //工具动力学参数 55 | aubo_robot_namespace::ToolDynamicsParam toolDynamicsParam; 56 | memset(&toolDynamicsParam, 0, sizeof(toolDynamicsParam)); 57 | 58 | int ret = robotService.rootServiceRobotStartup(toolDynamicsParam/**工具动力学参数**/, 59 | 6 /*碰撞等级*/, 60 | true /*是否允许读取位姿　默认为true*/, 61 | true, /*保留默认为true */ 62 | 1000, /*保留默认为1000 */ 63 | result); /*机械臂初始化*/ 64 | if(ret != aubo_robot_namespace::InterfaceCallSuccCode) 65 | { 66 | std::cerr<<"机械臂初始化失败:"<IK(xyz,rpy,joints); 183 | // 移动机械臂 184 | int ret = robotService.robotServiceLineMove(joints,true); 185 | 186 | if(ret != aubo_robot_namespace::InterfaceCallSuccCode){ 187 | cout<<"movel调用失败："<hand->send(CMD_MC_MOVE_RELEASE,new ReleaseData(1000)); 271 | }); 272 | t.detach();*/ 273 | 274 | // Hand hand = Hand::getInstance(deviceName); 275 | // bool flag = hand.send(CMD_MC_MOVE_RELEASE, new ReleaseData(1000)); 276 | // usleep(500*1000); 277 | this->paw->release(1000); 278 | } 279 | 280 | void Robot::closeHand(){ 281 | cout<<"关闭机械抓"<hand->send(CMD_MC_MOVE_CATCH_XG,new CatchXgData(1000,1000)); 286 | }); 287 | t.detach();*/ 288 | // Hand hand = Hand::getInstance(deviceName); 289 | // bool flag = hand.send(CMD_MC_MOVE_CATCH_XG,new CatchXgData(1000,1000)); 290 | // usleep(500*1000); 291 | 292 | this->paw->catchXG(); 293 | } 294 | 295 | Robot::~Robot() { 296 | cerr<<"机械臂被释放了"<paw->close(); 301 | // hand->disconnect(); 302 | // delete hand; 303 | robotService.robotServiceLogout(); 304 | } 305 | 306 | ServiceInterface Robot::getService(){ 307 | 308 | return robotService; 309 | } -------------------------------------------------------------------------------- /src/TestDoubleRobot备份.cpp: -------------------------------------------------------------------------------- 1 | #include "Robot.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | //#include 9 | #include 10 | #include 11 | 12 | #define DE2RA M_PI/180 13 | 14 | using namespace std; 15 | // 姿态1: 16 | double joint1[] = {18.819843*DE2RA, -17.610569*DE2RA, -109.448857*DE2RA,-3.861306*DE2RA, -88.872687*DE2RA, -160.007877*DE2RA}; 17 | double point1[] = {-0.227602, -0.205245, 0.312001,-178.483566*DE2RA, 1.75*DE2RA, 88.870956*DE2RA}; 18 | // 19 | //// 姿态2: 从点1的位置到点2的位置，逆解的时候会出现大翻转 20 | double joint2[] = {87.401334*DE2RA, -9.414021*DE2RA, -106.291800*DE2RA,-11.284257*DE2RA, -88.872687*DE2RA, -160.009120*DE2RA}; 21 | double point2[] = {0.106190, -0.325713, 0.302872,-176.938768*DE2RA, 2.561946*DE2RA, 157.537857*DE2RA}; 22 | 23 | 24 | // 右臂待机点 25 | double rightStandByJoints[]={-95.951385*DE2RA,-70.584759*DE2RA,52.387425*DE2RA,-29.676421*DE2RA,133.899589*DE2RA,174.029901*DE2RA}; 26 | double rightStandByPoints[]={-0.100499,-0.444972,-0.016334,111.274101*DE2RA,24.156603*DE2RA,135.808929*DE2RA}; 27 | 28 | 29 | 30 | 31 | // 右臂下移动到瓶子位置 32 | double rightJoint1[]={-130.533207*DE2RA,-58.556953*DE2RA,50.121663*DE2RA,22.039323*DE2RA,86.159913*DE2RA,136.317503*DE2RA}; 33 | double rightPoint1[]={-0.470703,-0.357681,0.125994,179.782471*DE2RA,-5.096696*DE2RA,-176.953720*DE2RA}; 34 | 35 | 36 | 37 | // 左臂待机点 38 | double leftStandByJoints[]={67.199472*DE2RA,68.128173*DE2RA,-53.337565*DE2RA,1.482907*DE2RA,-71.974150*DE2RA,174.005954*DE2RA}; 39 | double leftStandByPoints[]={-0.058421,-0.521114,-0.008554,147.223282*DE2RA,18.363787*DE2RA,152.379044*DE2RA}; 40 | 41 | 42 | // 创建一个机器人对象 43 | //Robot leftRobot; 44 | Robot* rightRobot = new Robot("192.168.17.250",8899,"admin","1","/dev/ttyUSB0"); 45 | Robot* leftRobot = new Robot("192.168.17.251",8899,"admin","1","/dev/ttyUSB1"); 46 | 47 | // 获取随机数 0.01~0.09之间的随机数 48 | double getRandom(){ 49 | double v = random()%10*1.0/100; 50 | return v; 51 | } 52 | 53 | void doTest(){ 54 | leftRobot->movej(leftStandByJoints); 55 | rightRobot->movej(rightStandByJoints); 56 | } 57 | 58 | bool standFlag = true; 59 | 60 | /*双臂待机表演*/ 61 | void standbyAction(Robot* robot,double* standPoint){ 62 | robot->movej(standPoint); 63 | // standFlag = true; 64 | // // 移动到待机的位置 65 | // robot->movel(standPoint); 66 | // 67 | // thread standThread([&](){ 68 | // //机械手开始表演 69 | // thread threadHand([&](){ 70 | // while(standFlag){ 71 | // robot->openHand(); 72 | // sleep(1.5); 73 | // robot->closeHand(); 74 | // sleep(2); 75 | // } 76 | // }); 77 | // threadHand.detach(); 78 | // 79 | // 80 | // while(standFlag){ 81 | // double randPos[aubo_robot_namespace::ARM_DOF]={0}; 82 | // 83 | // randPos[0] = getRandom()+standPoint[0]; 84 | // randPos[1] = getRandom()+standPoint[1]; 85 | // randPos[2] = getRandom()+standPoint[2]; 86 | // 87 | // randPos[3] = random()%35*DE2RA+standPoint[3]; 88 | // randPos[4] = random()%35*DE2RA+standPoint[4]; 89 | // randPos[5] = random()%35*DE2RA+standPoint[5]; 90 | // robot->movel(randPos); 91 | // sleep(2); 92 | // } 93 | // }); 94 | //// 95 | // standThread.detach(); 96 | } 97 | 98 | 99 | void rightHand(){ 100 | //1. 移动到要抓取水杯的位置点1 101 | rightRobot->movej(rightJoint1); 102 | //2. 调整姿态 103 | 104 | //3. 打开机械爪 105 | rightRobot->openHand(); 106 | 107 | //4. 移动到水杯的位置 108 | // double joint2[]={-132.473636*DE2RA,-77.131960*DE2RA,49.483187*DE2RA,43.471070*DE2RA,89.686859*DE2RA,136.333485*DE2RA}; 109 | double joint2[]={-125.779955*DE2RA,-72.321130*DE2RA,60.970462*DE2RA,46.974896*DE2RA,87.546564*DE2RA,140.727988*DE2RA}; 110 | rightRobot->movej(joint2); 111 | // double rightPoint2[aubo_robot_namespace::ARM_DOF] = {0}; 112 | // memcpy(rightPoint2,rightJoint1,6); 113 | // rightPoint2[2]=-0.023191; 114 | // rightRobot->movel(rightPoint2); 115 | 116 | 117 | //5. 关闭机械爪 118 | } 119 | 120 | void rightHandClose(){ 121 | rightRobot->closeHand(); 122 | sleep(1); 123 | //6. 保持当前姿态抬起机械爪 124 | double joint3[]={-113.827179*DE2RA,-56.857370*DE2RA,118.371741*DE2RA,87.633779*DE2RA,89.724896*DE2RA,158.083040*DE2RA}; 125 | rightRobot->movej(joint3); 126 | 127 | sleep(1); 128 | 129 | // 向外移动一点 130 | // double joint4[]={-120.287991*DE2RA,-71.269684*DE2RA,86.387522*DE2RA,79.171585*DE2RA,89.792153*DE2RA,148.444517*DE2RA}; 131 | double joint4[]={-121.919901*DE2RA,-71.295113*DE2RA,87.380324*DE2RA,78.732137*DE2RA,91.891043*DE2RA,148.444517*DE2RA}; 132 | rightRobot->movej(joint4); 133 | } 134 | 135 | 136 | 137 | void leftHand(){ 138 | //7. 移动到瓶盖上方 139 | double joint4[]={61.348752*DE2RA,91.817693*DE2RA,-61.696989*DE2RA,29.160892*DE2RA,-31.649215*DE2RA,175*DE2RA}; 140 | 141 | leftRobot->movej(joint4); 142 | //8. 调整姿态 143 | cout<<"左臂操作："<openHand(); 146 | //10. 向下移动机械爪 147 | sleep(1); 148 | 149 | //12. 调整姿态，开始扭瓶盖 150 | double newJoints[]={67.196105*DE2RA,85.900201*DE2RA,-82.755904*DE2RA,4.626307*DE2RA,-18.818792*DE2RA,174*DE2RA}; 151 | leftRobot->movej(newJoints); 152 | } 153 | 154 | void leftHandClose(){ 155 | 156 | //11. 关闭机械爪 157 | leftRobot->closeHand(); 158 | 159 | } 160 | 161 | 162 | 163 | double newJoints[]={67.196105*DE2RA,85.900201*DE2RA,-82.755904*DE2RA,4.626307*DE2RA,-18.818792*DE2RA,174*DE2RA}; 164 | void leftHandOpenCap(){ 165 | //13. 转动瓶盖 166 | // double newJoints1[6]={0}; 167 | // memcpy(newJoints1,newJoints,6); 168 | newJoints[5]-=60*DE2RA; 169 | cout<movej(newJoints); 171 | 172 | // ServiceInterface service = leftRobot->getService(); 173 | // aubo_robot_namespace::CoordCalibrateByJointAngleAndTool userCoord2; 174 | 175 | // aubo_robot_namespace::ToolInEndDesc toolDesc; 176 | // toolDesc.toolInEndPosition.x = 0; 177 | // toolDesc.toolInEndPosition.y = 0; 178 | // toolDesc.toolInEndPosition.z = 0; 179 | // 180 | // toolDesc.toolInEndOrientation.x = 0; 181 | // toolDesc.toolInEndOrientation.y = 0; 182 | // toolDesc.toolInEndOrientation.z = 0; 183 | // toolDesc.toolInEndOrientation.w = 1; 184 | // 185 | // userCoord2.coordType=aubo_robot_namespace::EndCoordinate; 186 | // userCoord2.toolDesc = toolDesc; 187 | // 188 | // double ratations3[]={0,1,0}; 189 | // int ret3=service.robotServiceRotateMove(userCoord2,ratations3,127.369057*DE2RA,true); 190 | // cout<movej(leftStandByJoints); 199 | 200 | 201 | // sleep(1); 202 | double newJoints[]={-111.990145*DE2RA,-76.038481*DE2RA,18.184523*DE2RA,86.308074*DE2RA,91.949268*DE2RA,174*DE2RA}; 203 | rightRobot->movej(newJoints); 204 | //11. 关闭机械爪 205 | 206 | 207 | //12. 调整姿态，开始扭瓶盖 208 | } 209 | 210 | /* 自动打开盖子*/ 211 | void autoOpenGap(){ 212 | rightHand(); 213 | rightHandClose(); 214 | leftHand(); 215 | leftHandClose(); 216 | leftHandOpenCap(); 217 | leftHandOpenCap(); 218 | leftHandOpenCap(); 219 | shougong(); 220 | } 221 | 222 | 223 | 224 | int main(int argc,char **argv){ 225 | // 创建Qt应用 226 | QApplication app(argc,argv); 227 | // 创建QT窗口 228 | QWidget widget; 229 | widget.setFixedWidth(300); 230 | // 创建布局 231 | QVBoxLayout layout; 232 | // 设置窗口布局为layout 233 | widget.setLayout(&layout); 234 | 235 | QPushButton testBtn("回到待机状态"); 236 | layout.addWidget(&testBtn); 237 | QObject::connect(&testBtn,&QPushButton::clicked,doTest); 238 | 239 | //抓取的按钮 240 | QPushButton putUpBtn("右臂准备抓取"); 241 | layout.addWidget(&putUpBtn); 242 | QObject::connect(&putUpBtn,&QPushButton::clicked,rightHand); 243 | 244 | QPushButton closeBtn("右臂抓取"); 245 | layout.addWidget(&closeBtn); 246 | QObject::connect(&closeBtn,&QPushButton::clicked,rightHandClose); 247 | 248 | 249 | 250 | // //右臂待机状态 251 | // QPushButton standByBtn("右臂待机"); 252 | // layout.addWidget(&standByBtn); 253 | // QObject::connect(&standByBtn,&QPushButton::clicked,[&](){ 254 | // standbyAction(rightRobot,rightStandByJoints); 255 | // }); 256 | 257 | //放下的按钮 258 | QPushButton putDownBtn("左臂准备扭瓶盖"); 259 | layout.addWidget(&putDownBtn); 260 | QObject::connect(&putDownBtn,&QPushButton::clicked,leftHand); 261 | //放下的按钮 262 | QPushButton leftHandCloseBtn("左臂夹住瓶盖"); 263 | layout.addWidget(&leftHandCloseBtn); 264 | QObject::connect(&leftHandCloseBtn,&QPushButton::clicked,leftHandClose); 265 | 266 | //左臂扭瓶盖 267 | QPushButton leftOpenBtn("扭瓶盖"); 268 | layout.addWidget(&leftOpenBtn); 269 | QObject::connect(&leftOpenBtn,&QPushButton::clicked,leftHandOpenCap); 270 | 271 | //左臂扭瓶盖 272 | QPushButton shougongBtn("收工"); 273 | layout.addWidget(&shougongBtn); 274 | QObject::connect(&shougongBtn,&QPushButton::clicked,shougong); 275 | 276 | //左臂扭瓶盖 277 | QPushButton autoBtn("人工智能"); 278 | layout.addWidget(&autoBtn); 279 | QObject::connect(&autoBtn,&QPushButton::clicked,autoOpenGap); 280 | 281 | 282 | //打开机械爪 283 | QPushButton openHand("打开右臂机械爪"); 284 | layout.addWidget(&openHand); 285 | QObject::connect(&openHand,&QPushButton::clicked,[&](){rightRobot->openHand();}); 286 | 287 | //关闭机械爪 288 | QPushButton closeHand("关闭右臂机械爪"); 289 | layout.addWidget(&closeHand); 290 | QObject::connect(&closeHand,&QPushButton::clicked,[&](){rightRobot->closeHand();}); 291 | 292 | //打开机械爪 293 | QPushButton openLeftHand("打开左臂机械爪"); 294 | layout.addWidget(&openLeftHand); 295 | QObject::connect(&openLeftHand,&QPushButton::clicked,[&](){leftRobot->openHand();}); 296 | 297 | //关闭机械爪 298 | QPushButton closeLeftHand("关闭左臂臂机械爪"); 299 | layout.addWidget(&closeLeftHand); 300 | QObject::connect(&closeLeftHand,&QPushButton::clicked,[&](){leftRobot->closeHand();}); 301 | 302 | // 将窗口显示出来 303 | widget.show(); 304 | 305 | int exec_code = app.exec(); 306 | 307 | return exec_code; 308 | } -------------------------------------------------------------------------------- /example/example_4.cpp: -------------------------------------------------------------------------------- 1 | #include "example_4.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "util.h" 11 | 12 | 13 | #define SERVER_HOST "192.168.17.250" 14 | #define SERVER_PORT 8899 15 | 16 | aubo_robot_namespace::wayPoint_S Example_4::s_currentWayPoing; 17 | 18 | Example_4::Example_4() 19 | { 20 | } 21 | 22 | void Example_4::RealTimeWaypointCallback(const aubo_robot_namespace::wayPoint_S *wayPointPtr, void *arg) 23 | { 24 | (void)arg; 25 | s_currentWayPoing = *wayPointPtr; 26 | //Util::printWaypoint(s_currentWayPoing); 27 | } 28 | 29 | 30 | void Example_4::demo() 31 | { 32 | ServiceInterface robotService; 33 | 34 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 35 | 36 | /** 接口调用: 登录 ***/ 37 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 38 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 39 | { 40 | std::cerr<<"登录成功."< 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "util.h" 11 | 12 | #define SERVER_HOST "127.0.0.1" 13 | #define SERVER_PORT 8899 14 | 15 | 16 | Example_6::Example_6() 17 | { 18 | } 19 | 20 | void Example_6::demo() 21 | { 22 | // example_MoveLtoPosition(); 23 | // example_MoveJtoPosition(); 24 | 25 | ServiceInterface robotService; 26 | 27 | int ret = aubo_robot_namespace::InterfaceCallSuccCode; 28 | 29 | /** 接口调用: 登录 ***/ 30 | ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456"); 31 | if(ret == aubo_robot_namespace::InterfaceCallSuccCode) 32 | { 33 | std::cerr<<"登录成功."< 1)? 0.0:(0.18*(i%3)); 232 | double offsetZ = (i%3 > 1)? 0.0:(0.18); 233 | position.x = currentWaypoint.cartPos.position.x; 234 | position.y = currentWaypoint.cartPos.position.y+offsetY; 235 | position.z = currentWaypoint.cartPos.position.z+offsetZ; 236 | 237 | //保持当前位姿通过直线运动的方式运动到目标位置 238 | ret = robotService.robotMoveLineToTargetPosition(userCoord, position, toolDesc, true); 239 | if(ret != aubo_robot_namespace::InterfaceCallSuccCode) 240 | { 241 | std::cerr<<"robotMoveLineToTargetPosition.　ret:"< 1)? 0.0:(0.18*(i%3)); 348 | double offsetZ = (i%3 > 1)? 0.0:(0.18); 349 | position.x = currentWaypoint.cartPos.position.x; 350 | position.y = currentWaypoint.cartPos.position.y+offsetY; 351 | position.z = currentWaypoint.cartPos.position.z+offsetZ; 352 | 353 | //保持当前位姿通过直线运动的方式运动到目标位置 354 | ret = robotService.robotMoveJointToTargetPosition(userCoord, position, toolDesc, true); 355 | if(ret != aubo_robot_namespace::InterfaceCallSuccCode) 356 | { 357 | std::cerr<<"robotMoveLineToTargetPosition.　ret:"<