![]() |
AUBO SDK
0.25.0
|
#include <math.h>
Public 成员函数 | |
Math () | |
virtual | ~Math () |
std::vector< double > | poseAdd (const std::vector< double > &p1, const std::vector< double > &p2) |
std::vector< double > | poseSub (const std::vector< double > &p1, const std::vector< double > &p2) |
位姿相减 | |
std::vector< double > | interpolatePose (const std::vector< double > &p1, const std::vector< double > &p2, double alpha) |
计算线性插值 | |
std::vector< double > | poseTrans (const std::vector< double > &pose_from, const std::vector< double > &pose_from_to) |
位姿变换 | |
std::vector< double > | poseTransInv (const std::vector< double > &pose_from, const std::vector< double > &pose_to_from) |
姿态逆变换 | |
std::vector< double > | poseInverse (const std::vector< double > &pose) |
获取位姿的逆 | |
double | poseDistance (const std::vector< double > &p1, const std::vector< double > &p2) |
计算两个位姿的位置距离 | |
double | poseAngleDistance (const std::vector< double > &p1, const std::vector< double > &p2) |
计算两个位姿的轴角距离 | |
bool | poseEqual (const std::vector< double > &p1, const std::vector< double > &p2, double eps=5e-5) |
判断两个位姿是否相等 | |
std::vector< double > | transferRefFrame (const std::vector< double > &F_b_a_old, const Vector3d &V_in_a, int type) |
std::vector< double > | poseRotation (const std::vector< double > &pose, const std::vector< double > &rotv) |
姿态旋转 | |
std::vector< double > | rpyToQuaternion (const std::vector< double > &rpy) |
欧拉角转四元数 | |
std::vector< double > | quaternionToRpy (const std::vector< double > &quat) |
四元数转欧拉角 | |
ResultWithErrno | tcpOffsetIdentify (const std::vector< std::vector< double > > &poses) |
四点法标定TCP偏移 | |
ResultWithErrno | calibrateCoordinate (const std::vector< std::vector< double > > &poses, int type) |
Three point method calibration for TCP offset | |
ResultWithErrno | calculateCircleFourthPoint (const std::vector< double > &p1, const std::vector< double > &p2, const std::vector< double > &p3, int mode) |
根据圆弧的三个点,计算出拟合成的圆的另一半圆弧的中间点位置 | |
std::vector< double > | forceTrans (const std::vector< double > &pose_a_in_b, const std::vector< double > &force_in_a) |
std::vector< double > | getDeltaPoseBySensorDistance (const std::vector< double > &distances, double position, double radius, double track_scale) |
std::vector< double > | deltaPoseTrans (const std::vector< double > &pose_a_in_b, const std::vector< double > &ft_in_a) |
std::vector< double > | deltaPoseAdd (const std::vector< double > &pose_a_in_b, const std::vector< double > &v_in_b) |
std::vector< double > | changePoseWithXYRef (const std::vector< double > &pose_tar, const std::vector< double > &pose_ref) |
std::vector< double > | homMatrixToPose (const std::vector< double > &homMatrix) |
std::vector< double > | poseToHomMatrix (const std::vector< double > &pose) |
Protected 属性 | |
void * | d_ |
arcs::common_interface::Math::Math | ( | ) |
|
virtual |
ResultWithErrno arcs::common_interface::Math::calculateCircleFourthPoint | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2, | ||
const std::vector< double > & | p3, | ||
int | mode | ||
) |
根据圆弧的三个点,计算出拟合成的圆的另一半圆弧的中间点位置
p1 | 圆弧的起始点 |
p2 | 圆弧的中间点 |
p3 | 圆弧的结束点 |
mode | 当mode等于1的时候,表示需要对姿态进行圆弧规划; 当mode等于0的时候,表示不需要对姿态进行圆弧规划 |
ResultWithErrno arcs::common_interface::Math::calibrateCoordinate | ( | const std::vector< std::vector< double > > & | poses, |
int | type | ||
) |
Three point method calibration for TCP offset
poses | 三个点的位姿集合 |
type | 类型: 0 - oxy 原点 x轴正方向 xy平面(y轴正方向) 1 - oxz 原点 x轴正方向 xz平面(z轴正方向) 2 - oyz 原点 y轴正方向 yz平面(z轴正方向) 3 - oyx 原点 y轴正方向 yx平面(x轴正方向) 4 - ozx 原点 z轴正方向 zx平面(x轴正方向) 5 - ozy 原点 z轴正方向 zy平面(y轴正方向) |
std::vector< double > arcs::common_interface::Math::changePoseWithXYRef | ( | const std::vector< double > & | pose_tar, |
const std::vector< double > & | pose_ref | ||
) |
changePoseWithXYRef: 修改 pose_tar 的xy轴方向,尽量与 pose_ref 一致,
pose_tar | 需要修改的目标位姿 |
pose_ref | 参考位姿 |
std::vector< double > arcs::common_interface::Math::deltaPoseAdd | ( | const std::vector< double > & | pose_a_in_b, |
const std::vector< double > & | v_in_b | ||
) |
addDeltaPose: 计算以给定速度变换单位时间后的位姿
pose_a_in_b | 当前时刻 a 相对于 b 的位姿 |
v_in_b | 当前时刻 a 坐标系的速度在 b 的描述 |
std::vector< double > arcs::common_interface::Math::deltaPoseTrans | ( | const std::vector< double > & | pose_a_in_b, |
const std::vector< double > & | ft_in_a | ||
) |
changeFTFrame: 变换力和力矩的参考坐标系
pose_a_in_b | a 坐标系在 b 坐标系的位姿 |
ft_in_a | 作用在 a 点的力和力矩在 a 坐标系的描述 |
std::vector< double > arcs::common_interface::Math::forceTrans | ( | const std::vector< double > & | pose_a_in_b, |
const std::vector< double > & | force_in_a | ||
) |
forceTrans: 变换力和力矩的参考坐标系 force_in_b = pose_a_in_b * force_in_a
pose_a_in_b | a 坐标系在 b 坐标系的位姿 |
force_in_a | 力和力矩在 a 坐标系的描述 |
std::vector< double > arcs::common_interface::Math::getDeltaPoseBySensorDistance | ( | const std::vector< double > & | distances, |
double | position, | ||
double | radius, | ||
double | track_scale | ||
) |
通过距离计算工具坐标系下的位姿增量
distances | N 个距离, N >=3 |
position | 距离参考轨迹的保持高度 |
radius | 传感器中心距离末端tcp的等效半径 |
track_scale | 跟踪比例, 设置范围(0, 1], 1表示跟踪更快 |
std::vector< double > arcs::common_interface::Math::homMatrixToPose | ( | const std::vector< double > & | homMatrix | ) |
homMatrixToPose: 由齐次变换矩阵得到位姿
homMatrix | 4*4 齐次变换矩阵, 输入元素采用横向排列 |
std::vector< double > arcs::common_interface::Math::interpolatePose | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2, | ||
double | alpha | ||
) |
计算线性插值
p1 | 起点的TCP位姿 |
p2 | 终点的TCP位姿 |
alpha | 系数, 当0<alpha<1,返回p1和p2两点直线的之间靠近p1端且占总路径比例为alpha的点; 例如当alpha=0.3,返回的是靠近p1那端,总路径的百分之30的点; 当alpha>1,返回p2; 当alpha<0,返回p1; |
std::vector< double > arcs::common_interface::Math::poseAdd | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2 | ||
) |
double arcs::common_interface::Math::poseAngleDistance | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2 | ||
) |
计算两个位姿的轴角距离
p1 | 位姿1 |
p2 | 位姿2 |
double arcs::common_interface::Math::poseDistance | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2 | ||
) |
计算两个位姿的位置距离
p1 | 位姿1 |
p2 | 位姿2 |
bool arcs::common_interface::Math::poseEqual | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2, | ||
double | eps = 5e-5 |
||
) |
判断两个位姿是否相等
p1 | 位姿1 |
p2 | 位姿2 |
eps | 误差 |
std::vector< double > arcs::common_interface::Math::poseInverse | ( | const std::vector< double > & | pose | ) |
获取位姿的逆
pose | 工具位姿(空间向量) |
std::vector< double > arcs::common_interface::Math::poseRotation | ( | const std::vector< double > & | pose, |
const std::vector< double > & | rotv | ||
) |
姿态旋转
pose | |
rotv |
std::vector< double > arcs::common_interface::Math::poseSub | ( | const std::vector< double > & | p1, |
const std::vector< double > & | p2 | ||
) |
位姿相减
两个参数都包含三个位置参数(x、y、z),统称为P, 以及三个旋转参数(R_x、R_y、R_z),统称为R。 此函数根据以下方式计算结果 p_3,即给定位姿的相加: p_3.P = p_1.P - p_2.P, p_3.R = p_1.R * p_2.R.inverse
p1 | 工具位姿1 |
p2 | 工具位姿2 |
std::vector< double > arcs::common_interface::Math::poseToHomMatrix | ( | const std::vector< double > & | pose | ) |
poseToHomMatrix: 位姿变换得到齐次变换矩阵
pose | 输入的位姿 |
std::vector< double > arcs::common_interface::Math::poseTrans | ( | const std::vector< double > & | pose_from, |
const std::vector< double > & | pose_from_to | ||
) |
位姿变换
第一个参数 p_from 用于转换第二个参数 p_from_to,并返回结果。 这意味着结果是从 p_from 的坐标系开始, 然后在该坐标系中移动 p_from_to后的位姿。
这个函数可以从两个不同的角度来看。 一种是函数将 p_from_to 根据 p_from 的参数进行转换,即平移和旋转。 另一种是函数被用于获取结果姿态,先对 p_from 进行移动,然后再对 p_from_to 进行移动。 如果将姿态视为转换矩阵,它看起来像是:
T_world->to = T_world->from * T_from->to, T_x->to = T_x->from * T_from->to
这两个方程描述了姿态变换的基本原理,根据给定的起始姿态和相对于起始姿态的姿态变化,可以计算出目标姿态。
举个例子,已知B相对于A的位姿、C相对于B的位姿,求C相对于A的位姿。 第一个参数是B相对于A的位姿,第二个参数是C相对于B的位姿, 返回值是C相对于A的位姿。
pose_from | 起始位姿(空间向量) |
pose_from_to | 相对于起始位姿的姿态变化(空间向量) |
std::vector< double > arcs::common_interface::Math::poseTransInv | ( | const std::vector< double > & | pose_from, |
const std::vector< double > & | pose_to_from | ||
) |
姿态逆变换
已知C相对于A的位姿、C相对于B的位姿,求B相对于A的位姿。 第一个参数是C相对于A的位姿,第二个参数是C相对于B的位姿, 返回值是B相对于A的位姿。
pose_from | 起始位姿 |
pose_to_from | 相对于结果位姿的姿态变化 |
std::vector< double > arcs::common_interface::Math::quaternionToRpy | ( | const std::vector< double > & | quat | ) |
四元数转欧拉角
quat | 四元数 |
std::vector< double > arcs::common_interface::Math::rpyToQuaternion | ( | const std::vector< double > & | rpy | ) |
欧拉角转四元数
rpy | 欧拉角 |
ResultWithErrno arcs::common_interface::Math::tcpOffsetIdentify | ( | const std::vector< std::vector< double > > & | poses | ) |
四点法标定TCP偏移
找一个尖点,将机械臂工具末端点绕着尖点示教四个位置,姿态差别要大。 设置完毕后即可计算出来结果。
poses | 四个点的位姿集合 |
std::vector< double > arcs::common_interface::Math::transferRefFrame | ( | const std::vector< double > & | F_b_a_old, |
const Vector3d & | V_in_a, | ||
int | type | ||
) |
F_b_a_old | |
V_in_a | |
type |