ARCS SDK API  0.25.0
载入中...
搜索中...
未找到
motion_control.h
浏览该文件的文档.
1/** @file motion_control.h
2 * @brief 运动控制接口
3 *
4 * The robot movements are programmed as pose-to-pose movements, that is move
5 * from the current position to a new position. The path between these two
6 * positions is then automatically calculated by the robot.
7 */
8#ifndef AUBO_SDK_MOTION_CONTROL_INTERFACE_H
9#define AUBO_SDK_MOTION_CONTROL_INTERFACE_H
10
11#include <vector>
12#include <functional>
13#include <thread>
14
15#include <aubo/global_config.h>
16#include <aubo/type_def.h>
17
18namespace arcs {
19namespace common_interface {
20
21/**
22 * pathBuffer类型
23 */
25{
26 PathBuffer_TOPPRA = 1, ///< 1: toppra 时间最优路径规划
27 PathBuffer_CubicSpline = 2, ///< 2: cubic_spline(录制的轨迹)
28
29 /// 3: 关节B样条插值,最少三个点
30 /// 废弃,建议用5替代,现在实际是关节空间 CUBIC_SPLINE
32
33 /// 4:关节B样条插值,最少三个点,但是传入的是笛卡尔空间位姿
34 /// 废弃,建议用6替代,现在实际是关节空间 CUBIC_SPLINE
36
37 ///< 5: 关节B样条插值,最少三个点
39
40 /// 6:关节B样条插值,最少三个点,但是传入的是笛卡尔空间位姿
42};
43
44/**
45 * MotionControl类
46 */
47class ARCS_ABI_EXPORT MotionControl
48{
49public:
51 virtual ~MotionControl();
52
53 /**
54 * 获取等效半径,单位 m
55 * moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动
56 * 可以通过 setEqradius 设置,默认为1
57 *
58 * @return 返回等效半径
59 *
60 * @throws arcs::common_interface::AuboException
61 *
62 * @par Python函数原型
63 * getEqradius(self: pyaubo_sdk.MotionControl) -> float
64 *
65 * @par Lua函数原型
66 * getEqradius() -> number
67 *
68 * @par JSON-RPC请求示例
69 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getEqradius","params":[],"id":1}
70 *
71 * @par JSON-RPC响应示例
72 * {"id":1,"jsonrpc":"2.0","result":1.0}
73 *
74 */
75 double getEqradius();
76
77 /**
78 * 设置等效半径,单位 m
79 * moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动,数值越大,姿态旋转速度越快
80 *
81 * @param eqradius 0表示只规划移动,姿态旋转跟随移动
82 *
83 * @return 成功返回0; 失败返回错误码
84 * AUBO_BAD_STATE
85 * AUBO_BUSY
86 * -AUBO_INVL_ARGUMENT
87 * -AUBO_BAD_STATE
88 *
89 * @throws arcs::common_interface::AuboException
90 *
91 * @par JSON-RPC请求示例
92 * {"jsonrpc":"2.0","method":"rob1.MotionControl.setEqradius","params":[0.8],"id":1}
93 *
94 * @par JSON-RPC响应示例
95 * {"id":1,"jsonrpc":"2.0","result":0}
96 *
97 * @par Python函数原型
98 * setEqradius(self: pyaubo_sdk.MotionControl, arg0: float) -> int
99 *
100 * @par Lua函数原型
101 * setEqradius(eqradius: number) -> number
102 */
103 int setEqradius(double eqradius);
104
105 /**
106 * 动态调整机器人运行速度和加速度比例 (0., 1.]
107 *
108 * @param fraction 机器人运行速度和加速度比例
109 * @return 成功返回0; 失败返回错误码
110 * AUBO_INVL_ARGUMENT
111 * AUBO_BUSY
112 * AUBO_BAD_STATE
113 * -AUBO_INVL_ARGUMENT
114 * -AUBO_BAD_STATE
115 *
116 * @throws arcs::common_interface::AuboException
117 *
118 * @par Python函数原型
119 * setSpeedFraction(self: pyaubo_sdk.MotionControl, arg0: float) -> int
120 *
121 * @par Lua函数原型
122 * setSpeedFraction(fraction: number) -> nil
123 *
124 * @par JSON-RPC请求示例
125 * {"jsonrpc":"2.0","method":"rob1.MotionControl.setSpeedFraction","params":[0.8],"id":1}
126 *
127 * @par JSON-RPC响应示例
128 * {"id":1,"jsonrpc":"2.0","result":0}
129 *
130 */
131 int setSpeedFraction(double fraction);
132
133 /**
134 * 获取速度和加速度比例,默认为 1
135 * 可以通过 setSpeedFraction 接口设置
136 *
137 * @return 返回速度和加速度比例
138 *
139 * @throws arcs::common_interface::AuboException
140 *
141 * @par Python函数原型
142 * getSpeedFraction(self: pyaubo_sdk.MotionControl) -> float
143 *
144 * @par Lua函数原型
145 * getSpeedFraction() -> number
146 *
147 * @par JSON-RPC请求示例
148 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getSpeedFraction","params":[],"id":1}
149 *
150 * @par JSON-RPC响应示例
151 * {"id":1,"jsonrpc":"2.0","result":1.0}
152 *
153 */
155
156 /**
157 * 速度比例设置临界区,使能之后速度比例被强制设定为1. ,
158 * 失能之后恢复之前的速度比例
159 *
160 * @param enable
161 * @return 成功返回0; 失败返回错误码
162 * AUBO_BAD_STATE
163 * AUBO_BUSY
164 * -AUBO_BAD_STATE
165 *
166 * @throws arcs::common_interface::AuboException
167 *
168 * @par JSON-RPC请求示例
169 * {"jsonrpc":"2.0","method":"rob1.MotionControl.speedFractionCritical","params":[true],"id":1}
170 *
171 * @par JSON-RPC响应示例
172 * {"id":1,"jsonrpc":"2.0","result":0}
173 *
174 */
175 int speedFractionCritical(bool enable);
176
177 /**
178 * 是否处于速度比例设置临界区
179 *
180 * @return 处于速度比例设置临界区返回 true; 反之返回 false
181 *
182 * @throws arcs::common_interface::AuboException
183 *
184 * @par JSON-RPC请求示例
185 * {"jsonrpc":"2.0","method":"rob1.MotionControl.isSpeedFractionCritical","params":[],"id":1}
186 *
187 * @par JSON-RPC响应示例
188 * {"id":1,"jsonrpc":"2.0","result":true}
189 *
190 */
192
193 /**
194 * 是否处交融区
195 *
196 * @return 处交融区返回 true; 反之返回 false
197 *
198 * @throws arcs::common_interface::AuboException
199 *
200 * @par JSON-RPC请求示例
201 * {"jsonrpc":"2.0","method":"rob1.MotionControl.isBlending","params":[],"id":1}
202 *
203 * @par JSON-RPC响应示例
204 * {"id":1,"jsonrpc":"2.0","result":false}
205 *
206 */
208
209 /**
210 * 设置偏移的最大速度和最大加速度
211 * 仅对pathOffsetSet中 type=1 有效
212 * @param v 最大速度
213 * @param a 最大加速度
214 * @return 成功返回0;失败返回错误码
215 * AUBO_BUSY
216 * AUBO_BAD_STATE
217 * -AUBO_INVL_ARGUMENT
218 * -AUBO_BAD_STATE
219 *
220 * @throws arcs::common_interface::AuboException
221 *
222 * @par Python函数原型
223 * pathOffsetLimits(self: pyaubo_sdk.MotionControl, arg0: float, arg1:
224 * float)
225 * -> int
226 *
227 * @par Lua函数原型
228 * pathOffsetLimits(v: number, a: number) -> nil
229 *
230 */
231 int pathOffsetLimits(double v, double a);
232
233 /**
234 * 设置偏移的参考坐标系
235 * 仅对pathOffsetSet中 type=1 有效
236 * @param ref_coord 参考坐标系 0-基坐标系 1-TCP
237 *
238 * @throws arcs::common_interface::AuboException
239 *
240 * @par Python函数原型
241 * pathOffsetCoordinate(self: pyaubo_sdk.MotionControl, arg0: int) -> float
242 *
243 * @par Lua函数原型
244 * pathOffsetCoordinate(ref_coord: number) -> number
245 *
246 * @par JSON-RPC请求示例
247 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathOffsetCoordinate","params":[0],"id":1}
248 *
249 * @par JSON-RPC响应示例
250 * {"id":1,"jsonrpc":"2.0","result":0}
251 *
252 */
253 int pathOffsetCoordinate(int ref_coord);
254
255 /**
256 * 路径偏移使能
257 *
258 * @return 成功返回0; 失败返回错误码
259 * AUBO_BUSY
260 * AUBO_BAD_STATE
261 * -AUBO_BAD_STATE
262 *
263 * @throws arcs::common_interface::AuboException
264 *
265 * @par Python函数原型
266 * pathOffsetEnable(self: pyaubo_sdk.MotionControl) -> int
267 *
268 * @par Lua函数原型
269 * pathOffsetEnable() -> number
270 *
271 * @par JSON-RPC请求示例
272 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathOffsetEnable","params":[],"id":1}
273 *
274 * @par JSON-RPC响应示例
275 * {"id":1,"jsonrpc":"2.0","result":0}
276 *
277 */
279
280 /**
281 * 设置路径偏移
282 *
283 * @param offset 在各方向的位姿偏移
284 * @param type 运动类型 0-位置规划 1-速度规划
285 * @return 成功返回0; 失败返回错误码
286 * AUBO_BAD_STATE
287 * AUBO_BUSY
288 * -AUBO_INVL_ARGUMENT
289 * -AUBO_BAD_STATE
290 *
291 * @throws arcs::common_interface::AuboException
292 *
293 * @par Python函数原型
294 * pathOffsetSet(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
295 * int) -> int
296 *
297 * @par Lua函数原型
298 * pathOffsetSet(offset: table, type: number) -> nil
299 *
300 * @par JSON-RPC请求示例
301 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathOffsetSet","params":[[0,0,0.01,0,0,0],0],"id":1}
302 *
303 * @par JSON-RPC响应示例
304 * {"id":1,"jsonrpc":"2.0","result":0}
305 *
306 */
307 int pathOffsetSet(const std::vector<double> &offset, int type = 0);
308
309 /**
310 * 路径偏移失能
311 *
312 * @return 成功返回0; 失败返回错误码
313 * AUBO_BAD_STATE
314 * AUBO_BUSY
315 * -AUBO_BAD_STATE
316 *
317 * @throws arcs::common_interface::AuboException
318 *
319 * @par Python函数原型
320 * pathOffsetDisable(self: pyaubo_sdk.MotionControl) -> int
321 *
322 * @par Lua函数原型
323 * pathOffsetDisable() -> nil
324 *
325 * @par JSON-RPC请求示例
326 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathOffsetDisable","params":[],"id":1}
327 *
328 * @par JSON-RPC响应示例
329 * {"id":1,"jsonrpc":"2.0","result":0}
330 *
331 */
333
334 /**
335 * @brief 监控轨迹偏移范围
336 * @param min: 沿坐标轴负方向最大偏移量
337 * @param max: 沿坐标轴负正方向最大偏移量
338 * @param strategy: 达到最大偏移量后监控策略
339 *     0-禁用监控
340 *     1-饱和限制,即维持最大姿态
341 *     2-保护停止
342 * @return
343 *
344 * @throws arcs::common_interface::AuboException
345 *
346 * @par Python函数原型
347 * pathOffsetSupv(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
348 * List[float], arg2: int) -> int
349 *
350 * @par Lua函数原型
351 * pathOffsetSupv(min: table, max: table, strategy: number) -> number
352 */
353 int pathOffsetSupv(const std::vector<double> &min,
354 const std::vector<double> &max, int strategy);
355
356 /**
357 * 关节偏移使能
358 *
359 * @return 成功返回0; 失败返回错误码
360 * AUBO_BAD_STATE
361 * AUBO_BUSY
362 * -AUBO_BAD_STATE
363 *
364 * @throws arcs::common_interface::AuboException
365 *
366 * @par Python函数原型
367 * jointOffsetEnable(self: pyaubo_sdk.MotionControl) -> int
368 *
369 * @par Lua函数原型
370 * jointOffsetEnable() -> nil
371 *
372 * @par JSON-RPC请求示例
373 * {"jsonrpc":"2.0","method":"rob1.MotionControl.jointOffsetEnable","params":[],"id":1}
374 *
375 * @par JSON-RPC响应示例
376 * {"id":1,"jsonrpc":"2.0","result":0}
377 *
378 */
380
381 /**
382 * 设置关节偏移
383 *
384 * @param offset 在各关节的位姿偏移
385 * @param type
386 * @return 成功返回0; 失败返回错误码
387 * AUBO_BAD_STATE
388 * AUBO_BUSY
389 * -AUBO_INVL_ARGUMENT
390 * -AUBO_BAD_STATE
391 *
392 * @throws arcs::common_interface::AuboException
393 *
394 * @par Python函数原型
395 * jointOffsetSet(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
396 * int) -> int
397 *
398 * @par Lua函数原型
399 * jointOffsetSet(offset: table, type: number) -> nil
400 *
401 * @par JSON-RPC请求示例
402 * {"jsonrpc":"2.0","method":"rob1.MotionControl.jointOffsetSet","params":[[0.1,0,0,0,0],1],"id":1}
403 *
404 * @par JSON-RPC响应示例
405 * {"id":1,"jsonrpc":"2.0","result":0}
406 *
407 */
408 int jointOffsetSet(const std::vector<double> &offset, int type = 1);
409
410 /**
411 * 关节偏移失能
412 *
413 * @return 成功返回0; 失败返回错误码
414 * AUBO_BAD_STATE
415 * AUBO_BUSY
416 * -AUBO_BAD_STATE
417 *
418 * @throws arcs::common_interface::AuboException
419 *
420 * @par Python函数原型
421 * jointOffsetDisable(self: pyaubo_sdk.MotionControl) -> int
422 *
423 * @par Lua函数原型
424 * jointOffsetDisable() -> nil
425 *
426 * @par JSON-RPC请求示例
427 * {"jsonrpc":"2.0","method":"rob1.MotionControl.jointOffsetDisable","params":[],"id":1}
428 *
429 * @par JSON-RPC响应示例
430 * {"id":1,"jsonrpc":"2.0","result":0}
431 *
432 */
434
435 /**
436 * 获取已经入队的指令段(INST)数量,运动指令包括
437 * moveJoint/moveLine/moveCircle 等运动指令以及 setPayload 等配置指令
438 *
439 * 指令一般会在接口宏定义里面用 _INST 指示, 比如 moveJoint
440 * _INST(MotionControl, 5, moveJoint, q, a, v, blend_radius, duration)
441 *
442 * @return 已经入队的指令段数量
443 *
444 * @throws arcs::common_interface::AuboException
445 *
446 * @par Python函数原型
447 * getQueueSize(self: pyaubo_sdk.MotionControl) -> int
448 *
449 * @par Lua函数原型
450 * getQueueSize() -> number
451 *
452 * @par JSON-RPC请求示例
453 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getQueueSize","params":[],"id":1}
454 *
455 * @par JSON-RPC响应示例
456 * {"id":1,"jsonrpc":"2.0","result":0}
457 *
458 */
460
461 /**
462 * 获取已经入队的运动规划插补点数量
463 *
464 * @return 已经入队的运动规划插补点数量
465 *
466 * @throws arcs::common_interface::AuboException
467 *
468 * @par Python函数原型
469 * getTrajectoryQueueSize(self: pyaubo_sdk.MotionControl) -> int
470 *
471 * @par Lua函数原型
472 * getTrajectoryQueueSize() -> number
473 *
474 * @par JSON-RPC请求示例
475 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getTrajectoryQueueSize","params":[],"id":1}
476 *
477 * @par JSON-RPC响应示例
478 * {"id":1,"jsonrpc":"2.0","result":0}
479 *
480 */
482
483 /**
484 * 获取当前正在插补的运动指令段的ID
485 *
486 * @return 当前正在插补的运动指令段的ID
487 * @retval -1 表示轨迹队列为空 \n
488 * 像movePathBuffer运动中的buffer或者规划器(moveJoint和moveLine等)里的队列都属于轨迹队列
489 *
490 * @throws arcs::common_interface::AuboException
491 *
492 * @par Python函数原型
493 * getExecId(self: pyaubo_sdk.MotionControl) -> int
494 *
495 * @par Lua函数原型
496 * getExecId() -> number
497 *
498 * @par JSON-RPC请求示例
499 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getExecId","params":[],"id":1}
500 *
501 * @par JSON-RPC响应示例
502 * {"id":1,"jsonrpc":"2.0","result":-1}
503 *
504 */
506
507 /**
508 * 获取指定ID的运动指令段的预期执行时间
509 *
510 * @param id 运动指令段ID
511 * @return 返回预期执行时间
512 *
513 * @throws arcs::common_interface::AuboException
514 *
515 * @par Python函数原型
516 * getDuration(self: pyaubo_sdk.MotionControl, arg0: int) -> float
517 *
518 * @par Lua函数原型
519 * getDuration(id: number) -> number
520 *
521 * @par JSON-RPC请求示例
522 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getDuration","params":[16],"id":1}
523 *
524 * @par JSON-RPC响应示例
525 * {"id":1,"jsonrpc":"2.0","result":0.0}
526 *
527 */
528 double getDuration(int id);
529
530 /**
531 * 获取指定ID的运动指令段的剩余执行时间
532 *
533 * @param id 运动指令段ID
534 * @return 返回剩余执行时间
535 *
536 * @throws arcs::common_interface::AuboException
537 *
538 * @par Python函数原型
539 * getMotionLeftTime(self: pyaubo_sdk.MotionControl, arg0: int) -> float
540 *
541 * @par Lua函数原型
542 * getMotionLeftTime(id: number) -> number
543 *
544 * @par JSON-RPC请求示例
545 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getMotionLeftTime","params":[16],"id":1}
546 *
547 * @par JSON-RPC响应示例
548 * {"id":1,"jsonrpc":"2.0","result":0.0}
549 *
550 */
551 double getMotionLeftTime(int id);
552
553 /**
554 * StopMove is used to stop robot and external axes movements and any
555 * belonging process temporarily. If the instruction StartMove is given then
556 * the movement and process resumes.
557 *
558 * This instruction can, for example, be used in a trap routine to stop the
559 * robot temporarily when an interrupt occurs.
560 *
561 * @param quick true: Stops the robot on the path as fast as possible.
562 * Without the optional parameter \Quick, the robot stops on the path, but
563 * the braking distance is longer (same as for normal Program Stop).
564 *
565 * @return 成功返回0; 失败返回错误码
566 * AUBO_BAD_STATE
567 * AUBO_BUSY
568 * -AUBO_BAD_STATE
569 *
570 * @throws arcs::common_interface::AuboException
571 *
572 */
573 int stopMove(bool quick, bool all_tasks);
574
575 /**
576 * StartMove is used to resume robot, external axes movement and belonging
577 * process after the movement has been stopped
578 *
579 * • by the instruction StopMove.
580 * • after execution of StorePath ... RestoPath sequence.
581 * • after asynchronously raised movements errors, such as ERR_PATH_STOP or
582 * specific process error after handling in the ERROR handler.
583 *
584 * @return 成功返回0; 失败返回错误码
585 * AUBO_BAD_STATE
586 * AUBO_BUSY
587 * -AUBO_BAD_STATE
588 *
589 * @throws arcs::common_interface::AuboException
590 *
591 * @par JSON-RPC请求示例
592 * {"jsonrpc":"2.0","method":"rob1.MotionControl.startMove","params":[],"id":1}
593 *
594 * @par JSON-RPC响应示例
595 * {"id":1,"jsonrpc":"2.0","result":0}
596 *
597 */
599
600 /**
601 * storePath
602 *
603 * @param keep_sync
604 * @return 成功返回0; 失败返回错误码
605 * AUBO_BAD_STATE
606 * AUBO_BUSY
607 * -AUBO_BAD_STATE
608 *
609 * @throws arcs::common_interface::AuboException
610 *
611 */
612 int storePath(bool keep_sync);
613
614 /**
615 * ClearPath (Clear Path) clears the whole motion path on the current motion
616 * path level (base level or StorePath level).
617 *
618 * @return 成功返回0; 失败返回错误码
619 * AUBO_BAD_STATE
620 * AUBO_BUSY
621 * -AUBO_BAD_STATE
622 *
623 * @throws arcs::common_interface::AuboException
624 *
625 * @par JSON-RPC请求示例
626 * {"jsonrpc":"2.0","method":"rob1.MotionControl.clearPath","params":[],"id":1}
627 *
628 * @par JSON-RPC响应示例
629 * {"id":1,"jsonrpc":"2.0","result":0}
630 *
631 */
633
634 /**
635 * restoPath
636 *
637 * @return 成功返回0; 失败返回错误码
638 * AUBO_BAD_STATE
639 * AUBO_BUSY
640 * -AUBO_BAD_STATE
641 *
642 * @throws arcs::common_interface::AuboException
643 *
644 * @par JSON-RPC请求示例
645 * {"jsonrpc":"2.0","method":"rob1.MotionControl.restoPath","params":[],"id":1}
646 *
647 * @par JSON-RPC响应示例
648 * {"id":1,"jsonrpc":"2.0","result":0}
649 *
650 */
652
653 /**
654 * 获取当前运动指令段的执行进度
655 *
656 * @return 返回执行进度
657 *
658 * @throws arcs::common_interface::AuboException
659 *
660 * @par Python函数原型
661 * getProgress(self: pyaubo_sdk.MotionControl) -> float
662 *
663 * @par Lua函数原型
664 * getProgress() -> number
665 *
666 * @par JSON-RPC请求示例
667 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getProgress","params":[],"id":1}
668 *
669 * @par JSON-RPC响应示例
670 * {"id":1,"jsonrpc":"2.0","result":0.0}
671 *
672 */
673 double getProgress();
674
675 /**
676 * 当工件安装在另外一台机器人的末端或者外部轴上时,指定其名字和安装位置
677 *
678 * @note 暂未实现
679 *
680 * @param module_name 控制模块名字
681 * @param mounting_pose 抓取的相对位置,
682 * 如果是机器人,则相对于机器人末端中心点(非TCP点)
683 * @return 成功返回0; 失败返回错误码
684 * AUBO_BAD_STATE
685 * AUBO_BUSY
686 * -AUBO_INVL_ARGUMENT
687 * -AUBO_BAD_STATE
688 *
689 * @throws arcs::common_interface::AuboException
690 *
691 * @par Python函数原型
692 * setWorkObjectHold(self: pyaubo_sdk.MotionControl, arg0: str, arg1:
693 * List[float]) -> int
694 *
695 * @par Lua函数原型
696 * setWorkObjectHold(module_name: string, mounting_pose: table) -> nil
697 *
698 */
699 int setWorkObjectHold(const std::string &module_name,
700 const std::vector<double> &mounting_pose);
701
702 /**
703 * getWorkObjectHold
704 *
705 * @note 暂未实现
706 *
707 * @return
708 *
709 * @throws arcs::common_interface::AuboException
710 *
711 * @par Python函数原型
712 * getWorkObjectHold(self: pyaubo_sdk.MotionControl) -> Tuple[str,
713 * List[float]]
714 *
715 * @par Lua函数原型
716 * getWorkObjectHold() -> table
717 *
718 * @par JSON-RPC请求示例
719 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getWorkObjectHold","params":[],"id":1}
720 *
721 * @par JSON-RPC响应示例
722 * {"id":1,"jsonrpc":"2.0","result":["",[]]}
723 *
724 */
725 std::tuple<std::string, std::vector<double>> getWorkObjectHold();
726
727 /**
728 * getPauseJointPositions
729 *
730 * @note 获取暂停点关节位置
731 * 常用于运行工程时发生碰撞后继续运动过程中(先通过resumeMoveJoint或resumeMoveLine运动到暂停位置,再恢复工程)
732 *
733 * @return 暂停关节位置
734 *
735 * @throws arcs::common_interface::AuboException
736 *
737 * @par Python函数原型
738 * getPauseJointPositions(self: pyaubo_sdk.MotionControl) -> List[float]
739 *
740 * @par Lua函数原型
741 * getPauseJointPositions() -> table
742 *
743 * @par JSON-RPC请求示例
744 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getPauseJointPositions","params":[],"id":1}
745 *
746 * @par JSON-RPC响应示例
747 * {"id":1,"jsonrpc":"2.0","result":[8.2321e-13,-0.200999,1.33999,0.334999,1.206,-6.39383e-12]}
748 *
749 */
750 std::vector<double> getPauseJointPositions();
751
752 /**
753 * 设置伺服模式
754 * 使用 setServoModeSelect 替代
755 *
756 * @param enable 是否使能
757 * @return 成功返回0; 失败返回错误码
758 * AUBO_BAD_STATE
759 * AUBO_BUSY
760 * -AUBO_BAD_STATE
761 *
762 * @throws arcs::common_interface::AuboException
763 *
764 * @par Python函数原型
765 * setServoMode(self: pyaubo_sdk.MotionControl, arg0: bool) -> int
766 *
767 * @par Lua函数原型
768 * setServoMode(enable: boolean) -> nil
769 *
770 * @par JSON-RPC请求示例
771 * {"jsonrpc":"2.0","method":"rob1.MotionControl.setServoMode","params":[true],"id":1}
772 *
773 * @par JSON-RPC响应示例
774 * {"id":1,"jsonrpc":"2.0","result":0}
775 *
776 */
777 ARCS_DEPRECATED int setServoMode(bool enable);
778
779 /**
780 * 判断伺服模式是否使能
781 * 使用 getServoModeSelect 替代
782 *
783 * @return 已使能返回true,反之则返回false
784 *
785 * @throws arcs::common_interface::AuboException
786 *
787 * @par Python函数原型
788 * isServoModeEnabled(self: pyaubo_sdk.MotionControl) -> bool
789 *
790 * @par Lua函数原型
791 * isServoModeEnabled() -> boolean
792 *
793 * @par JSON-RPC请求示例
794 * {"jsonrpc":"2.0","method":"rob1.MotionControl.isServoModeEnabled","params":[],"id":1}
795 *
796 * @par JSON-RPC响应示例
797 * {"id":1,"jsonrpc":"2.0","result":false}
798 *
799 */
800 ARCS_DEPRECATED bool isServoModeEnabled();
801
802 /**
803 * 设置伺服运动模式
804 *
805 * @param mode
806 * 0-退出伺服模式
807 * 1-规划伺服模式
808 * 2-透传模式(直接下发)
809 * 3-透传模式(缓存)
810 * @return
811 *
812 * @par JSON-RPC请求示例
813 * {"jsonrpc":"2.0","method":"rob1.MotionControl.setServoModeSelect","params":[0],"id":1}
814 *
815 * @par JSON-RPC响应示例
816 * {"id":1,"jsonrpc":"2.0","result":0}
817 *
818 */
819 int setServoModeSelect(int mode);
820
821 /**
822 * 获取伺服运动模式
823 *
824 * @return
825 *
826 * @par JSON-RPC请求示例
827 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getServoModeSelect","params":[],"id":1}
828 *
829 * @par JSON-RPC响应示例
830 * {"id":1,"jsonrpc":"2.0","result":0}
831 *
832 */
834
835 /**
836 * 关节空间伺服
837 *
838 * 目前可用参数只有 q 和 t;
839 * @param q 关节角, 单位 rad,
840 * @param a 加速度, 单位 rad/s^2,
841 * @param v 速度,单位 rad/s,
842 * @param t 运行时间,单位 s \n
843 * t 值越大,机器臂运动越慢,反之,运动越快;
844 * 该参数最优值为连续调用 servoJoint 接口的间隔时间。
845 * @param lookahead_time 前瞻时间,单位 s \n
846 * 指定机器臂开始减速前要运动的时长,用前瞻时间来平滑轨迹[0.03, 0.2],
847 * 当 lookahead_time 小于一个控制周期时,越小则超调量越大,
848 * 该参数最优值为一个控制周期。
849 * @param gain 比例增益
850 * 跟踪目标位置的比例增益[100, 200],
851 * 用于控制运动的顺滑性和精度,
852 * 比例增益越大,到达目标位置的时间越长,超调量越小。
853 *
854 * @return 成功返回0;失败返回错误码,正数为警告,负数为错误。
855 * AUBO_BAD_STATE(1): 当前安全模式处于非 Normal、ReducedMode、Recovery 状态;
856 * AUBO_BUSY(3): 上一条指令正在执行中;
857 * AUBO_QUEUE_FULL(2): 轨迹队列已满;
858 * -AUBO_REQUEST_IGNORE(-13): 当前处于非 servo 模式;
859 * -AUBO_BAD_STATE(-1): 当前机器臂模式处于非 Running 状态;
860 * -AUBO_TIMEOUT(-4): 调用接口超时;
861 * -AUBO_INVL_ARGUMENT(-5): 轨迹位置超限或速度超限;
862 *
863 * @throws arcs::common_interface::AuboException
864 *
865 * @par Python函数原型
866 * servoJoint(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
867 * float, arg2: float, arg3: float, arg4: float, arg5: float) -> int
868 *
869 * @par Lua函数原型
870 * servoJoint(q: table, a: number, v: number, t: number, lookahead_time:
871 * number, gain: number) -> nil
872 *
873 * @par JSON-RPC请求示例
874 * {"jsonrpc":"2.0","method":"rob1.MotionControl.servoJoint","params":[[0,0,0,0,0,0],0,0,10,0,0],"id":1}
875 *
876 * @par JSON-RPC响应示例
877 * {"id":1,"jsonrpc":"2.0","result":-13}
878 *
879 */
880 int servoJoint(const std::vector<double> &q, double a, double v, double t,
881 double lookahead_time, double gain);
882
883 /**
884 * 笛卡尔空间伺服
885 *
886 * 目前可用参数只有 pose 和 t;
887 * @param pose 位姿, 单位 m,
888 * @param a 加速度, 单位 m/s^2,
889 * @param v 速度,单位 m/s,
890 * @param t 运行时间,单位 s \n
891 * t 值越大,机器臂运动越慢,反之,运动越快;
892 * 该参数最优值为连续调用 servoCartesian 接口的间隔时间。
893 * @param lookahead_time 前瞻时间,单位 s \n
894 * 指定机器臂开始减速前要运动的时长,用前瞻时间来平滑轨迹[0.03, 0.2],
895 * 当 lookahead_time 小于一个控制周期时,越小则超调量越大,
896 * 该参数最优值为一个控制周期。
897 * @param gain 比例增益
898 * 跟踪目标位置的比例增益[100, 200],
899 * 用于控制运动的顺滑性和精度,
900 * 比例增益越大,到达目标位置的时间越长,超调量越小。
901 *
902 * @return 成功返回0;失败返回错误码,正数为警告,负数为错误。
903 * AUBO_BAD_STATE(1): 当前安全模式处于非 Normal、ReducedMode、Recovery 状态;
904 * AUBO_BUSY(3): 上一条指令正在执行中;
905 * AUBO_QUEUE_FULL(2): 轨迹队列已满;
906 * -AUBO_REQUEST_IGNORE(-13): 当前处于非 servo 模式;
907 * -AUBO_BAD_STATE(-1): 当前机器臂模式处于非 Running 状态;
908 * -AUBO_TIMEOUT(-4): 调用接口超时;
909 * -AUBO_INVL_ARGUMENT(-5): 轨迹位置超限或速度超限;
910 * -AUBO_IK_NO_CONVERGE(-23): 逆解计算不收敛,计算出错;
911 * -AUBO_IK_OUT_OF_RANGE(-24): 逆解计算超出机器人最大限制;
912 * -AUBO_IK_CONFIG_DISMATCH(-25): 逆解输入配置存在错误;
913 * -AUBO_IK_JACOBIAN_FAILED(-26): 逆解雅可比矩阵计算失败;
914 * -AUBO_IK_NO_SOLU(-27): 目标点存在解析解,但均不满足选解条件;
915 * -AUBO_IK_UNKOWN_ERROR(-28): 逆解返回未知类型错误;
916 *
917 * @throws arcs::common_interface::AuboException
918 *
919 * @par Python函数原型
920 * servoCartesian(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
921 * float, arg2: float, arg3: float, arg4: float, arg5: float) -> int
922 *
923 * @par Lua函数原型
924 * servoCartesian(pose: table, a: number, v: number, t: number,
925 * lookahead_time: number, gain: number) -> nil
926 *
927 * @par JSON-RPC请求示例
928 * {"jsonrpc":"2.0","method":"rob1.MotionControl.servoCartesian","params":[[0.58712,-0.15775,0.48703,2.76,0.344,1.432],0,0,10,0,0],"id":1}
929 *
930 * @par JSON-RPC响应示例
931 * {"id":1,"jsonrpc":"2.0","result":-13}
932 *
933 */
934 int servoCartesian(const std::vector<double> &pose, double a, double v,
935 double t, double lookahead_time, double gain);
936
937 /**
938 * 伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等
939 *
940 * @param q
941 * @param extq
942 * @param t
943 * @param smooth_scale
944 * @param delay_sacle
945 * @return
946 */
947 int servoJointWithAxes(const std::vector<double> &q,
948 const std::vector<double> &extq, double a, double v,
949 double t, double lookahead_time, double gain);
950
951 int servoJointWithAxisGroup(const std::vector<double> &q, double a,
952 double v, double t, double lookahead_time,
953 double gain, const std::string &group_name,
954 const std::vector<double> &extq);
955
956 /**
957 * 伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等
958 * 与 servoJointWithAxes 区别在于接收笛卡尔空间位姿而不是关节角度
959 * (由软件内部直接做逆解)
960 *
961 * @param pose
962 * @param extq
963 * @param t
964 * @param smooth_scale
965 * @param delay_sacle
966 * @return
967 */
968 int servoCartesianWithAxes(const std::vector<double> &pose,
969 const std::vector<double> &extq, double a,
970 double v, double t, double lookahead_time,
971 double gain);
972
973 int servoCartesianWithAxisGroup(const std::vector<double> &pose, double a,
974 double v, double t, double lookahead_time,
975 double gain, const std::string &group_name,
976 const std::vector<double> &extq);
977
978 /**
979 * 跟踪运动,用于执行离线轨迹、透传用户规划轨迹等
980 *
981 * @param q
982 * @param smooth_scale
983 * @param delay_sacle
984 * @return
985 *
986 * @par JSON-RPC请求示例
987 * {"jsonrpc":"2.0","method":"rob1.MotionControl.trackJoint","params":[[0,0,0,0,0,0],0.01,0.5,1],"id":1}
988 *
989 * @par JSON-RPC响应示例
990 * {"id":1,"jsonrpc":"2.0","result":0}
991 *
992 */
993 int trackJoint(const std::vector<double> &q, double t, double smooth_scale,
994 double delay_sacle);
995
996 /**
997 * 跟踪运动,用于执行离线轨迹、透传用户规划轨迹等
998 * 与 trackJoint 区别在于接收笛卡尔空间位姿而不是关节角度
999 * (由软件内部直接做逆解)
1000 *
1001 * @param pose
1002 * @param t
1003 * @param smooth_scale
1004 * @param delay_sacle
1005 * @return
1006 *
1007 * @par JSON-RPC请求示例
1008 * {"jsonrpc":"2.0","method":"rob1.MotionControl.trackCartesian","params":[[0.58712,-0.15775,0.48703,2.76,0.344,1.432],0.01,0.5,1],"id":1}
1009 *
1010 * @par JSON-RPC响应示例
1011 * {"id":1,"jsonrpc":"2.0","result":0}
1012 *
1013 */
1014 int trackCartesian(const std::vector<double> &pose, double t,
1015 double smooth_scale, double delay_sacle);
1016
1017 /**
1018 * 关节空间跟随
1019 *
1020 * @note 暂未实现
1021 *
1022 * @throws arcs::common_interface::AuboException
1023 *
1024 * @par Python函数原型
1025 * followJoint(self: pyaubo_sdk.MotionControl, arg0: List[float]) -> int
1026 *
1027 * @par Lua函数原型
1028 * followJoint(q: table) -> nil
1029 *
1030 */
1031 int followJoint(const std::vector<double> &q);
1032
1033 /**
1034 * 笛卡尔空间跟随
1035 *
1036 * @note 暂未实现
1037 *
1038 * @throws arcs::common_interface::AuboException
1039 *
1040 * @par Python函数原型
1041 * followLine(self: pyaubo_sdk.MotionControl, arg0: List[float]) -> int
1042 *
1043 * @par Lua函数原型
1044 * followLine(pose: table) -> nil
1045 *
1046 */
1047 int followLine(const std::vector<double> &pose);
1048
1049 /**
1050 * 关节空间速度跟随
1051 *
1052 * 当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
1053 *
1054 * @param qd 目标关节速度, 单位 rad/s
1055 * @param a 主轴的加速度, 单位 rad/s^2
1056 * @param t 函数返回所需要的时间, 单位 s \n
1057 * 如果 t = 0,当达到目标速度的时候,函数将返回;
1058 * 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。\n
1059 * 如果没有达到目标速度,会减速到零。
1060 * 如果达到了目标速度就是按照目标速度匀速运动。
1061 * @return 成功返回0; 失败返回错误码,正数为警告,负数为错误。
1062 * AUBO_BAD_STATE(1): 当前安全模式处于非 Normal、ReducedMode、Recovery 状态;
1063 * AUBO_BUSY(3): 上一条指令正在执行中;
1064 * -AUBO_BAD_STATE(-1): 当前机器臂模式处于非 Running 状态;
1065 * -AUBO_TIMEOUT(-4): 调用接口超时;
1066 * -AUBO_INVL_ARGUMENT(-5): 参数数组qd的长度小于当前机器臂的自由度;
1067 *
1068 * @throws arcs::common_interface::AuboException
1069 *
1070 * @par Python函数原型
1071 * speedJoint(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1072 * float, arg2: float) -> int
1073 *
1074 * @par Lua函数原型
1075 * speedJoint(qd: table, a: number, t: number) -> nil
1076 *
1077 * @par JSON-RPC请求示例
1078 * {"jsonrpc":"2.0","method":"rob1.MotionControl.speedJoint","params":[[0.2,0,0,0,0,0],1.5,100],"id":1}
1079 *
1080 * @par JSON-RPC响应示例
1081 * {"id":1,"jsonrpc":"2.0","result":0}
1082 *
1083 */
1084 int speedJoint(const std::vector<double> &qd, double a, double t);
1085
1086 /**
1087 * 关节空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置)
1088 *
1089 * 当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
1090 *
1091 * @param qd 目标关节速度, 单位 rad/s
1092 * @param a 主轴的加速度, 单位 rad/s^2
1093 * @param t 函数返回所需要的时间, 单位 s
1094 * 如果 t = 0,当达到目标速度的时候,函数将返回;
1095 * 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。
1096 * 如果没有达到目标速度,会减速到零。
1097 * 如果达到了目标速度就是按照目标速度匀速运动。
1098 * @return 成功返回0; 失败返回错误码
1099 * AUBO_BUSY
1100 * -AUBO_INVL_ARGUMENT
1101 * -AUBO_BAD_STATE
1102 *
1103 * @throws arcs::common_interface::AuboException
1104 *
1105 * @par Python函数原型
1106 * resumeSpeedJoint(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1107 * float, arg2: float) -> int
1108 *
1109 *
1110 * @par Lua函数原型
1111 * resumeSpeedJoint(q: table, a: number, t: number) -> nil
1112 *
1113 * @par JSON-RPC请求示例
1114 * {"jsonrpc":"2.0","method":"rob1.MotionControl.resumeSpeedJoint","params":[[0.2,0,0,0,0,0],1.5,100],"id":1}
1115 *
1116 * @par JSON-RPC响应示例
1117 * {"id":1,"jsonrpc":"2.0","result":-1}
1118 *
1119 */
1120 int resumeSpeedJoint(const std::vector<double> &qd, double a, double t);
1121
1122 /**
1123 * 笛卡尔空间速度跟随
1124 *
1125 * 当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
1126 *
1127 * @param xd 工具速度, 单位 m/s
1128 * @param a 工具位置加速度, 单位 m/s^2
1129 * @param t 函数返回所需要的时间, 单位 s \n
1130 * 如果 t = 0,当达到目标速度的时候,函数将返回;
1131 * 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。
1132 * 如果没有达到目标速度,会减速到零。
1133 * 如果达到了目标速度就是按照目标速度匀速运动。
1134 * @return 成功返回0; 失败返回错误码
1135 * AUBO_BUSY(3): 上一条指令正在执行中;
1136 * AUBO_BAD_STATE(1): 当前安全模式处于非 Normal、ReducedMode、Recovery 状态;
1137 * -AUBO_BAD_STATE(-1): 当前机器臂模式处于非 Running 状态;
1138 * -AUBO_TIMEOUT(-4): 调用接口超时;
1139 *
1140 * @throws arcs::common_interface::AuboException
1141 *
1142 * @par Python函数原型
1143 * speedLine(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1: float,
1144 * arg2: float) -> int
1145 *
1146 * @par Lua函数原型
1147 * speedLine(pose: table, a: number, t: number) -> nil
1148 *
1149 * @par JSON-RPC请求示例
1150 * {"jsonrpc":"2.0","method":"rob1.MotionControl.speedLine","params":[[0.25,0,0,0,0,0],1.2,100],"id":1}
1151 *
1152 * @par JSON-RPC响应示例
1153 * {"id":1,"jsonrpc":"2.0","result":0}
1154 *
1155 */
1156 int speedLine(const std::vector<double> &xd, double a, double t);
1157
1158 /**
1159 * 笛卡尔空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置)
1160 *
1161 * 当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
1162 *
1163 * @param xd 工具速度, 单位 m/s
1164 * @param a 工具位置加速度, 单位 m/s^2
1165 * @param t 函数返回所需要的时间, 单位 s \n
1166 * 如果 t = 0,当达到目标速度的时候,函数将返回;
1167 * 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。
1168 * 如果没有达到目标速度,会减速到零。
1169 * 如果达到了目标速度就是按照目标速度匀速运动。
1170 * @return 成功返回0; 失败返回错误码
1171 * -AUBO_BAD_STATE
1172 *
1173 * @throws arcs::common_interface::AuboException
1174 *
1175 * @par Python函数原型
1176 * resumeSpeedLine(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1177 * float, arg2: float) -> int
1178 *
1179 * @par Lua函数原型
1180 * resumeSpeedLine(pose: table, a: number, t: number) -> nil
1181 *
1182 * @par JSON-RPC请求示例
1183 * {"jsonrpc":"2.0","method":"rob1.MotionControl.resumeSpeedLine","params":[[0.25,0,0,0,0,0],1.2,100],"id":1}
1184 *
1185 * @par JSON-RPC响应示例
1186 * {"id":1,"jsonrpc":"2.0","result":-1}
1187 *
1188 */
1189 int resumeSpeedLine(const std::vector<double> &xd, double a, double t);
1190
1191 /**
1192 * 在关节空间做样条插值
1193 *
1194 * @param q 关节角度,如果传入参数维度为0,表示样条运动结束
1195 * @param a 加速度, 单位 rad/s^2,
1196 * 最大值可通过RobotConfig类中的接口getJointMaxAccelerations()来获取
1197 * @param v 速度, 单位 rad/s,
1198 * 最大值可通过RobotConfig类中的接口getJointMaxSpeeds()来获取
1199 * @param duration
1200 * @return 成功返回0; 失败返回错误码
1201 * AUBO_BUSY
1202 * AUBO_BAD_STATE
1203 * -AUBO_BAD_STATE
1204 *
1205 * @throws arcs::common_interface::AuboException
1206 *
1207 * @par Python函数原型
1208 * moveSpline(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1209 * float, arg2: float, arg3: float) -> int
1210 *
1211 * @par Lua函数原型
1212 * moveSpline(q: table, a: number, v: number, duration: number) -> nil
1213 *
1214 * @par JSON-RPC请求示例
1215 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveSpline","params":[[0,0,0,0,0,0],1,1,0],"id":1}
1216 *
1217 * @par JSON-RPC响应示例
1218 * {"id":1,"jsonrpc":"2.0","result":0}
1219 *
1220 */
1221 int moveSpline(const std::vector<double> &q, double a, double v,
1222 double duration);
1223
1224 /**
1225 * 添加关节运动
1226 *
1227 * @param q 关节角, 单位 rad
1228 * @param a 加速度, 单位 rad/s^2,
1229 * 最大值可通过RobotConfig类中的接口getJointMaxAccelerations()来获取
1230 * @param v 速度, 单位 rad/s,
1231 * 最大值可通过RobotConfig类中的接口getJointMaxSpeeds()来获取
1232 * @param blend_radius 交融半径, 单位 m
1233 * @param duration 运行时间,单位 s \n
1234 * 通常当给定了速度和加速度,便能够确定轨迹的运行时间。
1235 * 如果想延长轨迹的运行时间,便要设置 duration 这个参数。
1236 * duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。\n
1237 * 当 duration = 0的时候,
1238 * 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。
1239 * 如果duration不等于0,a 和 v 的值将被忽略。
1240 * @return 成功返回0;失败返回错误码,正数为警告,负数为错误。
1241 * AUBO_BAD_STATE(1): 当前安全模式处于非 Normal、ReducedMode、Recovery 状态;
1242 * AUBO_QUEUE_FULL(2): 规划队列已满;
1243 * AUBO_BUSY(3): 上一条指令正在执行中;
1244 * -AUBO_BAD_STATE(-1): 当前机器臂模式处于非 Running 状态;
1245 * -AUBO_TIMEOUT(-4): 调用接口超时;
1246 * -AUBO_INVL_ARGUMENT(-5): 参数数组q的长度小于当前机器臂的自由度;
1247 *
1248 * @throws arcs::common_interface::AuboException
1249 *
1250 * @par Python函数原型
1251 * moveJoint(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1: float,
1252 * arg2: float, arg3: float, arg4: float) -> int
1253 *
1254 * @par Lua函数原型
1255 * moveJoint(q: table, a: number, v: number, blend_radius: number, duration:
1256 * number) -> nil
1257 *
1258 * @par JSON-RPC请求示例
1259 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveJoint","params":[[-2.05177,
1260 * -0.400292, 1.19625, 0.0285152, 1.57033, -2.28774],0.3,0.3,0,0],"id":1}
1261 *
1262 * @par JSON-RPC响应示例
1263 * {"id":1,"jsonrpc":"2.0","result":0}
1264 *
1265 */
1266 int moveJoint(const std::vector<double> &q, double a, double v,
1267 double blend_radius, double duration);
1268
1269 /**
1270 * 机器人与外部轴同步运动
1271 *
1272 * @param group_name
1273 * @param q
1274 * @param a
1275 * @param v
1276 * @param blend_radius
1277 * @param duration
1278 * @return
1279 */
1280 int moveJointWithAxisGroup(const std::vector<double> &q, double a, double v,
1281 double blend_radius, double duration,
1282 const std::string &group_name,
1283 const std::vector<double> &extq);
1284
1285 /**
1286 * 通过关节运动移动到暂停点的位置
1287 *
1288 * @param q 关节角, 单位 rad
1289 * @param a 加速度, 单位 rad/s^2
1290 * @param v 速度, 单位 rad/s
1291 * @param duration 运行时间,单位 s \n
1292 * 通常当给定了速度和加速度,便能够确定轨迹的运行时间。
1293 * 如果想延长轨迹的运行时间,便要设置 duration 这个参数。
1294 * duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。\n
1295 * 当 duration = 0的时候,
1296 * 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。
1297 * 如果duration不等于0,a 和 v 的值将被忽略。
1298 * @return 成功返回0;失败返回错误码
1299 * -AUBO_INVL_ARGUMENT
1300 * -AUBO_BAD_STATE
1301 *
1302 * @throws arcs::common_interface::AuboException
1303 *
1304 * @par Python函数原型
1305 * resumeMoveJoint(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1306 * float, arg2: float, arg3: float) -> int
1307 *
1308 * @par Lua函数原型
1309 * resumeMoveJoint(q: table, a: number, v: number, duration: number) -> nil
1310 *
1311 * @par JSON-RPC请求示例
1312 * {"jsonrpc":"2.0","method":"rob1.MotionControl.resumeMoveJoint","params":[[-2.05177,
1313 * -0.400292, 1.19625, 0.0285152, 1.57033, -2.28774],0.3,0.3,0],"id":1}
1314 *
1315 * @par JSON-RPC响应示例
1316 * {"id":1,"jsonrpc":"2.0","result":-1}
1317 */
1318 int resumeMoveJoint(const std::vector<double> &q, double a, double v,
1319 double duration);
1320
1321 /**
1322 * 添加直线运动
1323 *
1324 * @param pose 目标位姿
1325 * @param a 加速度(如果位置变化小于1mm,姿态变化大于 1e-4
1326 * rad,此加速度会被作为角加速度,单位 rad/s^2.否则为线加速度,单位 m/s^2)
1327 * 最大值可通过RobotConfig类中的接口getTcpMaxAccelerations()来获取
1328 * @param v 速度(如果位置变化小于1mm,姿态变化大于 1e-4
1329 * rad,此速度会被作为角速度,单位 rad/s.否则为线速度,单位 m/s)
1330 * 最大值可通过RobotConfig类中的接口getTcpMaxSpeeds()来获取
1331 * @param blend_radius 交融半径,单位 m,值介于0.001和1之间
1332 * @param duration 运行时间,单位 s \n
1333 * 通常当给定了速度和加速度,便能够确定轨迹的运行时间。
1334 * 如果想延长轨迹的运行时间,便要设置 duration 这个参数。
1335 * duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。\n
1336 * 当 duration = 0的时候,
1337 * 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。
1338 * 如果duration不等于0,a 和 v 的值将被忽略。
1339 * @return 成功返回0;失败返回错误码,正数为警告,负数为错误。
1340 * AUBO_BAD_STATE(1): 当前安全模式处于非 Normal、ReducedMode、Recovery 状态;
1341 * AUBO_QUEUE_FULL(2): 轨迹队列已满;
1342 * AUBO_BUSY(3): 上一条指令正在执行中;
1343 * -AUBO_BAD_STATE(-1): 当前机器臂模式处于非 Running 状态;
1344 * -AUBO_TIMEOUT(-4): 调用接口超时;
1345 * -AUBO_INVL_ARGUMENT(-5): 参数数组 pose 的长度小于当前机器臂的自由度;
1346 *
1347 * @throws arcs::common_interface::AuboException
1348 *
1349 * @par Python函数原型
1350 * moveLine(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1: float,
1351 * arg2: float, arg3: float, arg4: float) -> int
1352 *
1353 * @par Lua函数原型
1354 * moveLine(pose: table, a: number, v: number, blend_radius: number,
1355 * duration: number) -> nil
1356 *
1357 * @par JSON-RPC请求示例
1358 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveLine","params":[[0.58815,0.0532,0.62391,2.46,0.479,1.619],1.2,0.25,0,0],"id":1}
1359 *
1360 * @par JSON-RPC响应示例
1361 * {"id":1,"jsonrpc":"2.0","result":0}
1362 *
1363 */
1364 int moveLine(const std::vector<double> &pose, double a, double v,
1365 double blend_radius, double duration);
1366
1367 /**
1368 * 直线运动与外部轴同步运动
1369 *
1370 * @param group_name
1371 * @param pose
1372 * @param a
1373 * @param v
1374 * @param blend_radius
1375 * @param duration
1376 * @return
1377 */
1378 int moveLineWithAxisGroup(const std::vector<double> &pose, double a,
1379 double v, double blend_radius, double duration,
1380 const std::string &group_name,
1381 const std::vector<double> &extq);
1382
1383 /**
1384 * 添加工艺运动
1385 *
1386 * @param pose
1387 * @param a
1388 * @param v
1389 * @param blend_radius
1390 * @return 成功返回0;失败返回错误码
1391 * AUBO_BAD_STATE
1392 * AUBO_BUSY
1393 * -AUBO_INVL_ARGUMENT
1394 * -AUBO_BAD_STATE
1395 *
1396 * @throws arcs::common_interface::AuboException
1397 *
1398 * @par JSON-RPC请求示例
1399 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveProcess","params":[[0.58815,0.0532,0.62391,2.46,0.479,1.619],1.2,0.25,0],"id":1}
1400 *
1401 * @par JSON-RPC响应示例
1402 * {"id":1,"jsonrpc":"2.0","result":0}
1403 *
1404 */
1405 int moveProcess(const std::vector<double> &pose, double a, double v,
1406 double blend_radius);
1407
1408 /**
1409 * 通过直线运动移动到暂停点的位置
1410 *
1411 * @param pose 目标位姿
1412 * @param a 加速度, 单位 m/s^2
1413 * @param v 速度, 单位 m/s
1414 * @param duration 运行时间,单位 s \n
1415 * 通常当给定了速度和加速度,便能够确定轨迹的运行时间。
1416 * 如果想延长轨迹的运行时间,便要设置 duration 这个参数。
1417 * duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。\n
1418 * 当 duration = 0的时候,
1419 * 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。
1420 * 如果duration不等于0,a 和 v 的值将被忽略。
1421 * @return 成功返回0;失败返回错误码
1422 * -AUBO_INVL_ARGUMENT
1423 * -AUBO_BAD_STATE
1424 *
1425 * @throws arcs::common_interface::AuboException
1426 *
1427 * @par Python函数原型
1428 * resumeMoveLine(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1429 * float, arg2: float, arg3: float) -> int
1430 *
1431 * @par Lua函数原型
1432 * resumeMoveLine(pose: table, a: number, v: number,duration: number) -> nil
1433 *
1434 * @par JSON-RPC请求示例
1435 * {"jsonrpc":"2.0","method":"rob1.MotionControl.resumeMoveLine","params":[[0.58815,0.0532,0.62391,2.46,0.479,1.619],1.2,0.25,0],"id":1}
1436 *
1437 * @par JSON-RPC响应示例
1438 * {"id":1,"jsonrpc":"2.0","result":0}
1439 *
1440 */
1441 int resumeMoveLine(const std::vector<double> &pose, double a, double v,
1442 double duration);
1443
1444 /**
1445 * 添加圆弧运动
1446 *
1447 * @todo 可以由多段圆弧组成圆周运动
1448 *
1449 * @param via_pose 圆弧运动途中点的位姿
1450 * @param end_pose 圆弧运动结束点的位姿
1451 * @param a 加速度(如果via_pose与上一个路点位置变化小于1mm,姿态变化大于 1e-4
1452 * rad, 此加速度会被作为角加速度,单位 rad/s^2.否则为线加速度,单位 m/s^2)
1453 * @param v 速度(如果via_pose与上一个路点位置变化小于1mm, 姿态变化大于 1e-4
1454 * rad, 此速度会被作为角速度, 单位 rad / s.否则为线速度, 单位 m / s)
1455 * @param blend_radius 交融半径,单位: m
1456 * @param duration 运行时间,单位: s \n
1457 * 通常当给定了速度和加速度,便能够确定轨迹的运行时间。
1458 * 如果想延长轨迹的运行时间,便要设置 duration 这个参数。
1459 * duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。\n
1460 * 当 duration = 0 的时候,
1461 * 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。
1462 * 如果duration不等于0,a 和 v 的值将被忽略。
1463 * @return 成功返回0;失败返回错误码
1464 * AUBO_BAD_STATE
1465 * AUBO_BUSY
1466 * -AUBO_INVL_ARGUMENT
1467 * -AUBO_BAD_STATE
1468 *
1469 * @throws arcs::common_interface::AuboException
1470 *
1471 * @par Python函数原型
1472 * moveCircle(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1473 * List[float], arg2: float, arg3: float, arg4: float, arg5: float) -> int
1474 *
1475 * @par Lua函数原型
1476 * moveCircle(via_pose: table, end_pose: table, a: number, v: number,
1477 * blend_radius: number, duration: number) -> nil
1478 *
1479 * @par JSON-RPC请求示例
1480 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveCircle","params":[[0.440164,-0.00249391,0.398658,2.45651,0,1.5708],[0.440164,0.166256,0.297599,2.45651,0,1.5708],1.2,0.25,0,0],"id":1}
1481 *
1482 * @par JSON-RPC响应示例
1483 * {"id":1,"jsonrpc":"2.0","result":0}
1484 *
1485 */
1486 int moveCircle(const std::vector<double> &via_pose,
1487 const std::vector<double> &end_pose, double a, double v,
1488 double blend_radius, double duration);
1489
1490 /**
1491 * moveCircle 与外部轴同步运动
1492 *
1493 * @param group_name
1494 * @param via_pose
1495 * @param end_pose
1496 * @param a
1497 * @param v
1498 * @param blend_radius
1499 * @param duration
1500 * @return
1501 */
1502 int moveCircleWithAxisGroup(const std::vector<double> &via_pose,
1503 const std::vector<double> &end_pose, double a,
1504 double v, double blend_radius, double duration,
1505 const std::string &group_name,
1506 const std::vector<double> &extq);
1507
1508 /**
1509 * 设置圆弧路径模式
1510 *
1511 * @param mode 模式 \n
1512 * 0:工具姿态相对于圆弧路径点坐标系保持不变 \n
1513 * 1:姿态线性变化,绕着空间定轴转动,从起始点姿态变化到目标点姿态 \n
1514 * 2:从起点姿态开始经过中间点姿态,变化到目标点姿态
1515 * @return 成功返回0;失败返回错误码
1516 * AUBO_BAD_STATE
1517 * AUBO_BUSY
1518 * -AUBO_INVL_ARGUMENT
1519 * -AUBO_BAD_STATE
1520 *
1521 * @throws arcs::common_interface::AuboException
1522 *
1523 * @par Python函数原型
1524 * setCirclePathMode(self: pyaubo_sdk.MotionControl, arg0: int) -> int
1525 *
1526 * @par Lua函数原型
1527 * setCirclePathMode(mode: number) -> nil
1528 *
1529 * @par JSON-RPC请求示例
1530 * {"jsonrpc":"2.0","method":"rob1.MotionControl.setCirclePathMode","params":[0],"id":1}
1531 *
1532 * @par JSON-RPC响应示例
1533 * {"id":1,"jsonrpc":"2.0","result":0}
1534 *
1535 */
1536 int setCirclePathMode(int mode);
1537
1538 /**
1539 * 高级圆弧或者圆周运动
1540 *
1541 * @param param 圆周运动参数
1542 * @return 成功返回0;失败返回错误码
1543 * AUBO_BAD_STATE
1544 * AUBO_BUSY
1545 * -AUBO_INVL_ARGUMENT
1546 * -AUBO_BAD_STATE
1547 *
1548 * @throws arcs::common_interface::AuboException
1549 *
1550 * @par Python函数原型
1551 * moveCircle2(self: pyaubo_sdk.MotionControl, arg0:
1552 * arcs::common_interface::CircleParameters) -> int
1553 *
1554 * @par Lua函数原型
1555 * moveCircle2(param: table) -> nil
1556 *
1557 * @par JSON-RPC请求示例
1558 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveCircle2","params":[{"pose_via":[0.440164,-0.00249391,0.398658,2.45651,0,1.570],
1559 * "pose_to":[0.440164,0.166256,0.297599,2.45651,0,1.5708],"a":1.2,"v":0.25,"blend_radius":0,"duration":0,"helix":0,
1560 * "spiral":0,"direction":0,"loop_times":0}],"id":1}
1561 *
1562 * @par JSON-RPC响应示例
1563 * {"id":1,"jsonrpc":"2.0","result":0}
1564 *
1565 */
1567
1568 /**
1569 * 新建一个路径点缓存
1570 *
1571 * @param name 指定路径的名字
1572 * @param type 路径的类型 \n
1573 * 1: toppra 时间最优路径规划 \n
1574 * 2: cubic_spline(录制的轨迹) \n
1575 * 3: 关节B样条插值,最少三个点
1576 * @param size 缓存区大小
1577 * @return 新建成功返回 AUBO_OK(0)
1578 *
1579 * @throws arcs::common_interface::AuboException
1580 *
1581 * @par Python函数原型
1582 * pathBufferAlloc(self: pyaubo_sdk.MotionControl, arg0: str, arg1: int,
1583 * arg2: int) -> int
1584 *
1585 * @par Lua函数原型
1586 * pathBufferAlloc(name: string, type: number, size: number) -> nil
1587 *
1588 * @par JSON-RPC请求示例
1589 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathBufferAlloc","params":["rec",2,3],"id":1}
1590 *
1591 * @par JSON-RPC响应示例
1592 * {"id":1,"jsonrpc":"2.0","result":0}
1593 *
1594 */
1595 int pathBufferAlloc(const std::string &name, int type, int size);
1596
1597 /**
1598 * 向路径缓存添加路点
1599 *
1600 * @param name 路径缓存的名字
1601 * @param waypoints 路点
1602 * @return 成功返回0;失败返回错误码
1603 * -AUBO_INVL_ARGUMENT
1604 * -AUBO_BAD_STATE
1605 *
1606 * @throws arcs::common_interface::AuboException
1607 *
1608 * @par Python函数原型
1609 * pathBufferAppend(self: pyaubo_sdk.MotionControl, arg0: str, arg1:
1610 * List[List[float]]) -> int
1611 *
1612 * @par Lua函数原型
1613 * pathBufferAppend(name: string, waypoints: table) -> nil
1614 *
1615 * @par JSON-RPC请求示例
1616 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathBufferAppend","params":["rec",[[-0.000000,0.000009,-0.000001,0.000002,0.000002,0.000000],[-0.000001,0.000010,-0.000002,0.000000,0.000003,0.000002]]],"id":1}
1617 *
1618 * @par JSON-RPC响应示例
1619 * {"id":1,"jsonrpc":"2.0","result":0}
1620 *
1621 */
1622 int pathBufferAppend(const std::string &name,
1623 const std::vector<std::vector<double>> &waypoints);
1624
1625 /**
1626 * 计算、优化等耗时操作,传入的参数相同时不会重新计算
1627 *
1628 * @param name 通过pathBufferAlloc新建的路径点缓存的名字
1629 * @param a 关节加速度限制,需要和机器人自由度大小相同,单位 m/s^2
1630 * @param v 关节速度限制,需要和机器人自由度大小相同,单位 m/s
1631 * @param t 时间 \n
1632 * pathBufferAlloc 这个接口分配内存的时候要指定类型,
1633 * 根据pathBufferAlloc这个接口的类型:\n
1634 * 类型为1时,表示运动持续时间\n
1635 * 类型为2时,表示采样时间间隔\n
1636 * 类型为3时,t参数设置为0
1637 * @return 成功返回0;失败返回错误码
1638 * -AUBO_INVL_ARGUMENT
1639 * -AUBO_BAD_STATE
1640 *
1641 * @throws arcs::common_interface::AuboException
1642 *
1643 * @par Python函数原型
1644 * pathBufferEval(self: pyaubo_sdk.MotionControl, arg0: str, arg1:
1645 * List[float], arg2: List[float], arg3: float) -> int
1646 *
1647 * @par Lua函数原型
1648 * pathBufferEval(name: string, a: table, v: table, t: number) -> nil
1649 *
1650 * @par JSON-RPC请求示例
1651 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathBufferEval","params":["rec",[1,1,1,1,1,1],[1,1,1,1,1,1],0.02],"id":1}
1652 *
1653 * @par JSON-RPC响应示例
1654 * {"id":1,"jsonrpc":"2.0","result":0}
1655 *
1656 */
1657 int pathBufferEval(const std::string &name, const std::vector<double> &a,
1658 const std::vector<double> &v, double t);
1659
1660 /**
1661 * 指定名字的buffer是否有效
1662 *
1663 * buffer需要满足三个条件有效: \n
1664 * 1、buffer存在,已经分配过内存 \n
1665 * 2、传进buffer的点要大于等于buffer的大小 \n
1666 * 3、要执行一次pathBufferEval
1667 *
1668 * @param name buffer的名字
1669 * @return 有效返回true; 无效返回false
1670 *
1671 * @throws arcs::common_interface::AuboException
1672 *
1673 * @par Python函数原型
1674 * pathBufferValid(self: pyaubo_sdk.MotionControl, arg0: str) -> bool
1675 *
1676 * @par Lua函数原型
1677 * pathBufferValid(name: string) -> boolean
1678 *
1679 * @par JSON-RPC请求示例
1680 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathBufferValid","params":["rec"],"id":1}
1681 *
1682 * @par JSON-RPC响应示例
1683 * {"id":1,"jsonrpc":"2.0","result":false}
1684 *
1685 */
1686 bool pathBufferValid(const std::string &name);
1687
1688 /**
1689 * 释放路径缓存
1690 *
1691 * @param name 缓存路径的名字
1692 * @return 成功返回0;失败返回错误码
1693 * -AUBO_INVL_ARGUMENT
1694 * -AUBO_BAD_STATE
1695 *
1696 * @throws arcs::common_interface::AuboException
1697 *
1698 * @par Python函数原型
1699 * pathBufferFree(self: pyaubo_sdk.MotionControl, arg0: str) -> int
1700 *
1701 * @par Lua函数原型
1702 * pathBufferFree(name: string) -> nil
1703 *
1704 * @par JSON-RPC请求示例
1705 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathBufferFree","params":["rec"],"id":1}
1706 *
1707 * @par JSON-RPC响应示例
1708 * {"id":1,"jsonrpc":"2.0","result":-5}
1709 *
1710 */
1711 int pathBufferFree(const std::string &name);
1712
1713 /**
1714 * 关节空间路径滤波器
1715 *
1716 * @brief pathBufferFilter
1717 *
1718 * @param name 缓存路径的名称
1719 * @param order 滤波器阶数(一般取2)
1720 * @param fd 截止频率,越小越光滑,但是延迟会大(一般取3-20)
1721 * @param fs 离散数据的采样频率(一般取20-500)
1722 *
1723 * @return 成功返回0
1724 *
1725 * @throws arcs::common_interface::AuboException
1726 *
1727 * @par Python函数原型
1728 * pathBufferFree(self: pyaubo_sdk.MotionControl, arg0: str, arg1: int,
1729 * arg2: float, arg3: float) -> int
1730 *
1731 * @par Lua函数原型
1732 * pathBufferFree(name: string, order: number, fd: number, fs:number) -> nil
1733 */
1734 int pathBufferFilter(const std::string &name, int order, double fd,
1735 double fs);
1736
1737 /**
1738 * 列出所有缓存路径的名字
1739 *
1740 * @return 返回所有缓存路径的名字
1741 *
1742 * @throws arcs::common_interface::AuboException
1743 *
1744 * @par Python函数原型
1745 * pathBufferList(self: pyaubo_sdk.MotionControl) -> List[str]
1746 *
1747 * @par Lua函数原型
1748 * pathBufferList() -> table
1749 *
1750 * @par JSON-RPC请求示例
1751 * {"jsonrpc":"2.0","method":"rob1.MotionControl.pathBufferList","params":[],"id":1}
1752 *
1753 * @par JSON-RPC响应示例
1754 * {"id":1,"jsonrpc":"2.0","result":[]}
1755 *
1756 */
1757 std::vector<std::string> pathBufferList();
1758
1759 /**
1760 * 执行缓存的路径
1761 *
1762 * @param name 缓存路径的名字
1763 * @return 成功返回0;失败返回错误码
1764 * -AUBO_INVL_ARGUMENT
1765 * -AUBO_BAD_STATE
1766 *
1767 * @throws arcs::common_interface::AuboException
1768 *
1769 * @par Python函数原型
1770 * movePathBuffer(self: pyaubo_sdk.MotionControl, arg0: str) -> int
1771 *
1772 * @par Lua函数原型
1773 * movePathBuffer(name: string) -> nil
1774 *
1775 * @par JSON-RPC请求示例
1776 * {"jsonrpc":"2.0","method":"rob1.MotionControl.movePathBuffer","params":["rec"],"id":1}
1777 *
1778 * @par JSON-RPC响应示例
1779 * {"id":1,"jsonrpc":"2.0","result":0}
1780 *
1781 */
1782 int movePathBuffer(const std::string &name);
1783
1784 /**
1785 * 相贯线接口
1786 *
1787 * @param pose由三个示教位姿组成(首先需要运动到起始点,起始点的要求:过主圆柱
1788 * 柱体轴心且与子圆柱体轴线平行的平面与子圆柱体在底部的交点)
1789 * p1:过子圆柱体轴线且与大圆柱体轴线平行的平面,与小圆柱体在左侧顶部的交点
1790 * p2:过子圆柱体轴线且与大圆柱体轴线平行的平面,与大圆柱体在左侧底部的交点
1791 * p3:过子圆柱体轴线且与大圆柱体轴线平行的平面,与大圆柱体在右侧底部的交点
1792 * @param v:速度
1793 * @param a:加速度
1794 * @param main_pipe_radius: 主圆柱体半径
1795 * @param sub_pipe_radius: 子圆柱体半径
1796 * @param normal_distance: 两圆柱体轴线距离
1797 * @param normal_alpha: 两圆柱体轴线的夹角
1798 *
1799 * @return 成功返回0;失败返回错误码
1800 * AUBO_BUSY
1801 * AUBO_BAD_STATE
1802 * -AUBO_INVL_ARGUMENT
1803 * -AUBO_BAD_STATE
1804 *
1805 * @throws arcs::common_interface::AuboException
1806 *
1807 * @par Python函数原型
1808 * moveIntersection(self: pyaubo_sdk.MotionControl, arg0: List[float], arg1:
1809 float, arg2: float, arg3: float, arg4: float, arg5: float, arg6: float) ->
1810 int
1811 *
1812 * @par Lua函数原型
1813 * moveIntersection(poses: table, a: number, v: number,
1814 * main_pipe_radius:number, sub_pipe_radius: number,normal_distance:
1815 * number,normal_alpha: number,) -> nil
1816 */
1817 int moveIntersection(const std::vector<std::vector<double>> &poses,
1818 double a, double v, double main_pipe_radius,
1819 double sub_pipe_radius, double normal_distance,
1820 double normal_alpha);
1821 /**
1822 * 关节空间停止运动
1823 *
1824 * @param acc 关节加速度,单位: rad/s^2
1825 * @return 成功返回0;失败返回错误码
1826 * AUBO_BUSY
1827 * AUBO_BAD_STATE
1828 * -AUBO_INVL_ARGUMENT
1829 * -AUBO_BAD_STATE
1830 *
1831 * @throws arcs::common_interface::AuboException
1832 *
1833 * @par Python函数原型
1834 * stopJoint(self: pyaubo_sdk.MotionControl, arg0: float) -> int
1835 *
1836 * @par Lua函数原型
1837 * stopJoint(acc: number) -> nil
1838 *
1839 * @par JSON-RPC请求示例
1840 * {"jsonrpc":"2.0","method":"rob1.MotionControl.stopJoint","params":[31],"id":1}
1841 *
1842 * @par JSON-RPC响应示例
1843 * {"id":1,"jsonrpc":"2.0","result":0}
1844 *
1845 */
1846 int stopJoint(double acc);
1847
1848 /**
1849 * 关节空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedJoint接口移动到安全位置后需要停止时调用此接口)
1850 *
1851 * @param acc 关节加速度,单位: rad/s^2
1852 * @return 成功返回0;失败返回错误码
1853 * AUBO_BUSY
1854 * AUBO_BAD_STATE
1855 * -AUBO_INVL_ARGUMENT
1856 * -AUBO_BAD_STATE
1857 *
1858 * @throws arcs::common_interface::AuboException
1859 *
1860 * @par Python函数原型
1861 * resumeStopJoint(self: pyaubo_sdk.MotionControl, arg0: float) -> int
1862 *
1863 * @par Lua函数原型
1864 * resumeStopJoint(acc: number) -> nil
1865 *
1866 * @par JSON-RPC请求示例
1867 * {"jsonrpc":"2.0","method":"rob1.MotionControl.resumeStopJoint","params":[31],"id":1}
1868 *
1869 * @par JSON-RPC响应示例
1870 * {"id":1,"jsonrpc":"2.0","result":-1}
1871 *
1872 */
1873 int resumeStopJoint(double acc);
1874
1875 /**
1876 * 笛卡尔空间停止运动
1877 *
1878 * @param acc 工具加速度, 单位: m/s^2
1879 * @param acc_rot
1880 * @return 成功返回0;失败返回错误码
1881 * AUBO_BUSY
1882 * AUBO_BAD_STATE
1883 * -AUBO_INVL_ARGUMENT
1884 * -AUBO_BAD_STATE
1885 *
1886 * @throws arcs::common_interface::AuboException
1887 *
1888 * @par Python函数原型
1889 * stopLine(self: pyaubo_sdk.MotionControl, arg0: float, arg1: float) -> int
1890 *
1891 * @par Lua函数原型
1892 * stopLine(acc: number, acc_rot: number) -> nil
1893 *
1894 * @par JSON-RPC请求示例
1895 * {"jsonrpc":"2.0","method":"rob1.MotionControl.stopLine","params":[10,10],"id":1}
1896 *
1897 * @par JSON-RPC响应示例
1898 * {"id":1,"jsonrpc":"2.0","result":0}
1899 *
1900 */
1901 int stopLine(double acc, double acc_rot);
1902
1903 /**
1904 * 笛卡尔空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedLine接口移动到安全位置后需要停止时调用此接口)
1905 *
1906 * @param acc 位置加速度, 单位: m/s^2
1907 * @param acc_rot 姿态加速度,单位: rad/s^2
1908 * @return 成功返回0;失败返回错误码
1909 * AUBO_BUSY
1910 * AUBO_BAD_STATE
1911 * -AUBO_INVL_ARGUMENT
1912 * -AUBO_BAD_STATE
1913 *
1914 * @throws arcs::common_interface::AuboException
1915 *
1916 * @par Python函数原型
1917 * resumeStopLine(self: pyaubo_sdk.MotionControl, arg0: float, arg1: float)
1918 * -> int
1919 *
1920 * @par Lua函数原型
1921 * resumeStopLine(acc: number, acc_rot: number) -> nil
1922 *
1923 * @par JSON-RPC请求示例
1924 * {"jsonrpc":"2.0","method":"rob1.MotionControl.resumeStopLine","params":[10,10],"id":1}
1925 *
1926 * @par JSON-RPC响应示例
1927 * {"id":1,"jsonrpc":"2.0","result":-1}
1928 *
1929 */
1930 int resumeStopLine(double acc, double acc_rot);
1931
1932 /**
1933 * 开始摆动: weaveStart 和 weaveEnd 的 moveLine/moveCircle 将根据 params
1934 * 进行摆动
1935 *
1936 * @param params Json字符串用于定义摆动参数
1937 * {
1938 * "type": <string>, // "SINE" "SPIRAL" "TRIANGLE" "TRAPEZOIDAL"
1939 * "step": <num>,
1940 * "amplitude": {<num>,<num>},
1941 * "hold_distance": {<num>,<num>},
1942 * "angle": <num>
1943 * "direction": <num>
1944 * }
1945 * @return 成功返回0;失败返回错误码
1946 * AUBO_BUSY
1947 * AUBO_BAD_STATE
1948 * -AUBO_INVL_ARGUMENT
1949 * -AUBO_BAD_STATE
1950 *
1951 * @throws arcs::common_interface::AuboException
1952 *
1953 */
1954 int weaveStart(const std::string &params);
1955
1956 /**
1957 * 结束摆动
1958 *
1959 * @return 成功返回0;失败返回错误码
1960 * AUBO_BUSY
1961 * AUBO_BAD_STATE
1962 * -AUBO_BAD_STATE
1963 *
1964 * @throws arcs::common_interface::AuboException
1965 *
1966 * @par JSON-RPC请求示例
1967 * {"jsonrpc":"2.0","method":"rob1.MotionControl.weaveEnd","params":[],"id":1}
1968 *
1969 * @par JSON-RPC响应示例
1970 * {"id":1,"jsonrpc":"2.0","result":0}
1971 *
1972 */
1974
1975 /**
1976 * 设置未来路径上点的采样时间间隔
1977 *
1978 * @param sample_time 采样时间间隔 单位: m/s^2
1979 * @return 成功返回0;失败返回错误码
1980 * AUBO_BUSY
1981 * AUBO_BAD_STATE
1982 * -AUBO_INVL_ARGUMENT
1983 * -AUBO_BAD_STATE
1984 *
1985 * @throws arcs::common_interface::AuboException
1986 *
1987 */
1988 int setFuturePointSamplePeriod(double sample_time);
1989
1990 /**
1991 * 获取未来路径上的轨迹点
1992 *
1993 * @return 路点(100ms * 10)
1994 *
1995 * @throws arcs::common_interface::AuboException
1996 *
1997 * @par JSON-RPC请求示例
1998 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getFuturePathPointsJoint","params":[],"id":1}
1999 *
2000 * @par JSON-RPC响应示例
2001 * {"id":1,"jsonrpc":"2.0","result":[]}
2002 *
2003 */
2004 std::vector<std::vector<double>> getFuturePathPointsJoint();
2005
2006 /**
2007 * 圆形传送带跟随
2008 *
2009 * @note 暂未实现
2010 *
2011 * @param encoder_id
2012 * 0-集成传感器
2013 *
2014 * @param center
2015 * @param tick_per_revo
2016 * @param rotate_tool
2017 * @return
2018 *
2019 * @throws arcs::common_interface::AuboException
2020 *
2021 */
2022 int conveyorTrackCircle(int encoder_id, const std::vector<double> &center,
2023 int tick_per_revo, bool rotate_tool);
2024
2025 /**
2026 * 线性传送带跟随
2027 *
2028 * @note 暂未实现
2029 *
2030 * @param encoder_id 预留
2031 * @param direction
2032 * @param tick_per_meter
2033 * @return
2034 *
2035 * @throws arcs::common_interface::AuboException
2036 *
2037 */
2038 int conveyorTrackLine(int encoder_id, const std::vector<double> &direction,
2039 int tick_per_meter);
2040
2041 /**
2042 * 终止传送带跟随
2043 *
2044 * @note 暂未实现
2045 *
2046 * @param a
2047 * @return
2048 *
2049 * @throws arcs::common_interface::AuboException
2050 *
2051 */
2052 int conveyorTrackStop(double a);
2053
2054 /**
2055 * 螺旋线运动
2056 *
2057 * @param param 封装的参数
2058 * @param blend_radius
2059 * @param v
2060 * @param a
2061 * @param t
2062 * @return 成功返回0;失败返回错误码
2063 * AUBO_BUSY
2064 * AUBO_BAD_STATE
2065 * -AUBO_INVL_ARGUMENT
2066 * -AUBO_BAD_STATE
2067 *
2068 * @throws arcs::common_interface::AuboException
2069 *
2070 * @par JSON-RPC请求示例
2071 * {"jsonrpc":"2.0","method":"rob1.MotionControl.moveSpiral",
2072 * "params":[{"spiral":0.005,"helix":0.005,"angle":18.84,"plane":0,"frame":[0,0,0,0,0,0]},0,0.25,1.2,0],"id":1}
2073 *
2074 * @par JSON-RPC响应示例
2075 * {"id":1,"jsonrpc":"2.0","result":0}
2076 *
2077 */
2078 int moveSpiral(const SpiralParameters &param, double blend_radius, double v,
2079 double a, double t);
2080
2081 /**
2082 * 获取前瞻段数
2083 *
2084 * @return 返回前瞻段数
2085 *
2086 * @throws arcs::common_interface::AuboException
2087 *
2088 * @par Python函数原型
2089 * getLookAheadSize(self: pyaubo_sdk.MotionControl) -> int
2090 *
2091 * @par Lua函数原型
2092 * getLookAheadSize() -> number
2093 *
2094 * @par JSON-RPC请求示例
2095 * {"jsonrpc":"2.0","method":"rob1.MotionControl.getLookAheadSize","params":[],"id":1}
2096 *
2097 * @par JSON-RPC响应示例
2098 * {"id":1,"jsonrpc":"2.0","result":1}
2099 *
2100 */
2102
2103 /**
2104 * 设置前瞻段数
2105 * 1.对于有较高速度平稳性要求的任务,如数控加工,涂胶,焊接等匀速需求,较长的前瞻段数可以提供更优的速度规划,产生的运动会更加平滑;
2106 * 2.对于快速响应的抓取类任务,更倾向于较短的前瞻段数,以提高反应速度,但可能因为进给的路径不够及时导致速度波动很大.
2107 *
2108 * @return 成功返回0
2109 *
2110 * @throws arcs::common_interface::AuboException
2111 *
2112 * @par Python函数原型
2113 * setLookAheadSize(self: pyaubo_sdk.MotionControl, arg0: int) -> int
2114 *
2115 * @par Lua函数原型
2116 * setLookAheadSize(eqradius: number) -> number
2117 *
2118 * @par JSON-RPC请求示例
2119 * {"jsonrpc":"2.0","method":"rob1.MotionControl.setLookAheadSize","params":[1],"id":1}
2120 *
2121 * @par JSON-RPC响应示例
2122 * {"id":1,"jsonrpc":"2.0","result":0}
2123 *
2124 */
2125 int setLookAheadSize(int size);
2126
2127protected:
2128 void *d_;
2129};
2130using MotionControlPtr = std::shared_ptr<MotionControl>;
2131} // namespace common_interface
2132} // namespace arcs
2133
2134#endif // AUBO_SDK_MOTION_CONTROL_INTERFACE_H
int resumeMoveJoint(const std::vector< double > &q, double a, double v, double duration)
通过关节运动移动到暂停点的位置
int pathOffsetLimits(double v, double a)
设置偏移的最大速度和最大加速度 仅对pathOffsetSet中 type=1 有效
int followJoint(const std::vector< double > &q)
关节空间跟随
int setLookAheadSize(int size)
设置前瞻段数 1.
int jointOffsetDisable()
关节偏移失能
int servoJointWithAxisGroup(const std::vector< double > &q, double a, double v, double t, double lookahead_time, double gain, const std::string &group_name, const std::vector< double > &extq)
int followLine(const std::vector< double > &pose)
笛卡尔空间跟随
int stopJoint(double acc)
关节空间停止运动
int speedJoint(const std::vector< double > &qd, double a, double t)
关节空间速度跟随
int moveCircle2(const CircleParameters &param)
高级圆弧或者圆周运动
int getServoModeSelect()
获取伺服运动模式
int pathOffsetSupv(const std::vector< double > &min, const std::vector< double > &max, int strategy)
监控轨迹偏移范围
int getTrajectoryQueueSize()
获取已经入队的运动规划插补点数量
int clearPath()
ClearPath (Clear Path) clears the whole motion path on the current motion path level (base level or S...
std::vector< std::vector< double > > getFuturePathPointsJoint()
获取未来路径上的轨迹点
int conveyorTrackStop(double a)
终止传送带跟随
int jointOffsetSet(const std::vector< double > &offset, int type=1)
设置关节偏移
int resumeSpeedLine(const std::vector< double > &xd, double a, double t)
笛卡尔空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置)
double getMotionLeftTime(int id)
获取指定ID的运动指令段的剩余执行时间
int pathBufferAlloc(const std::string &name, int type, int size)
新建一个路径点缓存
int servoCartesian(const std::vector< double > &pose, double a, double v, double t, double lookahead_time, double gain)
笛卡尔空间伺服
int moveLineWithAxisGroup(const std::vector< double > &pose, double a, double v, double blend_radius, double duration, const std::string &group_name, const std::vector< double > &extq)
直线运动与外部轴同步运动
double getSpeedFraction()
获取速度和加速度比例,默认为 1 可以通过 setSpeedFraction 接口设置
std::vector< double > getPauseJointPositions()
getPauseJointPositions
int startMove()
StartMove is used to resume robot, external axes movement and belonging process after the movement ha...
int storePath(bool keep_sync)
storePath
int moveJoint(const std::vector< double > &q, double a, double v, double blend_radius, double duration)
添加关节运动
bool isBlending()
是否处交融区
int moveSpline(const std::vector< double > &q, double a, double v, double duration)
在关节空间做样条插值
int resumeMoveLine(const std::vector< double > &pose, double a, double v, double duration)
通过直线运动移动到暂停点的位置
int stopLine(double acc, double acc_rot)
笛卡尔空间停止运动
int resumeStopLine(double acc, double acc_rot)
笛卡尔空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedLine接口移动到安全位置后需要停止时调用此接口)
std::tuple< std::string, std::vector< double > > getWorkObjectHold()
getWorkObjectHold
int servoCartesianWithAxisGroup(const std::vector< double > &pose, double a, double v, double t, double lookahead_time, double gain, const std::string &group_name, const std::vector< double > &extq)
int moveCircle(const std::vector< double > &via_pose, const std::vector< double > &end_pose, double a, double v, double blend_radius, double duration)
添加圆弧运动
int speedFractionCritical(bool enable)
速度比例设置临界区,使能之后速度比例被强制设定为1.
std::vector< std::string > pathBufferList()
列出所有缓存路径的名字
int stopMove(bool quick, bool all_tasks)
StopMove is used to stop robot and external axes movements and any belonging process temporarily.
int jointOffsetEnable()
关节偏移使能
ARCS_DEPRECATED bool isServoModeEnabled()
判断伺服模式是否使能 使用 getServoModeSelect 替代
int pathOffsetDisable()
路径偏移失能
int conveyorTrackCircle(int encoder_id, const std::vector< double > &center, int tick_per_revo, bool rotate_tool)
圆形传送带跟随
int conveyorTrackLine(int encoder_id, const std::vector< double > &direction, int tick_per_meter)
线性传送带跟随
int getQueueSize()
获取已经入队的指令段(INST)数量,运动指令包括 moveJoint/moveLine/moveCircle 等运动指令以及 setPayload 等配置指令
int movePathBuffer(const std::string &name)
执行缓存的路径
int moveIntersection(const std::vector< std::vector< double > > &poses, double a, double v, double main_pipe_radius, double sub_pipe_radius, double normal_distance, double normal_alpha)
相贯线接口
int moveCircleWithAxisGroup(const std::vector< double > &via_pose, const std::vector< double > &end_pose, double a, double v, double blend_radius, double duration, const std::string &group_name, const std::vector< double > &extq)
moveCircle 与外部轴同步运动
int pathBufferAppend(const std::string &name, const std::vector< std::vector< double > > &waypoints)
向路径缓存添加路点
int getLookAheadSize()
获取前瞻段数
int moveLine(const std::vector< double > &pose, double a, double v, double blend_radius, double duration)
添加直线运动
int resumeSpeedJoint(const std::vector< double > &qd, double a, double t)
关节空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置)
int moveProcess(const std::vector< double > &pose, double a, double v, double blend_radius)
添加工艺运动
int pathBufferFilter(const std::string &name, int order, double fd, double fs)
关节空间路径滤波器
bool pathBufferValid(const std::string &name)
指定名字的buffer是否有效
ARCS_DEPRECATED int setServoMode(bool enable)
设置伺服模式 使用 setServoModeSelect 替代
int setCirclePathMode(int mode)
设置圆弧路径模式
int setServoModeSelect(int mode)
设置伺服运动模式
int speedLine(const std::vector< double > &xd, double a, double t)
笛卡尔空间速度跟随
double getDuration(int id)
获取指定ID的运动指令段的预期执行时间
int trackCartesian(const std::vector< double > &pose, double t, double smooth_scale, double delay_sacle)
跟踪运动,用于执行离线轨迹、透传用户规划轨迹等 与 trackJoint 区别在于接收笛卡尔空间位姿而不是关节角度 (由软件内部直接做逆解)
int pathOffsetSet(const std::vector< double > &offset, int type=0)
设置路径偏移
int servoJointWithAxes(const std::vector< double > &q, const std::vector< double > &extq, double a, double v, double t, double lookahead_time, double gain)
伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等
double getProgress()
获取当前运动指令段的执行进度
int pathOffsetCoordinate(int ref_coord)
设置偏移的参考坐标系 仅对pathOffsetSet中 type=1 有效
int moveJointWithAxisGroup(const std::vector< double > &q, double a, double v, double blend_radius, double duration, const std::string &group_name, const std::vector< double > &extq)
机器人与外部轴同步运动
int pathOffsetEnable()
路径偏移使能
int moveSpiral(const SpiralParameters &param, double blend_radius, double v, double a, double t)
螺旋线运动
int weaveStart(const std::string &params)
开始摆动: weaveStart 和 weaveEnd 的 moveLine/moveCircle 将根据 params 进行摆动
int pathBufferFree(const std::string &name)
释放路径缓存
int setFuturePointSamplePeriod(double sample_time)
设置未来路径上点的采样时间间隔
double getEqradius()
获取等效半径,单位 m moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动 可以通过 setEqradius 设置,默认为1
int setWorkObjectHold(const std::string &module_name, const std::vector< double > &mounting_pose)
当工件安装在另外一台机器人的末端或者外部轴上时,指定其名字和安装位置
int setEqradius(double eqradius)
设置等效半径,单位 m moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动,数值越大,姿态旋转速度越快
int getExecId()
获取当前正在插补的运动指令段的ID
int setSpeedFraction(double fraction)
动态调整机器人运行速度和加速度比例 (0., 1.
int resumeStopJoint(double acc)
关节空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedJoint接口移动到安全位置后需要停止时调用此接口)
int trackJoint(const std::vector< double > &q, double t, double smooth_scale, double delay_sacle)
跟踪运动,用于执行离线轨迹、透传用户规划轨迹等
bool isSpeedFractionCritical()
是否处于速度比例设置临界区
int servoJoint(const std::vector< double > &q, double a, double v, double t, double lookahead_time, double gain)
关节空间伺服
int pathBufferEval(const std::string &name, const std::vector< double > &a, const std::vector< double > &v, double t)
计算、优化等耗时操作,传入的参数相同时不会重新计算
int servoCartesianWithAxes(const std::vector< double > &pose, const std::vector< double > &extq, double a, double v, double t, double lookahead_time, double gain)
伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等 与 servoJointWithAxes 区别在于接收笛卡尔空间位姿而不是关节角度 (由软件内部直接做逆解)
std::shared_ptr< MotionControl > MotionControlPtr
PathBufferType
pathBuffer类型
@ PathBuffer_JointSpline
3: 关节B样条插值,最少三个点 废弃,建议用5替代,现在实际是关节空间 CUBIC_SPLINE
@ PathBuffer_CubicSpline
2: cubic_spline(录制的轨迹)
@ PathBuffer_TOPPRA
1: toppra 时间最优路径规划
@ PathBuffer_JointSplineC
4:关节B样条插值,最少三个点,但是传入的是笛卡尔空间位姿 废弃,建议用6替代,现在实际是关节空间 CUBIC_SPLINE
@ PathBuffer_JointBSplineC
6:关节B样条插值,最少三个点,但是传入的是笛卡尔空间位姿
数据类型的定义