机器人坐标系转换之从世界坐标系到局部坐标系

三角函数实现

下面是代码c++和python实现:

cpp 复制代码
#include <iostream>
#include <cmath>

struct Point {
    double x;
    double y;
};

class RobotCoordinateTransform {
private:
    Point origin; // 局部坐标系的原点在世界坐标系中的坐标

public:
    RobotCoordinateTransform(double originX, double originY) : origin({originX, originY}) {}

    // 将世界坐标转换到局部坐标
    Point worldToLocal(double x_w, double y_w, double theta_w) {
        Point local;

        // 平移坐标系
        local.x = x_w - origin.x;
        local.y = y_w - origin.y;

        // 旋转坐标系
        double x_local = local.x * cos(theta_w) - local.y * sin(theta_w);
        double y_local = local.x * sin(theta_w) + local.y * cos(theta_w);

        local.x = x_local;
        local.y = y_local;

        return local;
    }
};

int main() {
    RobotCoordinateTransform transform(0, 0); // 假设局部坐标系原点在世界坐标系的(0, 0)点

    double x_w, y_w, theta_w;

    std::cout << "Enter world coordinates (x_w, y_w) and orientation theta_w (in radians): ";
    std::cin >> x_w >> y_w >> theta_w;

    Point local = transform.worldToLocal(x_w, y_w, theta_w);

    std::cout << "Local coordinates (x_local, y_local): (" << local.x << ", " << local.y << ")" << std::endl;

    return 0;
}
python 复制代码
import math

class RobotCoordinateTransform:
    def __init__(self, origin_x, origin_y):
        self.origin = (origin_x, origin_y)  # 局部坐标系的原点在世界坐标系中的坐标

    # 将世界坐标转换到局部坐标
    def world_to_local(self, x_w, y_w, theta_w):
        # 平移坐标系
        x_local = x_w - self.origin[0]
        y_local = y_w - self.origin[1]

        # 旋转坐标系
        x_local_rotated = x_local * math.cos(theta_w) - y_local * math.sin(theta_w)
        y_local_rotated = x_local * math.sin(theta_w) + y_local * math.cos(theta_w)

        return x_local_rotated, y_local_rotated

if __name__ == "__main__":
    origin_x = float(input("Enter the x-coordinate of the origin of the local coordinate system: "))
    origin_y = float(input("Enter the y-coordinate of the origin of the local coordinate system: "))

    transform = RobotCoordinateTransform(origin_x, origin_y)

    x_w = float(input("Enter the x-coordinate in the world coordinate system: "))
    y_w = float(input("Enter the y-coordinate in the world coordinate system: "))
    theta_w = float(input("Enter the orientation (in radians) in the world coordinate system: "))

    x_local, y_local = transform.world_to_local(x_w, y_w, theta_w)

    print(f"Local coordinates (x_local, y_local): ({x_local}, {y_local})")

矩阵实现:

下面是代码c++和python实现:

cpp 复制代码
#include <iostream>
#include <cmath>
#include <Eigen/Dense>  // Eigen库用于矩阵运算

class RobotCoordinateTransform {
private:
    Eigen::Vector2d origin;  // 局部坐标系的原点在世界坐标系中的坐标

public:
    RobotCoordinateTransform(double originX, double originY) : origin(originX, originY) {}

    // 将世界坐标转换到局部坐标
    std::pair<double, double> worldToLocal(double x_w, double y_w, double theta_w) {
        // 平移坐标系的矩阵
        Eigen::Matrix3d translationMatrix;
        translationMatrix << 1, 0, -origin[0],
                             0, 1, -origin[1],
                             0, 0, 1;

        // 旋转坐标系的矩阵
        Eigen::Matrix3d rotationMatrix;
        rotationMatrix << cos(theta_w), -sin(theta_w), 0,
                          sin(theta_w),  cos(theta_w), 0,
                          0,             0,            1;

        // 世界坐标的齐次坐标
        Eigen::Vector3d worldCoords(x_w, y_w, 1);

        // 应用平移和旋转变换
        Eigen::Vector3d localCoords = rotationMatrix * translationMatrix * worldCoords;

        return std::make_pair(localCoords[0], localCoords[1]);
    }
};

int main() {
    double originX, originY;
    std::cout << "Enter the x-coordinate of the origin of the local coordinate system: ";
    std::cin >> originX;
    std::cout << "Enter the y-coordinate of the origin of the local coordinate system: ";
    std::cin >> originY;

    RobotCoordinateTransform transform(originX, originY);

    double x_w, y_w, theta_w;
    std::cout << "Enter the x-coordinate in the world coordinate system: ";
    std::cin >> x_w;
    std::cout << "Enter the y-coordinate in the world coordinate system: ";
    std::cin >> y_w;
    std::cout << "Enter the orientation (in radians) in the world coordinate system: ";
    std::cin >> theta_w;

    auto [x_local, y_local] = transform.worldToLocal(x_w, y_w, theta_w);

    std::cout << "Local coordinates (x_local, y_local): (" << x_local << ", " << y_local << ")" << std::endl;

    return 0;
}
Eigen::Vector2d 用于存储坐标点和原点。
Eigen::Matrix3d 用于表示3x3矩阵,进行平移和旋转操作。
worldToLocal 方法使用上述的数学公式和矩阵进行坐标变换。
python 复制代码
import numpy as np
import math

class RobotCoordinateTransform:
    def __init__(self, origin_x, origin_y):
        self.origin = np.array([[origin_x], [origin_y]])  # 局部坐标系的原点在世界坐标系中的坐标

    def world_to_local(self, x_w, y_w, theta_w):
        # 平移坐标系的矩阵
        translation_matrix = np.array([
            [1, 0, -self.origin[0][0]],
            [0, 1, -self.origin[1][0]],
            [0, 0, 1]
        ])

        # 旋转坐标系的矩阵
        rotation_matrix = np.array([
            [math.cos(theta_w), -math.sin(theta_w), 0],
            [math.sin(theta_w), math.cos(theta_w), 0],
            [0, 0, 1]
        ])

        # 世界坐标的齐次坐标
        world_coords = np.array([[x_w], [y_w], [1]])

        # 应用平移和旋转变换
        local_coords = np.dot(rotation_matrix, np.dot(translation_matrix, world_coords))

        return local_coords[0][0], local_coords[1][0]

if __name__ == "__main__":
    origin_x = float(input("Enter the x-coordinate of the origin of the local coordinate system: "))
    origin_y = float(input("Enter the y-coordinate of the origin of the local coordinate system: "))

    transform = RobotCoordinateTransform(origin_x, origin_y)

    x_w = float(input("Enter the x-coordinate in the world coordinate system: "))
    y_w = float(input("Enter the y-coordinate in the world coordinate system: "))
    theta_w = float(input("Enter the orientation (in radians) in the world coordinate system: "))

    x_local, y_local = transform.world_to_local(x_w, y_w, theta_w)

    print(f"Local coordinates (x_local, y_local): ({x_local}, {y_local})")

Tips:

相关推荐
宋138102797203 小时前
Manus Xsens Metagloves虚拟现实手套
人工智能·机器人·vr·动作捕捉
禁默3 小时前
第六届机器人、智能控制与人工智能国际学术会议(RICAI 2024)
人工智能·机器人·智能控制
Robot25117 小时前
Figure 02迎重大升级!!人形机器人独角兽[Figure AI]商业化加速
人工智能·机器人·微信公众平台
FreeIPCC20 小时前
谈一下开源生态对 AI人工智能大模型的促进作用
大数据·人工智能·机器人·开源
施努卡机器视觉1 天前
电解车间铜业机器人剥片技术是现代铜冶炼过程中自动化和智能化的重要体现
运维·机器人·自动化
zhd15306915625ff1 天前
库卡机器人日常维护
网络·机器人·自动化·机器人备件
古月居GYH1 天前
ROS一键安装脚本
人工智能·机器人·ros
清流君1 天前
【运动规划】移动机器人运动规划与轨迹优化全解析 | 经典算法总结
人工智能·笔记·算法·机器人·自动驾驶·运动规划
Matlab程序猿小助手2 天前
【MATLAB源码-第218期】基于matlab的北方苍鹰优化算法(NGO)无人机三维路径规划,输出做短路径图和适应度曲线.
开发语言·嵌入式硬件·算法·matlab·机器人·无人机
xx小寂2 天前
ubuntu16.04在ros使用USB摄像头-解决could not open /dev/video0问题
ubuntu·机器人