forked from Guorong/inspiredhand
62 lines
1.8 KiB
C++
62 lines
1.8 KiB
C++
#include <iostream>
|
||
#include <string>
|
||
|
||
#include "jaka_robot/robot_action.h"
|
||
#include "jaka_robot/robot_controller.h"
|
||
#include "coordinated_actions/coordinated_actions.h"
|
||
#include "tools/config_loader.h"
|
||
#include "tools/tools.h"
|
||
|
||
int main()
|
||
{
|
||
// 1. 读取 config.json
|
||
const config::AppConfig &cfg = config::get_default_app_config();
|
||
const std::string robot_ip = cfg.robot_ip;
|
||
|
||
// 2. 创建机械臂控制对象
|
||
RobotController rc;
|
||
|
||
// 3. 从 config.json 读取默认速度/加速度
|
||
robot_action::RobotParams params{
|
||
static_cast<double>(cfg.robot.joint_speed_deg_s),
|
||
static_cast<double>(cfg.robot.joint_acc_deg_s2),
|
||
static_cast<double>(cfg.robot.line_speed_mm_s),
|
||
static_cast<double>(cfg.robot.line_acc_mm_s2),
|
||
static_cast<double>(cfg.robot.pose_speed_deg_s),
|
||
static_cast<double>(cfg.robot.pose_acc_deg_s2)
|
||
};
|
||
|
||
std::cout << "==== 固定抓取复现 Demo ====\n";
|
||
std::cout << "Robot IP: " << robot_ip << "\n";
|
||
|
||
// 4. 初始化机械臂：连接、上电、使能、设置速度参数
|
||
if (!robot_action::init_robot(rc, robot_ip, params)) {
|
||
std::cerr << "机械臂初始化失败\n";
|
||
return -1;
|
||
}
|
||
|
||
// 5. 初始化 TIO/RS485，用于控制灵巧手
|
||
if (!rc.init_tio_for_hand()) {
|
||
std::cerr << "TIO/RS485 初始化失败\n";
|
||
rc.disconnect();
|
||
return -1;
|
||
}
|
||
|
||
std::cout << "初始化完成。\n";
|
||
std::cout << "确认机械臂周围安全后，按回车开始执行固定抓取...\n";
|
||
tools::get_line_trimmed();
|
||
|
||
// 6. 执行一次固定抓取流程
|
||
bool ok = coordinated_actions::perform_grasp_task(rc);
|
||
|
||
if (ok) {
|
||
std::cout << "固定抓取任务执行完成。\n";
|
||
} else {
|
||
std::cerr << "固定抓取任务执行失败。\n";
|
||
}
|
||
|
||
// 7. 断开连接
|
||
rc.disconnect();
|
||
|
||
return ok ? 0 : -1;
|
||
} |