![]() |
AUBO SDK
0.25.0
|
#include <motion_control.h>
Public 成员函数 | |
MotionControl () | |
virtual | ~MotionControl () |
double | getEqradius () |
获取等效半径,单位 m moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动 可以通过 setEqradius 设置,默认为1 | |
int | setEqradius (double eqradius) |
设置等效半径,单位 m moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动,数值越大,姿态旋转速度越快 | |
int | setSpeedFraction (double fraction) |
动态调整机器人运行速度和加速度比例 (0., 1. | |
double | getSpeedFraction () |
获取速度和加速度比例,默认为 1 可以通过 setSpeedFraction 接口设置 | |
int | speedFractionCritical (bool enable) |
速度比例设置临界区,使能之后速度比例被强制设定为1. | |
bool | isSpeedFractionCritical () |
是否处于速度比例设置临界区 | |
bool | isBlending () |
是否处交融区 | |
int | pathOffsetLimits (double v, double a) |
设置偏移的最大速度和最大加速度 仅对pathOffsetSet中 type=1 有效 | |
int | pathOffsetCoordinate (int ref_coord) |
设置偏移的参考坐标系 仅对pathOffsetSet中 type=1 有效 | |
int | pathOffsetEnable () |
路径偏移使能 | |
int | pathOffsetSet (const std::vector< double > &offset, int type=0) |
设置路径偏移 | |
int | pathOffsetDisable () |
路径偏移失能 | |
int | pathOffsetSupv (const std::vector< double > &min, const std::vector< double > &max, int strategy) |
int | jointOffsetEnable () |
关节偏移使能 | |
int | jointOffsetSet (const std::vector< double > &offset, int type=1) |
设置关节偏移 | |
int | jointOffsetDisable () |
关节偏移失能 | |
int | getQueueSize () |
获取已经入队的指令段(INST)数量,运动指令包括 moveJoint/moveLine/moveCircle 等运动指令以及 setPayload 等配置指令 | |
int | getTrajectoryQueueSize () |
获取已经入队的运动规划插补点数量 | |
int | getExecId () |
获取当前正在插补的运动指令段的ID | |
double | getDuration (int id) |
获取指定ID的运动指令段的预期执行时间 | |
double | getMotionLeftTime (int id) |
获取指定ID的运动指令段的剩余执行时间 | |
int | stopMove (bool quick, bool all_tasks) |
StopMove 用于临时停止机器人和外部轴的运动以及相关工艺进程。如果调用 StartMove 指令,则运动和工艺进程将恢复。 | |
int | startMove () |
StartMove 用于在以下情况下恢复机器人、外部轴的运动以及相关工艺进程: • 通过 StopMove 指令停止后。 • 执行 StorePath ... RestoPath 序列后。 • 发生异步运动错误(如 ERR_PATH_STOP)或特定工艺错误并在 ERROR 处理器中处理后。 | |
int | storePath (bool keep_sync) |
storePath | |
int | clearPath () |
ClearPath (清除路径) 清除当前运动路径层级(基础层级或 StorePath 层级)上的所有运动路径。 | |
int | restoPath () |
restoPath | |
double | getProgress () |
获取当前运动指令段的执行进度 | |
int | setWorkObjectHold (const std::string &module_name, const std::vector< double > &mounting_pose) |
当工件安装在另外一台机器人的末端或者外部轴上时,指定其名字和安装位置 | |
std::tuple< std::string, std::vector< double > > | getWorkObjectHold () |
获取工件安装信息 | |
std::vector< double > | getPauseJointPositions () |
获取暂停点关节位置 | |
int | setResumeStartPoint (const std::vector< double > &q, int move_type, double blend_radius, const std::vector< double > &qdmax, const std::vector< double > &qddmax, const std::vector< double > &vmax, const std::vector< double > &amax) |
设置继续运动参数 | |
int | getResumeMode () |
获取继续运动模式 | |
ARCS_DEPRECATED int | setServoMode (bool enable) |
设置伺服模式 使用 setServoModeSelect 替代 | |
ARCS_DEPRECATED bool | isServoModeEnabled () |
判断伺服模式是否使能 使用 getServoModeSelect 替代 | |
int | setServoModeSelect (int mode) |
设置伺服运动模式 | |
int | getServoModeSelect () |
获取伺服运动模式 | |
int | servoJoint (const std::vector< double > &q, double a, double v, double t, double lookahead_time, double gain) |
关节空间伺服 | |
int | servoCartesian (const std::vector< double > &pose, double a, double v, double t, double lookahead_time, double gain) |
笛卡尔空间伺服 | |
int | servoJointWithAxes (const std::vector< double > &q, const std::vector< double > &extq, double a, double v, double t, double lookahead_time, double gain) |
伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等 | |
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 | servoCartesianWithAxes (const std::vector< double > &pose, const std::vector< double > &extq, double a, double v, double t, double lookahead_time, double gain) |
伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等 与 servoJointWithAxes 区别在于接收笛卡尔空间位姿而不是关节角度 (由软件内部直接做逆解) | |
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 | trackJoint (const std::vector< double > &q, double t, double smooth_scale, double delay_sacle) |
跟踪运动,用于执行离线轨迹、透传用户规划轨迹等 | |
int | trackCartesian (const std::vector< double > &pose, double t, double smooth_scale, double delay_sacle) |
跟踪运动,用于执行离线轨迹、透传用户规划轨迹等 与 trackJoint 区别在于接收笛卡尔空间位姿而不是关节角度 (由软件内部直接做逆解) | |
int | followJoint (const std::vector< double > &q) |
关节空间跟随 | |
int | followLine (const std::vector< double > &pose) |
笛卡尔空间跟随 | |
int | speedJoint (const std::vector< double > &qd, double a, double t) |
关节空间速度跟随 | |
int | resumeSpeedJoint (const std::vector< double > &qd, double a, double t) |
关节空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置) | |
int | speedLine (const std::vector< double > &xd, double a, double t) |
笛卡尔空间速度跟随 | |
int | resumeSpeedLine (const std::vector< double > &xd, double a, double t) |
笛卡尔空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置) | |
int | moveSpline (const std::vector< double > &q, double a, double v, double duration) |
在关节空间做样条插值 | |
int | moveJoint (const std::vector< double > &q, double a, double v, double blend_radius, double duration) |
添加关节运动 | |
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 | resumeMoveJoint (const std::vector< double > &q, double a, double v, double duration) |
通过关节运动移动到暂停点的位置 | |
int | moveLine (const std::vector< double > &pose, double a, double v, double blend_radius, double duration) |
添加直线运动 | |
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) |
直线运动与外部轴同步运动 | |
int | moveProcess (const std::vector< double > &pose, double a, double v, double blend_radius) |
添加工艺运动 | |
int | resumeMoveLine (const std::vector< double > &pose, double a, double v, double duration) |
通过直线运动移动到暂停点的位置 | |
int | moveCircle (const std::vector< double > &via_pose, const std::vector< double > &end_pose, double a, double v, double blend_radius, double duration) |
添加圆弧运动 | |
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 | setCirclePathMode (int mode) |
设置圆弧路径模式 | |
int | moveCircle2 (const CircleParameters ¶m) |
高级圆弧或者圆周运动 | |
int | pathBufferAlloc (const std::string &name, int type, int size) |
新建一个路径点缓存 | |
int | pathBufferAppend (const std::string &name, const std::vector< std::vector< double > > &waypoints) |
向路径缓存添加路点 | |
int | pathBufferEval (const std::string &name, const std::vector< double > &a, const std::vector< double > &v, double t) |
计算、优化等耗时操作,传入的参数相同时不会重新计算 | |
bool | pathBufferValid (const std::string &name) |
指定名字的buffer是否有效 | |
int | pathBufferFree (const std::string &name) |
释放路径缓存 | |
int | pathBufferFilter (const std::string &name, int order, double fd, double fs) |
关节空间路径滤波器 | |
std::vector< std::string > | pathBufferList () |
列出所有缓存路径的名字 | |
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 | stopJoint (double acc) |
关节空间停止运动 | |
int | resumeStopJoint (double acc) |
关节空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedJoint接口移动到安全位置后需要停止时调用此接口) | |
int | stopLine (double acc, double acc_rot) |
笛卡尔空间停止运动 | |
int | resumeStopLine (double acc, double acc_rot) |
笛卡尔空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedLine接口移动到安全位置后需要停止时调用此接口) | |
int | weaveStart (const std::string ¶ms) |
开始摆动: weaveStart 和 weaveEnd 的 moveLine/moveCircle 将根据 params 进行摆动 | |
int | weaveEnd () |
结束摆动 | |
int | setFuturePointSamplePeriod (double sample_time) |
设置未来路径上点的采样时间间隔 | |
std::vector< std::vector< double > > | getFuturePathPointsJoint () |
获取未来路径上的轨迹点 | |
int | setConveyorTrackEncoder (int encoder_id, int tick_per_meter) |
设置传送带编码器参数 | |
int | conveyorTrackCircle (int encoder_id, const std::vector< double > ¢er, bool rotate_tool) |
圆形传送带跟随 | |
int | conveyorTrackLine (int encoder_id, const std::vector< double > &direction) |
线性传送带跟随 | |
int | conveyorTrackStop (int encoder_id, double a) |
终止传送带跟随 | |
bool | conveyorTrackSwitch (int encoder_id) |
切换传送带追踪物品 如果当前物品正在处于跟踪状态,则将该物品出队,不再跟踪,返回true 如果没有物品正在处于跟踪状态,返回false | |
bool | hasItemOnConveyorToTrack (int encoder_id) |
传送带上是否有物品可以跟踪 | |
int | conveyorTrackCreatItem (int encoder_id, int item_id, const std::vector< double > &offset) |
增加传送带队列 | |
int | setConveyorTrackCompensate (int encoder_id, double comp) |
设置传送带跟踪的补偿值 | |
bool | isConveyorTrackSync (int encoder_id) |
判断传送带与机械臂之间是否达到相对静止 | |
int | setConveyorTrackLimit (int encoder_id, double limit) |
设置传送带跟踪的最大距离限制 | |
int | setConveyorTrackStartWindow (int encoder_id, double window_min, double window_max) |
设置传送带跟踪的启动窗口 | |
int | setConveyorTrackSensorOffset (int encoder_id, double offset) |
设置传送带示教位置到同步开关之间的距离 | |
int | setConveyorTrackSyncSeparation (int encoder_id, double distance, double time) |
设置传送带同步分离,用于过滤掉同步开关中不需要的信号 | |
bool | isConveyorTrackExceed (int encoder_id) |
传送带上工件的移动距离是否超过最大限值 | |
int | conveyorTrackClearItems (int encoder_id) |
清空传动带队列中的所有对象 | |
std::vector< int > | getConveyorTrackQueue (int encoder_id) |
获取传送带队列的编码器值 | |
int | getConveyorTrackNextItem (int encoder_id) |
获取下一个跟踪的传送带物品的id | |
int | moveSpiral (const SpiralParameters ¶m, double blend_radius, double v, double a, double t) |
螺旋线运动 | |
int | getLookAheadSize () |
获取前瞻段数 | |
int | setLookAheadSize (int size) |
设置前瞻段数 1. | |
int | weaveUpdateParameters (const std::string ¶ms) |
更新摆动过程中的频率和振幅 | |
int | enableJointSoftServo (const std::vector< double > &stiffness) |
设置关节电流环刚度系数 | |
int | disableJointSoftServo () |
关闭关节电流环刚度系数 | |
bool | isJointSoftServoEnabled () |
判断关节电流环刚度系数是否使能 | |
Protected 属性 | |
void * | d_ |
在文件 motion_control.h 第 56 行定义.
arcs::common_interface::MotionControl::MotionControl | ( | ) |
|
virtual |
int arcs::common_interface::MotionControl::clearPath | ( | ) |
ClearPath (清除路径) 清除当前运动路径层级(基础层级或 StorePath 层级)上的所有运动路径。
int arcs::common_interface::MotionControl::conveyorTrackCircle | ( | int | encoder_id, |
const std::vector< double > & | center, | ||
bool | rotate_tool | ||
) |
int arcs::common_interface::MotionControl::conveyorTrackClearItems | ( | int | encoder_id | ) |
清空传动带队列中的所有对象
encoder_id | 预留 |
int arcs::common_interface::MotionControl::conveyorTrackCreatItem | ( | int | encoder_id, |
int | item_id, | ||
const std::vector< double > & | offset | ||
) |
增加传送带队列
encoder_id | 预留 |
offset | 当前物品点位相对于模板物品点位的偏移值 |
int arcs::common_interface::MotionControl::conveyorTrackLine | ( | int | encoder_id, |
const std::vector< double > & | direction | ||
) |
线性传送带跟随
encoder_id | 预留 |
direction | 传送带相对机器人基坐标系的移动方向 |
int arcs::common_interface::MotionControl::conveyorTrackStop | ( | int | encoder_id, |
double | a | ||
) |
终止传送带跟随
encoder_id | 预留 |
a | 预留 |
bool arcs::common_interface::MotionControl::conveyorTrackSwitch | ( | int | encoder_id | ) |
切换传送带追踪物品 如果当前物品正在处于跟踪状态,则将该物品出队,不再跟踪,返回true 如果没有物品正在处于跟踪状态,返回false
encoder_id | 预留 |
int arcs::common_interface::MotionControl::disableJointSoftServo | ( | ) |
关闭关节电流环刚度系数
int arcs::common_interface::MotionControl::enableJointSoftServo | ( | const std::vector< double > & | stiffness | ) |
设置关节电流环刚度系数
stiffness | 各个关节刚度系数,百分比。[0 -> 1],值越大,表现为越硬。 |
int arcs::common_interface::MotionControl::followJoint | ( | const std::vector< double > & | q | ) |
关节空间跟随
int arcs::common_interface::MotionControl::followLine | ( | const std::vector< double > & | pose | ) |
笛卡尔空间跟随
int arcs::common_interface::MotionControl::getConveyorTrackNextItem | ( | int | encoder_id | ) |
获取下一个跟踪的传送带物品的id
encoder_id | 预留 |
std::vector< int > arcs::common_interface::MotionControl::getConveyorTrackQueue | ( | int | encoder_id | ) |
获取传送带队列的编码器值
encoder_id | 预留 |
double arcs::common_interface::MotionControl::getDuration | ( | int | id | ) |
获取指定ID的运动指令段的预期执行时间
id | 运动指令段ID |
double arcs::common_interface::MotionControl::getEqradius | ( | ) |
获取等效半径,单位 m moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动 可以通过 setEqradius 设置,默认为1
int arcs::common_interface::MotionControl::getExecId | ( | ) |
获取当前正在插补的运动指令段的ID
-1 | 表示轨迹队列为空 像movePathBuffer运动中的buffer或者规划器(moveJoint和moveLine等)里的队列都属于轨迹队列 |
std::vector< std::vector< double > > arcs::common_interface::MotionControl::getFuturePathPointsJoint | ( | ) |
获取未来路径上的轨迹点
int arcs::common_interface::MotionControl::getLookAheadSize | ( | ) |
获取前瞻段数
double arcs::common_interface::MotionControl::getMotionLeftTime | ( | int | id | ) |
获取指定ID的运动指令段的剩余执行时间
id | 运动指令段ID |
std::vector< double > arcs::common_interface::MotionControl::getPauseJointPositions | ( | ) |
获取暂停点关节位置
常用于运行工程时发生碰撞后继续运动过程中(先通过resumeMoveJoint或resumeMoveLine运动到暂停位置,再恢复工程)
double arcs::common_interface::MotionControl::getProgress | ( | ) |
获取当前运动指令段的执行进度
int arcs::common_interface::MotionControl::getQueueSize | ( | ) |
获取已经入队的指令段(INST)数量,运动指令包括 moveJoint/moveLine/moveCircle 等运动指令以及 setPayload 等配置指令
指令一般会在接口宏定义里面用 _INST 指示, 比如 moveJoint _INST(MotionControl, 5, moveJoint, q, a, v, blend_radius, duration)
int arcs::common_interface::MotionControl::getResumeMode | ( | ) |
获取继续运动模式
int arcs::common_interface::MotionControl::getServoModeSelect | ( | ) |
获取伺服运动模式
double arcs::common_interface::MotionControl::getSpeedFraction | ( | ) |
获取速度和加速度比例,默认为 1 可以通过 setSpeedFraction 接口设置
int arcs::common_interface::MotionControl::getTrajectoryQueueSize | ( | ) |
获取已经入队的运动规划插补点数量
std::tuple< std::string, std::vector< double > > arcs::common_interface::MotionControl::getWorkObjectHold | ( | ) |
获取工件安装信息
bool arcs::common_interface::MotionControl::hasItemOnConveyorToTrack | ( | int | encoder_id | ) |
传送带上是否有物品可以跟踪
encoder_id | 预留 |
bool arcs::common_interface::MotionControl::isBlending | ( | ) |
是否处交融区
bool arcs::common_interface::MotionControl::isConveyorTrackExceed | ( | int | encoder_id | ) |
传送带上工件的移动距离是否超过最大限值
encoder_id | 预留 |
bool arcs::common_interface::MotionControl::isConveyorTrackSync | ( | int | encoder_id | ) |
判断传送带与机械臂之间是否达到相对静止
encoder_id | 预留 |
bool arcs::common_interface::MotionControl::isJointSoftServoEnabled | ( | ) |
判断关节电流环刚度系数是否使能
ARCS_DEPRECATED bool arcs::common_interface::MotionControl::isServoModeEnabled | ( | ) |
判断伺服模式是否使能 使用 getServoModeSelect 替代
bool arcs::common_interface::MotionControl::isSpeedFractionCritical | ( | ) |
是否处于速度比例设置临界区
int arcs::common_interface::MotionControl::jointOffsetDisable | ( | ) |
关节偏移失能
int arcs::common_interface::MotionControl::jointOffsetEnable | ( | ) |
关节偏移使能
int arcs::common_interface::MotionControl::jointOffsetSet | ( | const std::vector< double > & | offset, |
int | type = 1 |
||
) |
设置关节偏移
offset | 在各关节的位姿偏移 |
type |
int arcs::common_interface::MotionControl::moveCircle | ( | const std::vector< double > & | via_pose, |
const std::vector< double > & | end_pose, | ||
double | a, | ||
double | v, | ||
double | blend_radius, | ||
double | duration | ||
) |
添加圆弧运动
via_pose | 圆弧运动途中点的位姿 |
end_pose | 圆弧运动结束点的位姿 |
a | 加速度(如果via_pose与上一个路点位置变化小于1mm,姿态变化大于 1e-4 rad, 此加速度会被作为角加速度,单位 rad/s^2.否则为线加速度,单位 m/s^2) |
v | 速度(如果via_pose与上一个路点位置变化小于1mm, 姿态变化大于 1e-4 rad, 此速度会被作为角速度, 单位 rad / s.否则为线速度, 单位 m / s) |
blend_radius | 交融半径,单位: m |
duration | 运行时间,单位: s 通常当给定了速度和加速度,便能够确定轨迹的运行时间。 如果想延长轨迹的运行时间,便要设置 duration 这个参数。 duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。 当 duration = 0 的时候, 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。 如果duration不等于0,a 和 v 的值将被忽略。 |
int arcs::common_interface::MotionControl::moveCircle2 | ( | const CircleParameters & | param | ) |
高级圆弧或者圆周运动
param | 圆周运动参数 |
int arcs::common_interface::MotionControl::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 与外部轴同步运动
group_name | 外部轴组名称 |
via_pose | 圆弧运动途中点的位姿 |
end_pose | 圆弧运动结束点的位姿 |
a | 加速度 |
v | 速度 |
blend_radius | 交融半径 |
duration | 运行时间 |
int arcs::common_interface::MotionControl::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 | ||
) |
相贯线接口
pose | 由三个示教位姿组成(首先需要运动到起始点,起始点的要求:过主圆柱 柱体轴心且与子圆柱体轴线平行的平面与子圆柱体在底部的交点) p1:过子圆柱体轴线且与大圆柱体轴线平行的平面,与小圆柱体在左侧顶部的交点 p2:过子圆柱体轴线且与大圆柱体轴线平行的平面,与大圆柱体在左侧底部的交点 p3:过子圆柱体轴线且与大圆柱体轴线平行的平面,与大圆柱体在右侧底部的交点 |
v | 速度 |
a | 加速度 |
main_pipe_radius | 主圆柱体半径 |
sub_pipe_radius | 子圆柱体半径 |
normal_distance | 两圆柱体轴线距离 |
normal_alpha | 两圆柱体轴线的夹角 |
int arcs::common_interface::MotionControl::moveJoint | ( | const std::vector< double > & | q, |
double | a, | ||
double | v, | ||
double | blend_radius, | ||
double | duration | ||
) |
添加关节运动
q | 关节角, 单位 rad |
a | 加速度, 单位 rad/s^2, 最大值可通过RobotConfig类中的接口getJointMaxAccelerations()来获取 |
v | 速度, 单位 rad/s, 最大值可通过RobotConfig类中的接口getJointMaxSpeeds()来获取 |
blend_radius | 交融半径, 单位 m |
duration | 运行时间,单位 s 通常当给定了速度和加速度,便能够确定轨迹的运行时间。 如果想延长轨迹的运行时间,便要设置 duration 这个参数。 duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。 当 duration = 0的时候, 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。 如果duration不等于0,a 和 v 的值将被忽略。 |
0 | 成功 |
AUBO_BAD_STATE(1) | 当前安全模式处于非 Normal、ReducedMode、Recovery 状态 |
AUBO_QUEUE_FULL(2) | 规划队列已满 |
AUBO_BUSY(3) | 上一条指令正在执行中 |
-AUBO_BAD_STATE(-1) | 可能的原因包括但不限于:线程已分离、线程被终止、task_id 未找到,或者当前机器人模式非 Running |
-AUBO_TIMEOUT(-4) | 调用接口超时 |
-AUBO_INVL_ARGUMENT(-5) | 参数数组q的长度小于当前机器臂的自由度 |
int arcs::common_interface::MotionControl::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 | ||
) |
机器人与外部轴同步运动
group_name | |
q | |
a | |
v | |
blend_radius | |
duration |
int arcs::common_interface::MotionControl::moveLine | ( | const std::vector< double > & | pose, |
double | a, | ||
double | v, | ||
double | blend_radius, | ||
double | duration | ||
) |
添加直线运动
pose | 目标位姿 |
a | 加速度(如果位置变化小于1mm,姿态变化大于 1e-4 rad,此加速度会被作为角加速度,单位 rad/s^2.否则为线加速度,单位 m/s^2) 最大值可通过RobotConfig类中的接口getTcpMaxAccelerations()来获取 |
v | 速度(如果位置变化小于1mm,姿态变化大于 1e-4 rad,此速度会被作为角速度,单位 rad/s.否则为线速度,单位 m/s) 最大值可通过RobotConfig类中的接口getTcpMaxSpeeds()来获取 |
blend_radius | 交融半径,单位 m,值介于0.001和1之间 |
duration | 运行时间,单位 s 通常当给定了速度和加速度,便能够确定轨迹的运行时间。 如果想延长轨迹的运行时间,便要设置 duration 这个参数。 duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。 当 duration = 0的时候, 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。 如果duration不等于0,a 和 v 的值将被忽略。 |
0 | 成功 |
AUBO_BAD_STATE(1) | 当前安全模式处于非 Normal、ReducedMode、Recovery 状态 |
AUBO_QUEUE_FULL(2) | 轨迹队列已满 |
AUBO_BUSY(3) | 上一条指令正在执行中 |
-AUBO_BAD_STATE(-1) | 可能的原因包括但不限于:线程已分离、线程被终止、task_id 未找到,或者当前机器人模式非 Running |
-AUBO_TIMEOUT(-4) | 调用接口超时 |
-AUBO_INVL_ARGUMENT(-5) | 参数数组 pose 的长度小于当前机器臂的自由度 |
int arcs::common_interface::MotionControl::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 | ||
) |
直线运动与外部轴同步运动
group_name | 外部轴组名称 |
pose | 目标位姿 |
a | 加速度 |
v | 速度 |
blend_radius | 交融半径 |
duration | 运行时间 |
int arcs::common_interface::MotionControl::movePathBuffer | ( | const std::string & | name | ) |
执行缓存的路径
name | 缓存路径的名字 |
int arcs::common_interface::MotionControl::moveProcess | ( | const std::vector< double > & | pose, |
double | a, | ||
double | v, | ||
double | blend_radius | ||
) |
添加工艺运动
pose | |
a | |
v | |
blend_radius |
int arcs::common_interface::MotionControl::moveSpiral | ( | const SpiralParameters & | param, |
double | blend_radius, | ||
double | v, | ||
double | a, | ||
double | t | ||
) |
螺旋线运动
param | 封装的参数 |
blend_radius | |
v | |
a | |
t |
int arcs::common_interface::MotionControl::moveSpline | ( | const std::vector< double > & | q, |
double | a, | ||
double | v, | ||
double | duration | ||
) |
在关节空间做样条插值
q | 关节角度,如果传入参数维度为0,表示样条运动结束 |
a | 加速度, 单位 rad/s^2, 最大值可通过RobotConfig类中的接口getJointMaxAccelerations()来获取 |
v | 速度, 单位 rad/s, 最大值可通过RobotConfig类中的接口getJointMaxSpeeds()来获取 |
duration |
int arcs::common_interface::MotionControl::pathBufferAlloc | ( | const std::string & | name, |
int | type, | ||
int | size | ||
) |
新建一个路径点缓存
name | 指定路径的名字 |
type | 路径的类型 1: toppra 时间最优路径规划 2: cubic_spline(录制的轨迹) 3: 关节B样条插值,最少三个点 |
size | 缓存区大小 |
int arcs::common_interface::MotionControl::pathBufferAppend | ( | const std::string & | name, |
const std::vector< std::vector< double > > & | waypoints | ||
) |
向路径缓存添加路点
name | 路径缓存的名字 |
waypoints | 路点 |
int arcs::common_interface::MotionControl::pathBufferEval | ( | const std::string & | name, |
const std::vector< double > & | a, | ||
const std::vector< double > & | v, | ||
double | t | ||
) |
计算、优化等耗时操作,传入的参数相同时不会重新计算
name | 通过pathBufferAlloc新建的路径点缓存的名字 |
a | 关节加速度限制,需要和机器人自由度大小相同,单位 rad/s^2 |
v | 关节速度限制,需要和机器人自由度大小相同,单位 rad/s |
t | 时间 pathBufferAlloc 这个接口分配内存的时候要指定类型, 根据pathBufferAlloc这个接口的类型: 类型为1时,表示运动持续时间 类型为2时,表示采样时间间隔 类型为3时:t表示运动持续时间 若 t=0, 则由使用软件内部默认的时间(推荐使用) 若 t!=0, t需要设置为 路径点数*相邻点时间间隔(points * interval,interval >= 0.01) |
int arcs::common_interface::MotionControl::pathBufferFilter | ( | const std::string & | name, |
int | order, | ||
double | fd, | ||
double | fs | ||
) |
关节空间路径滤波器
pathBufferFilter
name | 缓存路径的名称 |
order 滤波器阶数(一般取2) | |
fd 截止频率,越小越光滑,但是延迟会大(一般取3-20) | |
fs 离散数据的采样频率(一般取20-500) |
int arcs::common_interface::MotionControl::pathBufferFree | ( | const std::string & | name | ) |
释放路径缓存
name | 缓存路径的名字 |
std::vector< std::string > arcs::common_interface::MotionControl::pathBufferList | ( | ) |
列出所有缓存路径的名字
bool arcs::common_interface::MotionControl::pathBufferValid | ( | const std::string & | name | ) |
指定名字的buffer是否有效
buffer需要满足三个条件有效:
1、buffer存在,已经分配过内存
2、传进buffer的点要大于等于buffer的大小
3、要执行一次pathBufferEval
name | buffer的名字 |
int arcs::common_interface::MotionControl::pathOffsetCoordinate | ( | int | ref_coord | ) |
设置偏移的参考坐标系 仅对pathOffsetSet中 type=1 有效
ref_coord | 参考坐标系 0-基坐标系 1-TCP |
int arcs::common_interface::MotionControl::pathOffsetDisable | ( | ) |
路径偏移失能
int arcs::common_interface::MotionControl::pathOffsetEnable | ( | ) |
路径偏移使能
int arcs::common_interface::MotionControl::pathOffsetLimits | ( | double | v, |
double | a | ||
) |
设置偏移的最大速度和最大加速度 仅对pathOffsetSet中 type=1 有效
v | 最大速度 |
a | 最大加速度 |
int arcs::common_interface::MotionControl::pathOffsetSet | ( | const std::vector< double > & | offset, |
int | type = 0 |
||
) |
设置路径偏移
offset | 在各方向的位姿偏移 |
type | 运动类型 0-位置规划 1-速度规划 |
int arcs::common_interface::MotionControl::pathOffsetSupv | ( | const std::vector< double > & | min, |
const std::vector< double > & | max, | ||
int | strategy | ||
) |
监控轨迹偏移范围
min | 沿坐标轴负方向最大偏移量 |
max | 沿坐标轴正方向最大偏移量 |
strategy | 达到最大偏移量后监控策略 0-禁用监控 1-饱和限制,即维持最大姿态 2-保护停止 |
int arcs::common_interface::MotionControl::restoPath | ( | ) |
restoPath
int arcs::common_interface::MotionControl::resumeMoveJoint | ( | const std::vector< double > & | q, |
double | a, | ||
double | v, | ||
double | duration | ||
) |
通过关节运动移动到暂停点的位置
q | 关节角, 单位 rad |
a | 加速度, 单位 rad/s^2 |
v | 速度, 单位 rad/s |
duration | 运行时间,单位 s 通常当给定了速度和加速度,便能够确定轨迹的运行时间。 如果想延长轨迹的运行时间,便要设置 duration 这个参数。 duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。 当 duration = 0的时候, 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。 如果duration不等于0,a 和 v 的值将被忽略。 |
int arcs::common_interface::MotionControl::resumeMoveLine | ( | const std::vector< double > & | pose, |
double | a, | ||
double | v, | ||
double | duration | ||
) |
通过直线运动移动到暂停点的位置
pose | 目标位姿 |
a | 加速度, 单位 m/s^2 |
v | 速度, 单位 m/s |
duration | 运行时间,单位 s 通常当给定了速度和加速度,便能够确定轨迹的运行时间。 如果想延长轨迹的运行时间,便要设置 duration 这个参数。 duration 可以延长轨迹运动的时间,但是不能缩短轨迹时间。 当 duration = 0的时候, 表示不指定运行时间,即没有明确完成运动的时间,将根据速度与加速度计算运行时间。 如果duration不等于0,a 和 v 的值将被忽略。 |
int arcs::common_interface::MotionControl::resumeSpeedJoint | ( | const std::vector< double > & | qd, |
double | a, | ||
double | t | ||
) |
关节空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置)
当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
qd | 目标关节速度, 单位 rad/s |
a | 主轴的加速度, 单位 rad/s^2 |
t | 函数返回所需要的时间, 单位 s 如果 t = 0,当达到目标速度的时候,函数将返回; 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。 如果没有达到目标速度,会减速到零。 如果达到了目标速度就是按照目标速度匀速运动。 |
int arcs::common_interface::MotionControl::resumeSpeedLine | ( | const std::vector< double > & | xd, |
double | a, | ||
double | t | ||
) |
笛卡尔空间速度跟随(机械臂运行工程时发生碰撞,通过此接口移动到安全位置)
当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
xd | 工具速度, 单位 m/s |
a | 工具位置加速度, 单位 m/s^2 |
t | 函数返回所需要的时间, 单位 s 如果 t = 0,当达到目标速度的时候,函数将返回; 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。 如果没有达到目标速度,会减速到零。 如果达到了目标速度就是按照目标速度匀速运动。 |
int arcs::common_interface::MotionControl::resumeStopJoint | ( | double | acc | ) |
关节空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedJoint接口移动到安全位置后需要停止时调用此接口)
acc | 关节加速度,单位: rad/s^2 |
int arcs::common_interface::MotionControl::resumeStopLine | ( | double | acc, |
double | acc_rot | ||
) |
笛卡尔空间停止运动(机械臂运行工程时发生碰撞,通过resumeSpeedLine接口移动到安全位置后需要停止时调用此接口)
acc | 位置加速度, 单位: m/s^2 |
acc_rot | 姿态加速度,单位: rad/s^2 |
int arcs::common_interface::MotionControl::servoCartesian | ( | const std::vector< double > & | pose, |
double | a, | ||
double | v, | ||
double | t, | ||
double | lookahead_time, | ||
double | gain | ||
) |
笛卡尔空间伺服
目前可用参数只有 pose 和 t;
pose | 位姿, 单位 m, |
a | 加速度, 单位 m/s^2, |
v | 速度,单位 m/s, |
t | 运行时间,单位 s t 值越大,机器臂运动越慢,反之,运动越快; 该参数最优值为连续调用 servoCartesian 接口的间隔时间。 |
lookahead_time | 前瞻时间,单位 s 指定机器臂开始减速前要运动的时长,用前瞻时间来平滑轨迹[0.03, 0.2], 当 lookahead_time 小于一个控制周期时,越小则超调量越大, 该参数最优值为一个控制周期。 |
gain | 比例增益 跟踪目标位置的比例增益[100, 200], 用于控制运动的顺滑性和精度, 比例增益越大,到达目标位置的时间越长,超调量越小。 |
0 | 成功 |
AUBO_BAD_STATE(1) | 当前安全模式处于非 Normal、ReducedMode、Recovery 状态 |
AUBO_QUEUE_FULL(2) | 规划队列已满 |
AUBO_BUSY(3) | 上一条指令正在执行中 |
-AUBO_BAD_STATE(-1) | 可能的原因包括但不限于:线程已分离、线程被终止、task_id 未找到,或者当前机器人模式非 Running |
-AUBO_TIMEOUT(-4) | 调用接口超时 |
-AUBO_INVL_ARGUMENT(-5) | 轨迹位置超限或速度超限 |
-AUBO_REQUEST_IGNORE(-13) | 当前处于非 servo 模式 |
-AUBO_IK_NO_CONVERGE(-23) | 逆解计算不收敛,计算出错; |
-AUBO_IK_OUT_OF_RANGE(-24) | 逆解计算超出机器人最大限制; |
AUBO_IK_CONFIG_DISMATCH(-25) | 逆解输入配置存在错误; |
-AUBO_IK_JACOBIAN_FAILED(-26) | 逆解雅可比矩阵计算失败; |
-AUBO_IK_NO_SOLU(-27) | 目标点存在解析解,但均不满足选解条件; |
-AUBO_IK_UNKOWN_ERROR(-28) | 逆解返回未知类型错误; |
int arcs::common_interface::MotionControl::servoCartesianWithAxes | ( | const std::vector< double > & | pose, |
const std::vector< double > & | extq, | ||
double | a, | ||
double | v, | ||
double | t, | ||
double | lookahead_time, | ||
double | gain | ||
) |
伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等 与 servoJointWithAxes 区别在于接收笛卡尔空间位姿而不是关节角度 (由软件内部直接做逆解)
pose | |
extq | |
t | |
smooth_scale | |
delay_sacle |
int arcs::common_interface::MotionControl::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 arcs::common_interface::MotionControl::servoJoint | ( | const std::vector< double > & | q, |
double | a, | ||
double | v, | ||
double | t, | ||
double | lookahead_time, | ||
double | gain | ||
) |
关节空间伺服
目前可用参数只有 q 和 t;
q | 关节角, 单位 rad, |
a | 加速度, 单位 rad/s^2, |
v | 速度,单位 rad/s, |
t | 运行时间,单位 s t 值越大,机器臂运动越慢,反之,运动越快; 该参数最优值为连续调用 servoJoint 接口的间隔时间。 |
lookahead_time | 前瞻时间,单位 s 指定机器臂开始减速前要运动的时长,用前瞻时间来平滑轨迹[0.03, 0.2], 当 lookahead_time 小于一个控制周期时,越小则超调量越大, 该参数最优值为一个控制周期。 |
gain | 比例增益 跟踪目标位置的比例增益[100, 200], 用于控制运动的顺滑性和精度, 比例增益越大,到达目标位置的时间越长,超调量越小。 |
0 | 成功 |
AUBO_BAD_STATE(1) | 当前安全模式处于非 Normal、ReducedMode、Recovery 状态 |
AUBO_QUEUE_FULL(2) | 规划队列已满 |
AUBO_BUSY(3) | 上一条指令正在执行中 |
-AUBO_BAD_STATE(-1) | 可能的原因包括但不限于:线程已分离、线程被终止、task_id 未找到,或者当前机器人模式非 Running |
-AUBO_TIMEOUT(-4) | 调用接口超时 |
-AUBO_INVL_ARGUMENT(-5) | 轨迹位置超限或速度超限 |
-AUBO_REQUEST_IGNORE(-13) | 当前处于非 servo 模式 |
int arcs::common_interface::MotionControl::servoJointWithAxes | ( | const std::vector< double > & | q, |
const std::vector< double > & | extq, | ||
double | a, | ||
double | v, | ||
double | t, | ||
double | lookahead_time, | ||
double | gain | ||
) |
伺服运动(带外部轴),用于执行离线轨迹、透传用户规划轨迹等
q | |
extq | |
t | |
smooth_scale | |
delay_sacle |
int arcs::common_interface::MotionControl::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 arcs::common_interface::MotionControl::setCirclePathMode | ( | int | mode | ) |
设置圆弧路径模式
mode | 模式 0:工具姿态相对于圆弧路径点坐标系保持不变 1:姿态线性变化,绕着空间定轴转动,从起始点姿态变化到目标点姿态 2:从起点姿态开始经过中间点姿态,变化到目标点姿态 |
int arcs::common_interface::MotionControl::setConveyorTrackCompensate | ( | int | encoder_id, |
double | comp | ||
) |
设置传送带跟踪的补偿值
encoder_id | 预留 |
comp | 传送带补偿值 |
int arcs::common_interface::MotionControl::setConveyorTrackEncoder | ( | int | encoder_id, |
int | tick_per_meter | ||
) |
设置传送带编码器参数
encoder_id | 预留 |
tick_per_meter | 线性传送带===>一米的脉冲值 环形传送带===>一圈的脉冲值 |
int arcs::common_interface::MotionControl::setConveyorTrackLimit | ( | int | encoder_id, |
double | limit | ||
) |
设置传送带跟踪的最大距离限制
encoder_id | 预留 |
limit | 传送带跟踪的最大距离限制,单位:米 |
int arcs::common_interface::MotionControl::setConveyorTrackSensorOffset | ( | int | encoder_id, |
double | offset | ||
) |
设置传送带示教位置到同步开关之间的距离
encoder_id | 预留 |
offset | 传送带示教位置到同步开关之间的距离,单位:米 |
int arcs::common_interface::MotionControl::setConveyorTrackStartWindow | ( | int | encoder_id, |
double | window_min, | ||
double | window_max | ||
) |
设置传送带跟踪的启动窗口
encoder_id | 预留 |
min_window | 启动窗口的起始位置,单位:米 |
max_window | 启动窗口的结束位置,单位:米 |
int arcs::common_interface::MotionControl::setConveyorTrackSyncSeparation | ( | int | encoder_id, |
double | distance, | ||
double | time | ||
) |
设置传送带同步分离,用于过滤掉同步开关中不需要的信号
encoder_id | 预留 |
distance | 从出现一个同步信号后到把一个新的同步信号接受为一个有效对象前走的最短距离,单位:米 |
time | 从出现一个同步信号后到把一个新的同步信号接受为一个有效对象前走的最短时间,单位:秒 |
distance和time设置数值大于0即为生效 当distance与time同时设置时,优先生效distance
int arcs::common_interface::MotionControl::setEqradius | ( | double | eqradius | ) |
设置等效半径,单位 m moveLine/moveCircle时,末端姿态旋转的角度等效到末端位置移动,数值越大,姿态旋转速度越快
eqradius | 0表示只规划移动,姿态旋转跟随移动 |
int arcs::common_interface::MotionControl::setFuturePointSamplePeriod | ( | double | sample_time | ) |
设置未来路径上点的采样时间间隔
sample_time | 采样时间间隔 单位: m/s^2 |
int arcs::common_interface::MotionControl::setLookAheadSize | ( | int | size | ) |
设置前瞻段数 1.
对于有较高速度平稳性要求的任务,如数控加工,涂胶,焊接等匀速需求,较长的前瞻段数可以提供更优的速度规划,产生的运动会更加平滑; 2.对于快速响应的抓取类任务,更倾向于较短的前瞻段数,以提高反应速度,但可能因为进给的路径不够及时导致速度波动很大.
int arcs::common_interface::MotionControl::setResumeStartPoint | ( | const std::vector< double > & | q, |
int | move_type, | ||
double | blend_radius, | ||
const std::vector< double > & | qdmax, | ||
const std::vector< double > & | qddmax, | ||
const std::vector< double > & | vmax, | ||
const std::vector< double > & | amax | ||
) |
设置继续运动参数
q | 继续运动起始位置 |
move_type | 0:关节空间 1:笛卡尔空间 |
blend_radius | 交融半径 |
qdmax | 关节运动最大速度(6维度数据) |
qddmax | 关节运动最大加速度(6维度数据) |
vmax | 直线运动最大线速度,角速度(2维度数据) |
amax | 直线运动最大线加速度,角加速度(2维度数据) |
ARCS_DEPRECATED int arcs::common_interface::MotionControl::setServoMode | ( | bool | enable | ) |
设置伺服模式 使用 setServoModeSelect 替代
enable | 是否使能 |
int arcs::common_interface::MotionControl::setServoModeSelect | ( | int | mode | ) |
设置伺服运动模式
mode | 0-退出伺服模式 1-规划伺服模式 2-透传模式(直接下发) 3-透传模式(缓存) |
int arcs::common_interface::MotionControl::setSpeedFraction | ( | double | fraction | ) |
动态调整机器人运行速度和加速度比例 (0., 1.
]
fraction | 机器人运行速度和加速度比例 |
int arcs::common_interface::MotionControl::setWorkObjectHold | ( | const std::string & | module_name, |
const std::vector< double > & | mounting_pose | ||
) |
当工件安装在另外一台机器人的末端或者外部轴上时,指定其名字和安装位置
module_name | 控制模块名字 |
mounting_pose | 抓取的相对位置, 如果是机器人,则相对于机器人末端中心点(非TCP点) |
int arcs::common_interface::MotionControl::speedFractionCritical | ( | bool | enable | ) |
速度比例设置临界区,使能之后速度比例被强制设定为1.
, 失能之后恢复之前的速度比例
enable |
int arcs::common_interface::MotionControl::speedJoint | ( | const std::vector< double > & | qd, |
double | a, | ||
double | t | ||
) |
关节空间速度跟随
当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
qd | 目标关节速度, 单位 rad/s |
a | 主轴的加速度, 单位 rad/s^2 |
t | 函数返回所需要的时间, 单位 s 如果 t = 0,当达到目标速度的时候,函数将返回; 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。 如果没有达到目标速度,会减速到零。 如果达到了目标速度就是按照目标速度匀速运动。 |
0 | 成功 |
AUBO_BAD_STATE(1) | 当前安全模式处于非 Normal、ReducedMode、Recovery 状态 |
AUBO_BUSY(3) | 上一条指令正在执行中 |
-AUBO_BAD_STATE(-1) | 可能的原因包括但不限于:线程已分离、线程被终止、task_id 未找到,或者当前机器人模式非 Running |
-AUBO_TIMEOUT(-4) | 调用接口超时 |
-AUBO_INVL_ARGUMENT(-5) | 参数数组qd的长度小于当前机器臂的自由度 |
int arcs::common_interface::MotionControl::speedLine | ( | const std::vector< double > & | xd, |
double | a, | ||
double | t | ||
) |
笛卡尔空间速度跟随
当机械臂还没达到目标速度的时候,给一个新的目标速度,机械臂会立刻达到新的目标速度
xd | 工具速度, 单位 m/s |
a | 工具位置加速度, 单位 m/s^2 |
t | 函数返回所需要的时间, 单位 s 如果 t = 0,当达到目标速度的时候,函数将返回; 反之,则经过 t 时间后,函数返回,不管是否达到目标速度。 如果没有达到目标速度,会减速到零。 如果达到了目标速度就是按照目标速度匀速运动。 |
0 | 成功 |
AUBO_BAD_STATE(1) | 当前安全模式处于非 Normal、ReducedMode、Recovery 状态 |
AUBO_BUSY(3) | 上一条指令正在执行中 |
-AUBO_BAD_STATE(-1) | 可能的原因包括但不限于:线程已分离、线程被终止、task_id 未找到,或者当前机器人模式非 Running |
-AUBO_TIMEOUT(-4) | 调用接口超时 |
int arcs::common_interface::MotionControl::startMove | ( | ) |
StartMove 用于在以下情况下恢复机器人、外部轴的运动以及相关工艺进程: • 通过 StopMove 指令停止后。 • 执行 StorePath ... RestoPath 序列后。 • 发生异步运动错误(如 ERR_PATH_STOP)或特定工艺错误并在 ERROR 处理器中处理后。
int arcs::common_interface::MotionControl::stopJoint | ( | double | acc | ) |
关节空间停止运动
acc | 关节加速度,单位: rad/s^2 |
int arcs::common_interface::MotionControl::stopLine | ( | double | acc, |
double | acc_rot | ||
) |
笛卡尔空间停止运动
acc | 工具加速度, 单位: m/s^2 |
acc_rot |
int arcs::common_interface::MotionControl::stopMove | ( | bool | quick, |
bool | all_tasks | ||
) |
StopMove 用于临时停止机器人和外部轴的运动以及相关工艺进程。如果调用 StartMove 指令,则运动和工艺进程将恢复。
该指令可用于中断处理程序中,在发生中断时临时停止机器人。
quick | true: 以最快速度在路径上停止机器人。未指定 quick 参数时,机器人将在路径上停止,但制动距离较长(与普通程序停止相同)。 |
int arcs::common_interface::MotionControl::storePath | ( | bool | keep_sync | ) |
storePath
keep_sync |
int arcs::common_interface::MotionControl::trackCartesian | ( | const std::vector< double > & | pose, |
double | t, | ||
double | smooth_scale, | ||
double | delay_sacle | ||
) |
跟踪运动,用于执行离线轨迹、透传用户规划轨迹等 与 trackJoint 区别在于接收笛卡尔空间位姿而不是关节角度 (由软件内部直接做逆解)
pose | |
t | |
smooth_scale | |
delay_sacle |
int arcs::common_interface::MotionControl::trackJoint | ( | const std::vector< double > & | q, |
double | t, | ||
double | smooth_scale, | ||
double | delay_sacle | ||
) |
跟踪运动,用于执行离线轨迹、透传用户规划轨迹等
q | |
smooth_scale | |
delay_sacle |
int arcs::common_interface::MotionControl::weaveEnd | ( | ) |
结束摆动
int arcs::common_interface::MotionControl::weaveStart | ( | const std::string & | params | ) |
开始摆动: weaveStart 和 weaveEnd 的 moveLine/moveCircle 将根据 params 进行摆动
params | Json字符串用于定义摆动参数 { "type": <string>, // "SINE" "SPIRAL" "TRIANGLE" "TRAPEZOIDAL" "step": <num>, "amplitude": {<num>,<num>}, "hold_distance": {<num>,<num>}, "angle": <num> "direction": <num> } |
int arcs::common_interface::MotionControl::weaveUpdateParameters | ( | const std::string & | params | ) |
更新摆动过程中的频率和振幅
params | Json字符串用于定义摆动参数 { "frequency": <num>, "amplitude": {<num>,<num>} } |
|
protected |
在文件 motion_control.h 第 5084 行定义.