1. 获取不同坐标系的位置

本期将介绍如何连接控制器，并获取机器人在不同坐标系下的实时位置。

机器人在运行和调试过程中，同一个位置可以使用不同的坐标系进行描述。程序首先使用 connect_robot 连接控制器，再通过返回的连接句柄 fd 调用 get_current_position，依次查询机器人在关节坐标、直角坐标、工具坐标和用户坐标下的当前位置。

get_current_position 接口中的 coord 参数用于指定需要查询的坐标系：

• coord = 0：关节坐标
• coord = 1：直角坐标
• coord = 2：工具坐标
• coord = 3：用户坐标

接口查询成功后，会将当前位置写入长度为 7 的 pos 容器。不同坐标系下各分量的含义不同，使用时应结合机器人型号、当前工具坐标系和用户坐标系配置进行确认。

⚠️ 使用注意

本 Demo 只读取并输出机器人当前位置，不会发送运动指令。运行前仍需确认：

• 连接配置： 控制器 IP 地址和 SDK 服务端口与实际配置一致
• 坐标类型： coord 的取值位于 0～3 范围内
• 坐标配置： 查询工具坐标或用户坐标前，确认控制器中对应坐标系已经正确设置
• 返回结果： 实际项目中应检查 get_current_position 的返回值，接口调用成功后再使用 pos 中的数据

不同坐标系下的位置数据不能直接混用。将查询结果用于运动、偏移或坐标转换前，应先确认坐标系类型及各分量的单位和含义。

/*
 * Demo # : 01
 * 功能：获取机器人在关节、直角、工具和用户坐标系下的当前位置
 * 依赖：nrc_api.h
 *
 * 说明：
 * 1. coord = 0：关节坐标；coord = 1：直角坐标
 * 2. coord = 2：工具坐标；coord = 3：用户坐标
 * 3. 本 Demo 只查询并输出位置，不会向机器人发送运动指令
 */

#include <iostream>
#include "cpp_interface/nrc_api.h"  // 提供控制器连接和机器人位置查询接口

/**
 * @brief Demo 程序入口：连接控制器，并依次输出四种坐标系下的机器人当前位置。
 * @return 控制器连接失败或查询结束时返回 0。
 */
int main()
{
    // 使用现场控制器的 IP 地址和 SDK 服务端口建立连接，并保存返回的连接句柄。
    SOCKETFD fd = connect_robot("192.168.1.15", "6001");
    if (fd <= 0)
    {
        std::cout << "连接失败" << std::endl;
        return 0;
    }
    std::cout << "连接成功: "<< fd << std::endl;

    std::vector<double> pos(7);  // 保存 SDK 返回的七维位置数据，具体含义随坐标系类型变化。
    for (int coord = 0; coord < 4; coord++)
    {
        // 按 coord 指定的坐标系查询当前位置，查询结果由 SDK 写入 pos。
        get_current_position(fd, coord, pos);

        // 按接口返回顺序输出当前位置的七个分量。
        for (auto v : pos)
        {
            std::cout << v << " ";
        }
        std::cout << std::endl;
    }

    return 0;
}