ARCS SDK API  0.25.0
载入中...
搜索中...
未找到
math.h
浏览该文件的文档.
1/** @file math.h
2 * \~chinese @brief 数学方法接口,如欧拉角与四元数转换、位姿的加减运算
3 * \~english @brief Mathematic operation interface, such as euler to quaternion conversion, addition/subtraction of poses
4 */
5#ifndef AUBO_SDK_MATH_INTERFACE_H
6#define AUBO_SDK_MATH_INTERFACE_H
7
8#include <vector>
9#include <memory>
10
11#include <aubo/type_def.h>
12#include <aubo/global_config.h>
13
14namespace arcs {
15namespace common_interface {
16
17class ARCS_ABI_EXPORT Math
18{
19public:
21 virtual ~Math();
22
23 /**
24 * \english
25 * Pose addition
26 *
27 * Both arguments contain three position parameters (x, y, z) jointly called
28 * P, and three rotation parameters (R_x, R_y, R_z) jointly called R. This
29 * function calculates the result x_3 as the addition of the given poses as
30 * follows:
31 *
32 * p_3.P = p_1.P + p_2.P
33 *
34 * p_3.R = p_1.R * p_2.R
35 *
36 * @param p1 Tool pose 1
37 * @param p2 Tool pose 2
38 * @return sum of position parts and product of rotation parts (pose)
39 *
40 * @par Python interface prototype
41 * poseAdd(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) ->
42 * List[float]
43 *
44 * @par Lua interface prototype
45 * poseAdd(p1: table, p2: table) -> table
46 *
47 * @par JSON-RPC request example
48 * {"jsonrpc":"2.0","method":"Math.poseAdd","params":[[0.2, 0.5, 0.1, 1.57,
49 * 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
50 *
51 * @par JSON-RPC response example
52 * {"id":1,"jsonrpc":"2.0","result":[0.4,1.0,0.7,3.14,-0.0,0.0]}
53 * \endengish
54 *
55 * \chinese
56 * 位姿相加。
57 * 两个参数都包含三个位置参数(x、y、z),统称为P,
58 * 以及三个旋转参数(R_x、R_y、R_z),统称为R。
59 * 此函数根据以下方式计算结果 p_3,即给定位姿的相加:
60 * p_3.P = p_1.P + p_2.P,
61 * p_3.R = p_1.R * p_2.R
62 *
63 * @param p1 工具位姿1(pose)
64 * @param p2 工具位姿2(pose)
65 * @return 位置部分之和和旋转部分之积(pose)
66 *
67 * @par Python函数原型
68 * poseAdd(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) ->
69 * List[float]
70 *
71 * @par Lua函数原型
72 * poseAdd(p1: table, p2: table) -> table
73 *
74 * @par JSON-RPC请求示例
75 * {"jsonrpc":"2.0","method":"Math.poseAdd","params":[[0.2, 0.5, 0.1, 1.57,
76 * 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
77 *
78 * @par JSON-RPC响应示例
79 * {"id":1,"jsonrpc":"2.0","result":[0.4,1.0,0.7,3.14,-0.0,0.0]}
80 * \endchinese
81 */
82 std::vector<double> poseAdd(const std::vector<double> &p1,
83 const std::vector<double> &p2);
84
85 /**
86 * \chinese
87 * 位姿相减
88 *
89 * 两个参数都包含三个位置参数(x、y、z),统称为P,
90 * 以及三个旋转参数(R_x、R_y、R_z),统称为R。
91 * 此函数根据以下方式计算结果 p_3,即给定位姿的相加:
92 * p_3.P = p_1.P - p_2.P,
93 * p_3.R = p_1.R * p_2.R.inverse
94 *
95 * @param p1 工具位姿1
96 * @param p2 工具位姿2
97 * @return 位姿相减计算结果
98 *
99 * @par Python函数原型
100 * poseSub(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) ->
101 * List[float]
102 *
103 * @par Lua函数原型
104 * poseSub(p1: table, p2: table) -> table
105 *
106 * @par JSON-RPC请求示例
107 * {"jsonrpc":"2.0","method":"Math.poseSub","params":[[0.2, 0.5, 0.1, 1.57,
108 * 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
109 *
110 * @par JSON-RPC响应示例
111 * {"id":1,"jsonrpc":"2.0","result":[0.0,0.0,-0.5,0.0,-0.0,0.0]}
112 * \endchinese
113 *
114 * \english
115 * Pose subtraction
116 *
117 * Both arguments contain three position parameters (x, y, z) jointly called
118 * P, and three rotation parameters (R_x, R_y, R_z) jointly called R. This
119 * function calculates the result x_3 as the addition of the given poses as
120 * follows:
121 *
122 * p_3.P = p_1.P - p_2.P,
123 *
124 * p_3.R = p_1.R * p_2.R.inverse
125 *
126 * @param p1 tool pose 1
127 * @param p2 tool pose 2
128 * @return difference between two poses
129 *
130 * @par Python interface prototype
131 * poseSub(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) ->
132 * List[float]
133 *
134 * @par Lua interface prototype
135 * poseSub(p1: table, p2: table) -> table
136 *
137 * @par JSON-RPC request example
138 * {"jsonrpc":"2.0","method":"Math.poseSub","params":[[0.2, 0.5, 0.1, 1.57,
139 * 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
140 *
141 * @par JSON-RPC response example
142 * {"id":1,"jsonrpc":"2.0","result":[0.0,0.0,-0.5,0.0,-0.0,0.0]}
143 * \endenglish
144 */
145 std::vector<double> poseSub(const std::vector<double> &p1,
146 const std::vector<double> &p2);
147
148 /**
149 * \chinese
150 * 计算线性插值
151 *
152 * @param p1 起点的TCP位姿
153 * @param p2 终点的TCP位姿
154 * @param alpha 系数,
155 * 当0<alpha<1,返回p1和p2两点直线的之间靠近p1端且占总路径比例为alpha的点;
156 * 例如当alpha=0.3,返回的是靠近p1那端,总路径的百分之30的点;
157 * 当alpha>1,返回p2;
158 * 当alpha<0,返回p1;
159 * @return 插值计算结果
160 *
161 * @par Python函数原型
162 * interpolatePose(self: pyaubo_sdk.Math, arg0: List[float], arg1:
163 * List[float], arg2: float) -> List[float]
164 *
165 * @par Lua函数原型
166 * interpolatePose(p1: table, p2: table, alpha: number) -> table
167 *
168 * @par JSON-RPC请求示例
169 * {"jsonrpc":"2.0","method":"Math.interpolatePose","params":[[0.2, 0.2,
170 * 0.4, 0, 0, 0],[0.2, 0.2, 0.6, 0, 0, 0],0.5],"id":1}
171 *
172 * @par JSON-RPC响应示例
173 * {"id":1,"jsonrpc":"2.0","result":[0.2,0.2,0.5,0.0,-0.0,0.0]}
174 * \endchinese
175 *
176 * \english
177 * Calculate linear interpolation
178 *
179 * @param p1 starting TCP pose
180 * @param p2 ending TCP pose
181 * @param alpha coefficient;
182 * When 0<alpha<1,return a point between p1 & p2 that is closer to p1, at alpha percentage of the path;
183 * For example when alpha=0.3,point returned is closer to p1,at 30% of the total distance;
184 * When alpha>1, return p2;
185 * When alpha<0,return p1;
186 * @return interpolation result
187 *
188 * @par Python interface prototype
189 * interpolatePose(self: pyaubo_sdk.Math, arg0: List[float], arg1:
190 * List[float], arg2: float) -> List[float]
191 *
192 * @par Lua interface prototype
193 * interpolatePose(p1: table, p2: table, alpha: number) -> table
194 *
195 * @par JSON-RPC request example
196 * {"jsonrpc":"2.0","method":"Math.interpolatePose","params":[[0.2, 0.2,
197 * 0.4, 0, 0, 0],[0.2, 0.2, 0.6, 0, 0, 0],0.5],"id":1}
198 *
199 * @par JSON-RPC response example
200 * {"id":1,"jsonrpc":"2.0","result":[0.2,0.2,0.5,0.0,-0.0,0.0]}
201 * \endenglish
202 *
203 */
204 std::vector<double> interpolatePose(const std::vector<double> &p1,
205 const std::vector<double> &p2,
206 double alpha);
207 /**
208 * \chinese
209 * 位姿变换
210 *
211 * 第一个参数 p_from 用于转换第二个参数 p_from_to,并返回结果。
212 * 这意味着结果是从 p_from 的坐标系开始,
213 * 然后在该坐标系中移动 p_from_to后的位姿。
214 *
215 * 这个函数可以从两个不同的角度来看。
216 * 一种是函数将 p_from_to 根据 p_from 的参数进行转换,即平移和旋转。
217 * 另一种是函数被用于获取结果姿态,先对 p_from 进行移动,然后再对 p_from_to
218 * 进行移动。 如果将姿态视为转换矩阵,它看起来像是:
219 *
220 * T_world->to = T_world->from * T_from->to,
221 * T_x->to = T_x->from * T_from->to
222 *
223 * 这两个方程描述了姿态变换的基本原理,根据给定的起始姿态和相对于起始姿态的姿态变化,可以计算出目标姿态。
224 *
225 * 举个例子,已知B相对于A的位姿、C相对于B的位姿,求C相对于A的位姿。
226 * 第一个参数是B相对于A的位姿,第二个参数是C相对于B的位姿,
227 * 返回值是C相对于A的位姿。
228 *
229 * @param pose_from 起始位姿(空间向量)
230 * @param pose_from_to 相对于起始位姿的姿态变化(空间向量)
231 * @return 结果位姿 (空间向量)
232 *
233 * @par Python函数原型
234 * poseTrans(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) ->
235 * List[float]
236 *
237 * @par Lua函数原型
238 * poseTrans(pose_from: table, pose_from_to: table) -> table
239 *
240 * @par JSON-RPC请求示例
241 * {"jsonrpc":"2.0","method":"Math.poseTrans","params":[[0.2, 0.5,
242 * 0.1, 1.57, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
243 *
244 * @par JSON-RPC响应示例
245 * {"id":1,"jsonrpc":"2.0","result":[0.4,-0.09960164640373415,0.6004776374923573,3.14,-0.0,0.0]}
246 * \endchinese
247 *
248 * \english
249 * Pose transformation
250 *
251 * The first argument, p_from, is used to transform the second argument,
252 * p_from_to, and the result is then returned. This means that the result is
253 * the resulting pose, when starting at the coordinate system of p_from, and
254 * then in that coordinate system moving p_from_to.
255 *
256 * This function can be seen in two different views. Either the function
257 * transforms, that is translates and rotates, p_from_to by the parameters
258 * of p_from. Or the function is used to get the resulting pose, when first
259 * making a move of p_from and then from there, a move of p_from_to. If the
260 * poses were regarded as transformation matrices, it would look like:
261 *
262 * T_world->to = T_world->from * T_from->to,
263 * T_x->to = T_x->from * T_from->to
264 *
265 *
266 * These two equations describes the foundations for pose transformation.
267 * Based on a starting pose and the pose transformation relative to the starting pose, we can get the target pose.
268 *
269 * For example, we know pose of B relative to A, pose of C relative to B, find pose of C relative to A.
270 * param 1 is pose of B relative to A,param 2 is pose of C relative to B,
271 * the return value is the pose of C relative to A
272 *
273 * @param pose_from starting pose(vector in 3D space)
274 * @param pose_from_to pose transformation relative to starting pose(vector in 3D space)
275 * @return final pose (vector in 3D space)
276 *
277 * @par Python interface prototype
278 * poseTrans(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float]) ->
279 * List[float]
280 *
281 * @par Lua interface prototype
282 * poseTrans(pose_from: table, pose_from_to: table) -> table
283 *
284 * @par JSON-RPC request example
285 * {"jsonrpc":"2.0","method":"Math.poseTrans","params":[[0.2, 0.5,
286 * 0.1, 1.57, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
287 *
288 * @par JSON-RPC response example
289 * {"id":1,"jsonrpc":"2.0","result":[0.4,-0.09960164640373415,0.6004776374923573,3.14,-0.0,0.0]}
290 * \endenglish
291 */
292 std::vector<double> poseTrans(const std::vector<double> &pose_from,
293 const std::vector<double> &pose_from_to);
294
295 /**
296 * \english
297 * Pose inverse transformation
298 *
299 * Given pose of C relative to A, pose of C relative to B, find pose of B relative to A.
300 * param 1 is pose of C relative to A,param 2 is pose of C relative to B,
301 * the return value is the pose of B relative to A
302 *
303 * @param pose_from starting pose
304 * @param pose_to_from pose transformation relative to final pose
305 * @return resulting pose
306 *
307 * @par Python interface prototype
308 * poseTransInv(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float])
309 * -> List[float]
310 *
311 * @par Lua interface prototype
312 * poseTransInv(pose_from: table, pose_to_from: table) -> table
313 *
314 * @par JSON-RPC request example
315 * {"jsonrpc":"2.0","method":"Math.poseTransInv","params":[[0.4, -0.0996016,
316 * 0.600478, 3.14, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
317 *
318 * @par JSON-RPC response example
319 * {"id":1,"jsonrpc":"2.0","result":[0.2,0.5000000464037341,0.10000036250764266,1.57,-0.0,0.0]}
320 * \endenglish
321 *
322 * \chinese
323 * 姿态逆变换
324 *
325 * 已知C相对于A的位姿、C相对于B的位姿,求B相对于A的位姿。
326 * 第一个参数是C相对于A的位姿,第二个参数是C相对于B的位姿,
327 * 返回值是B相对于A的位姿。
328 *
329 * @param pose_from 起始位姿
330 * @param pose_to_from 相对于结果位姿的姿态变化
331 * @return 结果位姿
332 *
333 * @par Python函数原型
334 * poseTransInv(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float])
335 * -> List[float]
336 *
337 * @par Lua函数原型
338 * poseTransInv(pose_from: table, pose_to_from: table) -> table
339 *
340 * @par JSON-RPC请求示例
341 * {"jsonrpc":"2.0","method":"Math.poseTransInv","params":[[0.4, -0.0996016,
342 * 0.600478, 3.14, 0, 0],[0.2, 0.5, 0.6, 1.57, 0, 0]],"id":1}
343 *
344 * @par JSON-RPC响应示例
345 * {"id":1,"jsonrpc":"2.0","result":[0.2,0.5000000464037341,0.10000036250764266,1.57,-0.0,0.0]}
346 * \endchinese
347 */
348 std::vector<double> poseTransInv(const std::vector<double> &pose_from,
349 const std::vector<double> &pose_to_from);
350
351 /**
352 * \chinese
353 * 获取位姿的逆
354 *
355 * @param pose 工具位姿(空间向量)
356 * @return 工具位姿的逆转换(空间向量)
357 *
358 * @par Python函数原型
359 * poseInverse(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]
360 *
361 * @par Lua函数原型
362 * poseInverse(pose: table) -> table
363 *
364 * @par JSON-RPC请求示例
365 * {"jsonrpc":"2.0","method":"Math.poseInverse","params":[[0.2, 0.5,
366 * 0.1, 1.57, 0, 3.14]],"id":1}
367 *
368 * @par JSON-RPC响应示例
369 * {"id":1,"jsonrpc":"2.0","result":[0.19920341988726448,-0.09960155178838484,-0.5003973704832628,
370 * 1.5699999989900404,-0.0015926530848129354,-3.1415913853161266]}
371 *
372 * \endchinese
373 *
374 * \english
375 * Get the inverse of a pose
376 *
377 * @param pose tool pose (spatial vector)
378 * @return inverse tool pose transformation (spatial vector)
379 *
380 * @par Python interface prototype
381 * poseInverse(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]
382 *
383 * @par Lua interface prototype
384 * poseInverse(pose: table) -> table
385 *
386 * @par JSON-RPC request example
387 * {"jsonrpc":"2.0","method":"Math.poseInverse","params":[[0.2, 0.5,
388 * 0.1, 1.57, 0, 3.14]],"id":1}
389 *
390 * @par JSON-RPC response example
391 * {"id":1,"jsonrpc":"2.0","result":[0.19920341988726448,-0.09960155178838484,-0.5003973704832628,
392 * 1.5699999989900404,-0.0015926530848129354,-3.1415913853161266]}
393 * \endenglish
394 *
395 */
396 std::vector<double> poseInverse(const std::vector<double> &pose);
397
398 /**
399 * \chinese
400 * 计算两个位姿的位置距离
401 *
402 * @param p1 位姿1
403 * @param p2 位姿2
404 * @return 两个位姿的位置距离
405 *
406 * @par JSON-RPC请求示例
407 * {"jsonrpc":"2.0","method":"Math.poseDistance","params":[[0.1, 0.3, 0.1,
408 * 0.3142, 0.0, 1.571],[0.2, 0.5, 0.6, 0, -0.172, 0.0]],"id":1}
409 *
410 * @par JSON-RPC响应示例
411 * {"id":1,"jsonrpc":"2.0","result":0.5477225575051661}
412 * \endchinese
413 *
414 * \english
415 * Calculate distance between two poses
416 *
417 * @param p1 pose 1
418 * @param p2 pose 2
419 * @return distance between the poses
420 *
421 * @par JSON-RPC request example
422 * {"jsonrpc":"2.0","method":"Math.poseDistance","params":[[0.1, 0.3, 0.1,
423 * 0.3142, 0.0, 1.571],[0.2, 0.5, 0.6, 0, -0.172, 0.0]],"id":1}
424 *
425 * @par JSON-RPC response example
426 * {"id":1,"jsonrpc":"2.0","result":0.5477225575051661}
427 * \endenglish
428 *
429 */
430 double poseDistance(const std::vector<double> &p1,
431 const std::vector<double> &p2);
432
433 /**
434 * \chinese
435 * 计算两个位姿的轴角距离
436 *
437 * @param p1 位姿1
438 * @param p2 位姿2
439 * @return 轴角距离
440 * \endchinese
441 *
442 * \english
443 * Calculate axis-angle difference between two poses
444 *
445 * @param p1 pose 1
446 * @param p2 pose 2
447 * @return axis angle difference
448 * \endenglish
449 */
450 double poseAngleDistance(const std::vector<double> &p1,
451
452 const std::vector<double> &p2);
453
454 /**
455 * \chinese
456 * 判断两个位姿是否相等
457 *
458 * @param p1 位姿1
459 * @param p2 位姿2
460 * @param eps 误差
461 * @return 相等返回true,反之返回false
462 *
463 * @par JSON-RPC请求示例
464 * {"jsonrpc":"2.0","method":"Math.poseDistance","params":[[0.1, 0.3, 0.1,
465 * 0.3142, 0.0, 1.571],[0.1, 0.3, 0.1, 0.3142, 0.0, 1.5711]],"id":1}
466 *
467 * @par JSON-RPC响应示例
468 * {"id":1,"jsonrpc":"2.0","result":0.0}
469 * \endchinese
470 *
471 * \english
472 * Determine if two poses are equivalent
473 *
474 * @param p1 pose 1
475 * @param p2 pose 2
476 * @param eps error margin
477 * @return true or false
478 *
479 * @par JSON-RPC request example
480 * {"jsonrpc":"2.0","method":"Math.poseDistance","params":[[0.1, 0.3, 0.1,
481 * 0.3142, 0.0, 1.571],[0.1, 0.3, 0.1, 0.3142, 0.0, 1.5711]],"id":1}
482 *
483 * @par JSON-RPC response example
484 * {"id":1,"jsonrpc":"2.0","result":0.0}
485 * \endenglish
486 */
487 bool poseEqual(const std::vector<double> &p1, const std::vector<double> &p2,
488 double eps = 5e-5);
489
490 /**
491 * \chinese
492 * @param F_b_a_old
493 * @param V_in_a
494 * @param type
495 * @return
496 *
497 * @par Python函数原型
498 * transferRefFrame(self: pyaubo_sdk.Math, arg0: List[float], arg1:
499 * List[float[3]], arg2: int) -> List[float]
500 *
501 * @par Lua函数原型
502 * transferRefFrame(F_b_a_old: table, V_in_a: table, type: number) -> table
503 * \endchinese
504 *
505 * \english
506 * @param F_b_a_old
507 * @param V_in_a
508 * @param type
509 * @return
510 *
511 * @par Python interface prototype
512 * transferRefFrame(self: pyaubo_sdk.Math, arg0: List[float], arg1:
513 * List[float[3]], arg2: int) -> List[float]
514 *
515 * @par Lua interface prototype
516 * transferRefFrame(F_b_a_old: table, V_in_a: table, type: number) -> table
517 * \endenglish
518 */
519 std::vector<double> transferRefFrame(const std::vector<double> &F_b_a_old,
520 const Vector3d &V_in_a, int type);
521
522 /**
523 * \chinese
524 * 姿态旋转
525 *
526 * @param pose
527 * @param rotv
528 * @return
529 *
530 * @par Python函数原型
531 * poseRotation(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float])
532 * -> List[float]
533 *
534 * @par Lua函数原型
535 * poseRotation(pose: table, rotv: table) -> table
536 * \endchinese
537 *
538 * \english
539 * Pose rotation
540 *
541 * @param pose
542 * @param rotv
543 * @return
544 *
545 * @par Python interface prototype
546 * poseRotation(self: pyaubo_sdk.Math, arg0: List[float], arg1: List[float])
547 * -> List[float]
548 *
549 * @par Lua interface prototype
550 * poseRotation(pose: table, rotv: table) -> table
551 * \endenglish
552 */
553 std::vector<double> poseRotation(const std::vector<double> &pose,
554 const std::vector<double> &rotv);
555
556 /**
557 * \chinese
558 * 欧拉角转四元数
559 *
560 * @param rpy 欧拉角
561 * @return 四元数
562 *
563 * @par Python函数原型
564 * rpyToQuaternion(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]
565 *
566 * @par Lua函数原型
567 * rpyToQuaternion(rpy: table) -> table
568 *
569 * @par JSON-RPC请求示例
570 * {"jsonrpc":"2.0","method":"Math.rpyToQuaternion","params":[[0.611, 0.785,
571 * 0.960]],"id":1}
572 *
573 * @par JSON-RPC响应示例
574 * {"id":1,"jsonrpc":"2.0","result":[0.834721517970497,0.07804256900772265,0.4518931575790371,0.3048637712043723]}
575 * \endchinese
576 *
577 * \english
578 * Euler angles to quaternions
579 *
580 * @param rpy euler angles
581 * @return quaternions
582 *
583 * @par Python interface prototype
584 * rpyToQuaternion(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]
585 *
586 * @par Lua interface prototype
587 * rpyToQuaternion(rpy: table) -> table
588 *
589 * @par JSON-RPC request example
590 * {"jsonrpc":"2.0","method":"Math.rpyToQuaternion","params":[[0.611, 0.785,
591 * 0.960]],"id":1}
592 *
593 * @par JSON-RPC response example
594 * {"id":1,"jsonrpc":"2.0","result":[0.834721517970497,0.07804256900772265,0.4518931575790371,0.3048637712043723]}
595 * \endenglish
596 */
597 std::vector<double> rpyToQuaternion(const std::vector<double> &rpy);
598
599 /**
600 * \chinese
601 * 四元数转欧拉角
602 *
603 * @param quat 四元数
604 * @return 欧拉角
605 *
606 * @par Python函数原型
607 * quaternionToRpy(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]
608 *
609 * @par Lua函数原型
610 * quaternionToRpy(quat: table) -> table
611 *
612 * @par JSON-RPC请求示例
613 * {"jsonrpc":"2.0","method":"Math.quaternionToRpy","params":[[0.834722,
614 * 0.0780426, 0.451893, 0.304864]],"id":1}
615 *
616 * @par JSON-RPC响应示例
617 * {"id":1,"jsonrpc":"2.0","result":[0.6110000520523781,0.7849996877683915,0.960000543982093]}
618 * \endchinese
619 *
620 * \english
621 * Quaternions to euler angles
622 *
623 * @param quat quaternions
624 * @return euler angles
625 *
626 * @par Python interface prototype
627 * quaternionToRpy(self: pyaubo_sdk.Math, arg0: List[float]) -> List[float]
628 *
629 * @par Lua interface prototype
630 * quaternionToRpy(quat: table) -> table
631 *
632 * @par JSON-RPC request example
633 * {"jsonrpc":"2.0","method":"Math.quaternionToRpy","params":[[0.834722,
634 * 0.0780426, 0.451893, 0.304864]],"id":1}
635 *
636 * @par JSON-RPC response example
637 * {"id":1,"jsonrpc":"2.0","result":[0.6110000520523781,0.7849996877683915,0.960000543982093]}
638 * \endenglish
639 */
640 std::vector<double> quaternionToRpy(const std::vector<double> &quat);
641
642 /**
643 * \chinese
644 * 四点法标定TCP偏移
645 *
646 * 找一个尖点,将机械臂工具末端点绕着尖点示教四个位置,姿态差别要大。
647 * 设置完毕后即可计算出来结果。
648 *
649 * @param poses 四个点的位姿集合
650 * @return TCP标定结果和标定结果是否有效
651 *
652 * @par Python函数原型
653 * tcpOffsetIdentify(self: pyaubo_sdk.Math, arg0: List[List[float]]) ->
654 * Tuple[List[float], int]
655 *
656 * @par Lua函数原型
657 * tcpOffsetIdentify(poses: table) -> table
658 * \endchinese
659 *
660 * \english
661 * Four point method calibration for TCP offset
662 *
663 * About a sharp point, move the robot's tcp in four different poses. Difference between each pose should be drastic.
664 * Result can be obtained based on these four poses
665 *
666 * @param poses combination of four different poses
667 * @return TCP calibration result and whether successfull
668 *
669 * @par Python interface prototype
670 * tcpOffsetIdentify(self: pyaubo_sdk.Math, arg0: List[List[float]]) ->
671 * Tuple[List[float], int]
672 *
673 * @par Lua interface prototype
674 * tcpOffsetIdentify(poses: table) -> table
675 * \endenglish
676 */
678 const std::vector<std::vector<double>> &poses);
679
680 /**
681 * \chinese
682 * Three point method calibration for TCP offset
683 *
684 * @param poses 三个点的位姿集合
685 * @param type 类型:\n
686 * 0 - oxy 原点 x轴正方向 xy平面(y轴正方向)\n
687 * 1 - oxz 原点 x轴正方向 xz平面(z轴正方向)\n
688 * 2 - oyz 原点 y轴正方向 yz平面(z轴正方向)\n
689 * 3 - oyx 原点 y轴正方向 yx平面(x轴正方向)\n
690 * 4 - ozx 原点 z轴正方向 zx平面(x轴正方向)\n
691 * 5 - ozy 原点 z轴正方向 zy平面(y轴正方向)\n
692 * @return 坐标系标定结果和标定结果是否有效
693 *
694 * @par JSON-RPC请求示例
695 * {"jsonrpc":"2.0","method":"Math.calibrateCoordinate","params":[[[0.55462,0.06219,0.37175,-3.142,0.0,1.580],
696 * [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}
697 *
698 * @par JSON-RPC响应示例
699 * {"id":1,"jsonrpc":"2.0","result":[[0.55462,0.06219,0.37175,-3.722688983883945e-05,-1.6940658945086007e-21,0.5932768162455785],0]}
700 * \endchinese
701 *
702 * \english
703 * Calibrate coordinate system with 3 points
704 *
705 * @param poses set of 3 poses
706 * @param type type:\n
707 * 0 - oxy origin, +x axis, xy plane (+y direction) \n
708 * 1 - oxz origin, +x axis, xz plane (+z direction) \n
709 * 2 - oyz origin, +y axis, yz plane (+z direction) \n
710 * 3 - oyx origin, +y axis, yx plane (+x direction) \n
711 * 4 - ozx origin, +z axis, zx plane (+x direction) \n
712 * 5 - ozy origin, +z axis, zy plane (+y direction) \n
713 * @return Coordinate system calibration result and whether the calibration result is valid
714 *
715 * @par JSON-RPC request example
716 * {"jsonrpc":"2.0","method":"Math.calibrateCoordinate","params":[[[0.55462,0.06219,0.37175,-3.142,0.0,1.580],
717 * [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}
718 *
719 * @par JSON-RPC response example
720 * {"id":1,"jsonrpc":"2.0","result":[[0.55462,0.06219,0.37175,-3.722688983883945e-05,-1.6940658945086007e-21,0.5932768162455785],0]}
721 * \endenglish
722 */
724 const std::vector<std::vector<double>> &poses, int type);
725
726 /**
727 * \chinese
728 * 根据圆弧的三个点,计算出拟合成的圆的另一半圆弧的中间点位置
729 *
730 * @param p1 圆弧的起始点
731 * @param p2 圆弧的中间点
732 * @param p3 圆弧的结束点
733 * @param mode 当mode等于1的时候,表示需要对姿态进行圆弧规划;
734 * 当mode等于0的时候,表示不需要对姿态进行圆弧规划
735 *
736 * @return 拟合成的圆的另一半圆弧的中间点位置和计算结果是否有效
737 *
738 * @par JSON-RPC请求示例
739 * {"jsonrpc":"2.0","method":"Math.calculateCircleFourthPoint","params":[[0.5488696249770836,-0.1214996547187204,0.2631931199112321,-3.14159198038469,-3.673205103150083e-06,1.570796326792424],
740 * [0.5488696249770835,-0.1214996547187207,0.3599720701808493,-3.14159198038469,-3.6732051029273e-06,1.570796326792423],
741 * [0.5488696249770836,-0.0389996547187214,0.3599720701808496,-3.141591980384691,-3.673205102557476e-06,
742 * 1.570796326792422],1],"id":1}
743 *
744 * @par JSON-RPC响应示例
745 * {"id":1,"jsonrpc":"2.0","result":[[0.5488696249770837,-0.031860179583911546,0.27033259504604207,-3.1415919803846903,-3.67320510285378e-06,1.570796326792423],1]}
746 *
747 * \endchinese
748 *
749 * \english
750 * Based on three points on an arc, calculate the position of the midpoint of the other half of the fitted circle's arc
751 *
752 * @param p1 start point of the arc
753 * @param p2 middle point of the arc
754 * @param p3 end point of the arc
755 * @param mode when mode = 1, need to plan for orientation around arc;
756 * when mode = 0, do not need to plan for orientation around arc.
757 * @return position of the midpoint of the other half of the fitted circle's arc and whether the result is valid.
758 *
759 * @par JSON-RPC request example
760 * {"jsonrpc":"2.0","method":"Math.calculateCircleFourthPoint","params":[[0.5488696249770836,-0.1214996547187204,0.2631931199112321,-3.14159198038469,-3.673205103150083e-06,1.570796326792424],
761 * [0.5488696249770835,-0.1214996547187207,0.3599720701808493,-3.14159198038469,-3.6732051029273e-06,1.570796326792423],
762 * [0.5488696249770836,-0.0389996547187214,0.3599720701808496,-3.141591980384691,-3.673205102557476e-06,
763 * 1.570796326792422],1],"id":1}
764 *
765 * @par JSON-RPC response example
766 * {"id":1,"jsonrpc":"2.0","result":[[0.5488696249770837,-0.031860179583911546,0.27033259504604207,-3.1415919803846903,-3.67320510285378e-06,1.570796326792423],1]}
767 *
768 * \endenglish
769 */
770 ResultWithErrno calculateCircleFourthPoint(const std::vector<double> &p1,
771 const std::vector<double> &p2,
772 const std::vector<double> &p3,
773 int mode);
774 /**
775 * \chinese
776 * @brief forceTrans:
777 * 变换力和力矩的参考坐标系 force_in_b = pose_a_in_b * force_in_a
778 * @param pose_a_in_b: a 坐标系在 b 坐标系的位姿
779 * @param force_in_a: 力和力矩在 a 坐标系的描述
780 * @return force_in_b,力和力矩在 b 坐标系的描述
781 * \endchinese
782 *
783 * \english
784 * @brief forceTrans:
785 * Transform the reference frame of force and torque: force_in_b = pose_a_in_b * force_in_a
786 * @param pose_a_in_b: pose of frame a in frame b
787 * @param force_in_a: force and torque described in frame a
788 * @return Force_in_b, force and torque described in frame b
789 * \endenglish
790 */
791 std::vector<double> forceTrans(const std::vector<double> &pose_a_in_b,
792 const std::vector<double> &force_in_a);
793
794 /**
795 * \chinese
796 * @brief 通过距离计算工具坐标系下的位姿增量
797 * @param distances: N 个距离, N >=3
798 * @param position: 距离参考轨迹的保持高度
799 * @param radius: 传感器中心距离末端tcp的等效半径
800 * @param track_scale: 跟踪比例, 设置范围(0, 1], 1表示跟踪更快
801 * @return 基于工具坐标系的位姿增量
802 * \endchinese
803 *
804 * \english
805 * @brief Calculate pose increment in tool coordinate system based on sensor data
806 * @param distances: N distances, N >= 3
807 * @param position: reference height to maintain from the trajectory
808 * @param radius: effective radius from sensor center to tool TCP
809 * @param track_scale: tracking ratio, range (0, 1], 1 means faster tracking
810 * @return Pose increment in tool coordinate system
811 * \endenglish
812 */
813 std::vector<double> getDeltaPoseBySensorDistance(
814 const std::vector<double> &distances, double position, double radius,
815 double track_scale);
816
817 /**
818 * \chinese
819 * @brief changeFTFrame: 变换力和力矩的参考坐标系
820 * @param pose_a_in_b: a 坐标系在 b 坐标系的位姿
821 * @param ft_in_a: 作用在 a 点的力和力矩在 a 坐标系的描述
822 * @return ft_in_b,作用在 b 点的力和力矩在 b 坐标系的描述
823 * \endchinese
824 *
825 * \english
826 * @brief changeFTFrame: Transform the reference frame of force and torque
827 * @param pose_a_in_b: pose of frame a in frame b
828 * @param ft_in_a: force and torque applied at point a, described in frame a
829 * @return ft_in_b, force and torque applied at point b, described in frame b
830 * \endenglish
831 */
832 std::vector<double> deltaPoseTrans(const std::vector<double> &pose_a_in_b,
833 const std::vector<double> &ft_in_a);
834
835 /**
836 * \chinese
837 * @brief addDeltaPose: 计算以给定速度变换单位时间后的位姿
838 * @param pose_a_in_b: 当前时刻 a 相对于 b 的位姿
839 * @param v_in_b: 当前时刻 a 坐标系的速度在 b 的描述
840 * @return pose_in_b, 单位时间后的位姿在 b 的描述
841 * \endchinese
842 *
843 * \english
844 * @brief addDeltaPose: Calculate the pose after unit time given a velocity
845 * @param pose_a_in_b: current pose of a relative to b
846 * @param v_in_b: velocity of frame a described in frame b at current time
847 * @return pose_in_b, pose after unit time described in frame b
848 * \endenglish
849 */
850 std::vector<double> deltaPoseAdd(const std::vector<double> &pose_a_in_b,
851 const std::vector<double> &v_in_b);
852
853 /**
854 * \chinese
855 * @brief changePoseWithXYRef: 修改 pose_tar 的xy轴方向,尽量与 pose_ref 一致,
856 * @param pose_tar: 需要修改的目标位姿
857 * @param pose_ref: 参考位姿
858 * @return 修改后的位姿,采用pose_tar的 xyz 坐标和 z 轴方向
859 * \endchinese
860 *
861 * \english
862 * @brief changePoseWithXYRef: Modify the XY axis direction of pose_tar to be as consistent as possible with pose_ref
863 * @param pose_tar: target pose to be modified
864 * @param pose_ref: reference pose
865 * @return Modified pose, using the xyz coordinates and z axis direction of pose_tar
866 * \endenglish
867 */
868 std::vector<double> changePoseWithXYRef(
869 const std::vector<double> &pose_tar,
870 const std::vector<double> &pose_ref);
871
872 /**
873 * \chinese
874 * @brief homMatrixToPose: 由齐次变换矩阵得到位姿
875 * @param homMatrix: 4*4 齐次变换矩阵, 输入元素采用横向排列
876 * @return 对应的位姿
877 * \endchinese
878 *
879 * \english
880 * @brief homMatrixToPose: Get pose from homogeneous transformation matrix
881 * @param homMatrix: 4x4 homogeneous transformation matrix, input elements are arranged row-wise
882 * @return corresponding pose
883 * \endenglish
884 */
885 std::vector<double> homMatrixToPose(const std::vector<double> &homMatrix);
886
887 /**
888 * \chinese
889 * @brief poseToHomMatrix: 位姿变换得到齐次变换矩阵
890 * @param pose: 输入的位姿
891 * @return 输出的齐次变换矩阵,元素横向排列
892 * \endchinese
893 *
894 * \english
895 * @brief poseToHomMatrix: Get homogeneous transformation matrix from pose
896 * @param pose: input pose
897 * @return output homogeneous transformation matrix, elements arranged row-wise
898 * \endenglish
899 */
900 std::vector<double> poseToHomMatrix(const std::vector<double> &pose);
901
902protected:
903 void *d_;
904};
905using MathPtr = std::shared_ptr<Math>;
906
907} // namespace common_interface
908} // namespace arcs
909#endif
std::vector< double > transferRefFrame(const std::vector< double > &F_b_a_old, const Vector3d &V_in_a, int type)
double poseDistance(const std::vector< double > &p1, const std::vector< double > &p2)
计算两个位姿的位置距离
std::vector< double > poseTrans(const std::vector< double > &pose_from, const std::vector< double > &pose_from_to)
位姿变换
std::vector< double > forceTrans(const std::vector< double > &pose_a_in_b, const std::vector< double > &force_in_a)
std::vector< double > poseToHomMatrix(const std::vector< double > &pose)
std::vector< double > poseRotation(const std::vector< double > &pose, const std::vector< double > &rotv)
姿态旋转
ResultWithErrno calculateCircleFourthPoint(const std::vector< double > &p1, const std::vector< double > &p2, const std::vector< double > &p3, int mode)
根据圆弧的三个点,计算出拟合成的圆的另一半圆弧的中间点位置
std::vector< double > changePoseWithXYRef(const std::vector< double > &pose_tar, const std::vector< double > &pose_ref)
std::vector< double > poseAdd(const std::vector< double > &p1, const std::vector< double > &p2)
double poseAngleDistance(const std::vector< double > &p1, const std::vector< double > &p2)
计算两个位姿的轴角距离
std::vector< double > getDeltaPoseBySensorDistance(const std::vector< double > &distances, double position, double radius, double track_scale)
ResultWithErrno calibrateCoordinate(const std::vector< std::vector< double > > &poses, int type)
Three point method calibration for TCP offset
std::vector< double > poseSub(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)
判断两个位姿是否相等
ResultWithErrno tcpOffsetIdentify(const std::vector< std::vector< double > > &poses)
四点法标定TCP偏移
std::vector< double > quaternionToRpy(const std::vector< double > &quat)
四元数转欧拉角
std::vector< double > rpyToQuaternion(const std::vector< double > &rpy)
欧拉角转四元数
std::vector< double > homMatrixToPose(const std::vector< double > &homMatrix)
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)
获取位姿的逆
std::vector< double > interpolatePose(const std::vector< double > &p1, const std::vector< double > &p2, double alpha)
计算线性插值
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::array< double, 3 > Vector3d
Definition type_def.h:29
std::shared_ptr< Math > MathPtr
Definition math.h:905
std::tuple< std::vector< double >, int > ResultWithErrno
Definition type_def.h:698
数据类型的定义