Math (数学工具)

数学工具API接口 更多...

函数
std::vector< double >	arcs::common_interface::Math::poseAdd (const std::vector< double > &p1, const std::vector< double > &p2)
std::vector< double >	arcs::common_interface::Math::poseSub (const std::vector< double > &p1, const std::vector< double > &p2)
	位姿相减
std::vector< double >	arcs::common_interface::Math::interpolatePose (const std::vector< double > &p1, const std::vector< double > &p2, double alpha)
	计算线性插值
std::vector< double >	arcs::common_interface::Math::poseTrans (const std::vector< double > &pose_from, const std::vector< double > &pose_from_to)
	位姿变换
std::vector< double >	arcs::common_interface::Math::poseTransInv (const std::vector< double > &pose_from, const std::vector< double > &pose_to_from)
	姿态逆变换
std::vector< double >	arcs::common_interface::Math::poseInverse (const std::vector< double > &pose)
	获取位姿的逆
double	arcs::common_interface::Math::poseDistance (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)
	计算两个位姿的轴角距离
bool	arcs::common_interface::Math::poseEqual (const std::vector< double > &p1, const std::vector< double > &p2, double eps=5e-5)
	判断两个位姿是否相等
std::vector< double >	arcs::common_interface::Math::transferRefFrame (const std::vector< double > &F_b_a_old, const Vector3d &V_in_a, int type)
std::vector< double >	arcs::common_interface::Math::poseRotation (const std::vector< double > &pose, const std::vector< double > &rotv)
	姿态旋转
std::vector< double >	arcs::common_interface::Math::rpyToQuaternion (const std::vector< double > &rpy)
	欧拉角转四元数
std::vector< double >	arcs::common_interface::Math::quaternionToRpy (const std::vector< double > &quat)
	四元数转欧拉角
ResultWithErrno	arcs::common_interface::Math::tcpOffsetIdentify (const std::vector< std::vector< double > > &poses)
	四点法标定TCP偏移
ResultWithErrno	arcs::common_interface::Math::calibrateCoordinate (const std::vector< std::vector< double > > &poses, int type)
	三点法标定坐标系
ResultWithErrno	arcs::common_interface::Math::calculateCircleFourthPoint (const std::vector< double > &p1, const std::vector< double > &p2, const std::vector< double > &p3, int mode)
	根据圆弧的三个点,计算出拟合成的圆的另一半圆弧的中间点位置
std::vector< double >	arcs::common_interface::Math::forceTrans (const std::vector< double > &pose_a_in_b, const std::vector< double > &force_in_a)
std::vector< double >	arcs::common_interface::Math::getDeltaPoseBySensorDistance (const std::vector< double > &distances, double position, double radius, double track_scale)
std::vector< double >	arcs::common_interface::Math::deltaPoseTrans (const std::vector< double > &pose_a_in_b, const std::vector< double > &ft_in_a)
std::vector< double >	arcs::common_interface::Math::deltaPoseAdd (const std::vector< double > &pose_a_in_b, const std::vector< double > &v_in_b)
std::vector< double >	arcs::common_interface::Math::changePoseWithXYRef (const std::vector< double > &pose_tar, const std::vector< double > &pose_ref)
std::vector< double >	arcs::common_interface::Math::homMatrixToPose (const std::vector< double > &homMatrix)
std::vector< double >	arcs::common_interface::Math::poseToHomMatrix (const std::vector< double > &pose)

详细描述

数学工具API接口

函数说明

calculateCircleFourthPoint()

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的时候，表示不需要对姿态进行圆弧规划

返回

拟合成的圆的另一半圆弧的中间点位置和计算结果是否有效

Lua函数原型

calculateCircleFourthPoint(p1: table, p2: table, p3: table, mode: int)

Lua示例

center_pose, cal_result = calculateCircleFourthPoint({0.5488696249770836,-0.1214996547187204,0.2631931199112321,-3.14159198038469,-3.673205103150083e-06,1.570796326792424}, {0.5488696249770835,-0.1214996547187207,0.3599720701808493,-3.14159198038469,-3.6732051029273e-06,1.570796326792423}, {0.5488696249770836,-0.0389996547187214,0.3599720701808496,-3.141591980384691,-3.673205102557476e-06,1.570796326792422},1)

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.calculateCircleFourthPoint","params":[[0.5488696249770836,-0.1214996547187204,0.2631931199112321,-3.14159198038469,-3.673205103150083e-06,1.570796326792424], [0.5488696249770835,-0.1214996547187207,0.3599720701808493,-3.14159198038469,-3.6732051029273e-06,1.570796326792423], [0.5488696249770836,-0.0389996547187214,0.3599720701808496,-3.141591980384691,-3.673205102557476e-06, 1.570796326792422],1],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[[0.5488696249770837,-0.031860179583911546,0.27033259504604207,-3.1415919803846903,-3.67320510285378e-06,1.570796326792423],1]}

calibrateCoordinate()

ResultWithErrno arcs::common_interface::Math::calibrateCoordinate	(	const std::vector< std::vector< double > > &	poses,
		int	type )

三点法标定坐标系

参数

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轴正方向）

返回

坐标系标定结果和标定结果是否有效

Lua函数原型

calibrateCoordinate(poses: table, type: int) -> table, number

Lua示例

p1 = {0.55462,0.06219,0.37175,-3.142,0.0,1.580}
p2 = {0.63746,0.11805,0.37175,-3.142,0.0,1.580}
p3 = {0.40441,0.28489,0.37174,-3.142,0.0,1.580}
p = {p1,p2,p3}
coord_pose, cal_result = calibrateCoordinate(p,0)

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.calibrateCoordinate","params":[[[0.55462,0.06219,0.37175,-3.142,0.0,1.580], [0.63746,0.11805,0.37175,-3.142,0.0,1.580],[0.40441,0.28489,0.37174,-3.142,0.0,1.580]],0],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[[0.55462,0.06219,0.37175,-3.722688983883945e-05,-1.6940658945086007e-21,0.5932768162455785],0]}

changePoseWithXYRef()

std::vector< double > arcs::common_interface::Math::changePoseWithXYRef	(	const std::vector< double > &	pose_tar,
		const std::vector< double > &	pose_ref )

changePoseWithXYRef: 修改 pose_tar 的ｘｙ轴方向,尽量与 pose_ref 一致，

参数

pose_tar	需要修改的目标位姿
pose_ref	参考位姿

返回

修改后的位姿，采用pose_tar的 xyz 坐标和 z 轴方向

deltaPoseAdd()

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 的描述

返回

pose_in_b, 单位时间后的位姿在 b 的描述

deltaPoseTrans()

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 坐标系的描述

返回

ft_in_b，作用在 b 点的力和力矩在 b 坐标系的描述

forceTrans()

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 坐标系的描述

返回

force_in_b，力和力矩在 b 坐标系的描述

getDeltaPoseBySensorDistance()

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表示跟踪更快

返回

基于工具坐标系的位姿增量

homMatrixToPose()

std::vector< double > arcs::common_interface::Math::homMatrixToPose

(

const std::vector< double > &

homMatrix

)

homMatrixToPose: 由齐次变换矩阵得到位姿

参数

homMatrix

4*4 齐次变换矩阵, 输入元素采用横向排列

返回

对应的位姿

interpolatePose()

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；

返回

插值计算结果

Python函数原型

interpolatePose(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float], arg2: float) -> List[float]

Lua函数原型

interpolatePose(p1: table, p2: table, alpha: number) -> table

Lua示例

pose_interpolate = interpolatePose({0.2, 0.2, 0.4, 0, 0, 0},{0.2, 0.2, 0.6, 0, 0, 0},0.5)

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.interpolatePose","params":[[0.2, 0.2, 0.4, 0, 0, 0],[0.2, 0.2, 0.6, 0, 0, 0],0.5],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.2,0.2,0.5,0.0,-0.0,0.0]}

poseAdd()

std::vector< double > arcs::common_interface::Math::poseAdd	(	const std::vector< double > &	p1,
		const std::vector< double > &	p2 )

poseAngleDistance()

double arcs::common_interface::Math::poseAngleDistance	(	const std::vector< double > &	p1,
		const std::vector< double > &	p2 )

计算两个位姿的轴角距离

参数

p1	位姿1
p2	位姿2

返回

轴角距离

Lua函数原型

poseAngleDistance(p1: table, p2: table) -> number

Lua示例

pose_angle_distance = poseAngleDistance({0.1, 0.3, 0.1, 0.3142, 0.0, 1.571},{0.2, 0.5, 0.6, 0, -0.172, 0.0})

poseDistance()

double arcs::common_interface::Math::poseDistance	(	const std::vector< double > &	p1,
		const std::vector< double > &	p2 )

计算两个位姿的位置距离

参数

p1	位姿1
p2	位姿2

返回

两个位姿的位置距离

Lua函数原型

poseDistance(p1: table, p2: table) -> number

Lua示例

pose_distance = poseDistance({0.1, 0.3, 0.1, 0.3142, 0.0, 1.571},{0.2, 0.5, 0.6, 0, -0.172, 0.0})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.poseDistance","params":[[0.1, 0.3, 0.1, 0.3142, 0.0, 1.571],[0.2, 0.5, 0.6, 0, -0.172, 0.0]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":0.5477225575051661}

poseEqual()

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	误差

返回

相等返回true，反之返回false

Lua函数原型

poseEqual(p1: table, p2: table, eps: number) -> boolean

Lua示例

pose_equal = poseEqual({0.1, 0.3, 0.1, 0.3142, 0.0, 1.571},{0.1, 0.3, 0.1, 0.3142, 0.0, 1.5711},0.01)

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.poseEqual","params":[[0.1, 0.3, 0.1, 0.3142, 0.0, 1.571],[0.1, 0.3, 0.1, 0.3142, 0.0, 1.5711],0.01]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":0.0}

poseInverse()

std::vector< double > arcs::common_interface::Math::poseInverse

(

const std::vector< double > &

pose

)

获取位姿的逆

参数

pose	工具位姿（空间向量）

返回

工具位姿的逆转换（空间向量）

Python函数原型

poseInverse(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]

Lua函数原型

poseInverse(pose: table) -> table

Lua示例

pose_inverse = poseInverse({0.2, 0.5, 0.1, 1.57, 0, 3.14})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.poseInverse","params":[[0.2, 0.5, 0.1, 1.57, 0, 3.14]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.19920341988726448,-0.09960155178838484,-0.5003973704832628, 1.5699999989900404,-0.0015926530848129354,-3.1415913853161266]}

poseRotation()

std::vector< double > arcs::common_interface::Math::poseRotation	(	const std::vector< double > &	pose,
		const std::vector< double > &	rotv )

姿态旋转

参数

pose
rotv

返回

Python函数原型

poseRotation(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) -> List[float]

Lua函数原型

poseRotation(pose: table, rotv: table) -> table

poseSub()

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

返回

位姿相减计算结果

Python函数原型

poseSub(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) -> List[float]

Lua函数原型

poseSub(p1: table, p2: table) -> table

Lua示例

pose_sub = poseSub({0.2, 0.5, 0.1, 1.57, 0, 0},{0.2, 0.5, 0.6, 1.57, 0, 0})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.poseSub","params":[[0.2, 0.5, 0.1, 1.57, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.0,0.0,-0.5,0.0,-0.0,0.0]}

poseToHomMatrix()

std::vector< double > arcs::common_interface::Math::poseToHomMatrix

(

const std::vector< double > &

pose

)

poseToHomMatrix: 位姿变换得到齐次变换矩阵

参数

pose

输入的位姿

返回

输出的齐次变换矩阵,元素横向排列

poseTrans()

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	相对于起始位姿的姿态变化（空间向量）

返回

结果位姿 (空间向量)

Python函数原型

poseTrans(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) -> List[float]

Lua函数原型

poseTrans(pose_from: table, pose_from_to: table) -> table

Lua示例

pose_trans = poseTrans({0.2, 0.5, 0.1, 1.57, 0, 0},{0.2, 0.5, 0.6, 1.57, 0, 0})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.poseTrans","params":[[0.2, 0.5, 0.1, 1.57, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.4,-0.09960164640373415,0.6004776374923573,3.14,-0.0,0.0]}

poseTransInv()

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	相对于结果位姿的姿态变化

返回

结果位姿

Python函数原型

poseTransInv(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) -> List[float]

Lua函数原型

poseTransInv(pose_from: table, pose_to_from: table) -> table

Lua示例

pose_trans_inv = poseTransInv({0.4, -0.0996016, 0.600478, 3.14, 0, 0},{0.2, 0.5, 0.6, 1.57, 0, 0})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.poseTransInv","params":[[0.4, -0.0996016, 0.600478, 3.14, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.2,0.5000000464037341,0.10000036250764266,1.57,-0.0,0.0]}

quaternionToRpy()

std::vector< double > arcs::common_interface::Math::quaternionToRpy

(

const std::vector< double > &

quat

)

四元数转欧拉角

参数

quat

四元数

返回

欧拉角

Python函数原型

quaternionToRpy(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]

Lua函数原型

quaternionToRpy(quat: table) -> table

Lua示例

rpy = quaternionToRpy({0.834722,0.0780426, 0.451893, 0.304864})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.quaternionToRpy","params":[[0.834722, 0.0780426, 0.451893, 0.304864]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.6110000520523781,0.7849996877683915,0.960000543982093]}

rpyToQuaternion()

std::vector< double > arcs::common_interface::Math::rpyToQuaternion

(

const std::vector< double > &

rpy

)

欧拉角转四元数

参数

rpy

欧拉角

返回

四元数

Python函数原型

rpyToQuaternion(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]

Lua函数原型

rpyToQuaternion(rpy: table) -> table

Lua示例

quaternion = rpyToQuaternion({0.611, 0.785, 0.960})

JSON-RPC请求示例

{"jsonrpc":"2.0","method":"Math.rpyToQuaternion","params":[[0.611, 0.785, 0.960]],"id":1}

JSON-RPC响应示例

{"id":1,"jsonrpc":"2.0","result":[0.834721517970497,0.07804256900772265,0.4518931575790371,0.3048637712043723]}

tcpOffsetIdentify()

ResultWithErrno arcs::common_interface::Math::tcpOffsetIdentify

(

const std::vector< std::vector< double > > &

poses

)

四点法标定TCP偏移

找一个尖点，将机械臂工具末端点绕着尖点示教四个位置，姿态差别要大。 设置完毕后即可计算出来结果。

参数

poses

四个点的位姿集合

返回

TCP标定结果和标定结果是否有效

Python函数原型

tcpOffsetIdentify(self: pyaubo_sdk.Math, arg0: List[List[float]]) -> Tuple[List[float], int]

Lua函数原型

tcpOffsetIdentify(poses: table) -> table

Lua示例

p1 = {0.48659,0.10456,0.24824,-3.135,0.004,1.569}
p2 = {0.38610,0.09096,0.22432,2.552,-0.437,1.008}
p3 = {0.38610,0.05349,0.16791,-3.021,-0.981,0.517}
p4 = {0.49675,0.06417,0.21408,-2.438,0.458,0.651}
p = {p1,p2,p3,p4}
tcp_result = tcpOffsetIdentify(p)

transferRefFrame()

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

返回

Python函数原型

transferRefFrame(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float[3]], arg2: int) -> List[float]

Lua函数原型

transferRefFrame(F_b_a_old: table, V_in_a: table, type: number) -> table