forked from Guorong/inspiredhand
57 lines
2.3 KiB
C++
57 lines
2.3 KiB
C++
#include <iostream>
|
||
#include <string>
|
||
|
||
#include "robot_action.h"
|
||
#include "robot_controller.h"
|
||
#include "inspire_hand/hand_actions.h"
|
||
#include "coordinated_actions/coordinated_actions.h"
|
||
#include "tools/tools.h"
|
||
#include "tools/config_loader.h"
|
||
|
||
int main(int argc, char **argv) {
|
||
// 从配置读取 IP
|
||
const config::AppConfig &cfg = config::get_default_app_config();
|
||
const std::string fixed_ip = cfg.robot_ip;
|
||
RobotController rc;
|
||
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 << "机械臂+灵巧手演示程序 - 菜单\n";
|
||
|
||
//机械臂初始化
|
||
if (!robot_action::init_robot(rc, fixed_ip, params))
|
||
return -1;
|
||
|
||
//TIO与灵巧手初始化（内部从配置获取灵巧手角度/速度/力度）
|
||
if(!rc.init_tio_for_hand())
|
||
return -1;
|
||
|
||
std::cout << "连接与使能完成，并采用默认速度，移动到初始位置。\n";
|
||
std::cout << "机械臂设置: 关节速度=" << params.joint_speed_deg_s
|
||
<< "deg/s, 关节加速度=" << params.joint_acc_deg_s2
|
||
<< "deg/s^2, 直线速度=" << params.line_speed_mm_s
|
||
<< "mm/s, 直线加速度=" << params.line_acc_mm_s2 << "mm/s^2" << "\n";
|
||
// 从配置中读取灵巧手的初始角度/速度/力度并打印
|
||
const auto &hand = cfg.hand;
|
||
std::cout << "灵巧手设置: 初始角度=("
|
||
<< hand.angle[0] << "," << hand.angle[1] << "," << hand.angle[2] << ","
|
||
<< hand.angle[3] << "," << hand.angle[4] << "," << hand.angle[5] << ")"
|
||
<< ", 初始速度=("
|
||
<< hand.speed[0] << "," << hand.speed[1] << "," << hand.speed[2] << ","
|
||
<< hand.speed[3] << "," << hand.speed[4] << "," << hand.speed[5] << ")"
|
||
<< ", 初始力度=("
|
||
<< hand.force[0] << "," << hand.force[1] << "," << hand.force[2] << ","
|
||
<< hand.force[3] << "," << hand.force[4] << "," << hand.force[5] << ")"
|
||
<< "\n";
|
||
coordinated_actions::run_interaction_loop(rc, params);
|
||
|
||
rc.disconnect();
|
||
return 0;
|
||
}
|