力控功能使用流程
一. 功能说明
- 通过给机械臂末端传感器施加一定的外力来改变机械臂的运动方向
二、负载及传感器偏置设置：
负载辨识
【1】根据机械臂型号安装合适的负载
【2】在示教器中点击【配置】>【负载】>【测量】>【力传感器辨识】
【3】根据界面提示的步骤操作并设置最终结果。
负载辨识sdk接口流程：
【1】设置辨识的三个参考点，通过moveJoint接口移动到目标点，并记录在目标点时刻下的传感器数据及位置。
// 负载辨识参考点
std::vector<double> joint1 = { -0.261799, 0.261799, 1.309,
1.0472, 1.39626, 0.0 };
std::vector<double> joint2 = { -0.628319, 0.471239, 1.65806,
-0.471239, 0.0, 0.0 };
std::vector<double> joint3 = { -0.628319, 0.366519, 1.74533,
-0.10472, 1.5708, 0.0 };
printf("goto p0\n");
// 接口调用: 关节运动
cli->getRobotInterface(robot_name)
->getMotionControl()
->moveJoint(joint1, 10 * (M_PI / 180), 5 * (M_PI / 180), 0, 0);
// 阻塞
waitArrivel(cli->getRobotInterface(robot_name));
//等待机器人停稳
std::this_thread::sleep_for(std::chrono::seconds(1));
//记录关节位置和tcp force
auto q1 = cli->getRobotInterface(robot_name)
->getRobotState()
->getJointPositions();
auto tcp_force1 = cli->getRobotInterface(robot_name)
->getRobotState()
->getTcpForceSensors();
printf("goto p1\n");
// 接口调用: 关节运动
cli->getRobotInterface(robot_name)
->getMotionControl()
->moveJoint(joint2, 20 * (M_PI / 180), 10 * (M_PI / 180), 0, 0);
// 阻塞
waitArrivel(cli->getRobotInterface(robot_name));
//等待机器人停稳
std::this_thread::sleep_for(std::chrono::seconds(1));
//记录关节位置和tcp force
auto q2 = cli->getRobotInterface(robot_name)
->getRobotState()
->getJointPositions();
auto tcp_force2 = cli->getRobotInterface(robot_name)
->getRobotState()
->getTcpForceSensors();
printf("goto p2\n");
// 接口调用: 关节运动
cli->getRobotInterface(robot_name)
->getMotionControl()
->moveJoint(joint3, 20 * (M_PI / 180), 10 * (M_PI / 180), 0, 0);
// 阻塞
waitArrivel(cli->getRobotInterface(robot_name));
//等待机器人停稳
std::this_thread::sleep_for(std::chrono::seconds(1));
//记录关节位置和tcp force
auto q3 = cli->getRobotInterface(robot_name)
->getRobotState()
->getJointPositions();
auto tcp_force3 = cli->getRobotInterface(robot_name)
->getRobotState()
->getTcpForceSensors();
【2】求解三个点的位姿，并将位姿与传感器数据传入负载辨识计算接口中
auto pose1 = cli->getRobotInterface(robot_name)
->getRobotAlgorithm()
->forwardKinematics(q1);
auto pose2 = cli->getRobotInterface(robot_name)
->getRobotAlgorithm()
->forwardKinematics(q2);
auto pose3 = cli->getRobotInterface(robot_name)
->getRobotAlgorithm()
->forwardKinematics(q3);
std::vector<std::vector<double>> calib_forces{ tcp_force1, tcp_force2,
tcp_force3 };
std::vector<std::vector<double>> calib_poses{ std::get<0>(pose1),
std::get<0>(pose2),
std::get<0>(pose3) };
auto result = cli->getRobotInterface(robot_name)
->getRobotAlgorithm()
->calibrateTcpForceSensor(calib_forces, calib_poses);
【3】设置负载参数和传感器偏置
// 设置负载
cli->getRobotInterface(robot_name)
->getRobotConfig()
->setPayload(std::get<2>(result), std::get<1>(result),
{ 0. }, { 0. });
// 设置力传感器偏移
cli->getRobotInterface(robot_name)
->getRobotConfig()
->setTcpForceOffset(std::get<0>(result));
三、力控功能使用流程：
1.设置传感器类型
通过selectTcpForceSensor接口设置传感器类型，一般为内置embedded 或外置kw_ftsensor
#ifdef EMBEDDED
// 内置传感器
std::vector<double> sensor_pose = { 0, 0, 0, 0, 0, 0 };
cli->getRobotInterface(robot_name)
->getRobotConfig()
->selectTcpForceSensor("embedded");
#else
// 外置坤维传感器
std::vector<double> sensor_pose = { 0, 0, 0.047, 0, 0, 0 };
cli->getRobotInterface(robot_name)
->getRobotConfig()
->selectTcpForceSensor("kw_ftsensor");
#endif
2. 设置传感器偏移
内置传感器偏移为0，sensor_pose = { 0, 0, 0, 0, 0, 0 }。外置传感器Z轴有47mm的偏移，sensor_pose = { 0, 0, 0.047, 0, 0, 0 }。设置传感器偏移的接口为setTcpForceSensorPose
// 设置传感器安装位姿
cli->getRobotInterface(robot_name)
->getRobotConfig()
->setTcpForceSensorPose(sensor_pose);
3.设置m（质量）d（阻尼）k（刚度）参数
参数的维度为6个姿态，即[x,y,z,rx,ry,rz]，其中m为质量，参考值为[10.0,10.0,10.0,2.0,2.0,2.0]。d为阻尼，用来调节
力控运动过程中的速度大小，在相同受力情况下，d越大，速度越小，参考值为 [200.0,200.0,200.0,10.0,10.0,10.0]。k为刚度，用来调节机械臂弹性，刚度越大回弹越快。
通过调用setDynamicModel(m,d,k)来设置 mdk 参数。
std::vector<double> admittance_m = { 20.0, 20.0, 20.0, 10.0, 10.0, 10.0 };
std::vector<double> admittance_d = { 200.0, 200.0, 200.0, 20.0, 20.0, 20.0 };
std::vector<double> admittance_k = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
// 设置mdk参数
cli->getRobotInterface(robot_name)
->getForceControl()
->setDynamicModel(admittance_m, admittance_d, admittance_k);
4.设置目标力
通过设置目标力接口来设置力控坐标系、开启方向、目标力、速度限制以及控制类型
/**
* 设置力控参考(目标)值
*
* @param feature: 参考几何特征，用于生成力控参考坐标系
* @param compliance: 柔性轴（方向）选择
* @param wrench: 目标力/力矩
* @param limits: 速度限制
* @param type: 力控参考坐标系类型
*
* @return 成功返回0；失败返回错误码
* AUBO_BUSY
* AUBO_BAD_STATE
* -AUBO_INVL_ARGUMENT
* -AUBO_BAD_STATE
*
* @throws arcs::common_interface::AuboException
*
* @par Python函数原型
* setTargetForce(self: pyaubo_sdk.ForceControl, arg0: List[float], arg1:
* List[bool], arg2: List[float], arg3: List[float], arg4:
* arcs::common_interface::TaskFrameType) -> int
*
* @par Lua函数原型
* setTargetForce(feature: table, compliance: table, wrench: table, limits:
* table, type: number) -> nil
*
*/
int setTargetForce(const std::vector<double> &feature,
const std::vector<bool> &compliance,
const std::vector<double> &wrench,
const std::vector<double> &limits,
TaskFrameType type = TaskFrameType::FRAME_FORCE);
5.力控使用
//使能力控
cli->getRobotInterface(robot_name)
->getForceControl()
->fcEnable();
//退出力控
cli->getRobotInterface(robot_name)
->getForceControl()
->fcDisable();
//力控使能状态 返回 true 或 false
cli->getRobotInterface(robot_name)
->getForceControl()
->isFcEnabled()
//使能软浮动
cli->getRobotInterface(robot_name)
->getForceControl()
->softFloatEnable();
//退出软浮动
cli->getRobotInterface(robot_name)
->getForceControl()
->softFloatDisable();
//软浮动使能状态 返回 true 或 false
cli->getRobotInterface(robot_name)
->getForceControl()
->isSoftFloatEnabled()
四、力控应用案例
1. 软浮动插枪
// 实现阻塞功能: 当机械臂运动到目标路点时，程序再往下执行
int waitArrivel(RobotInterfacePtr impl)
{
int cnt = 0;
while (impl->getMotionControl()->getExecId() == -1) {
if (cnt++ > 5) {
std::cout << "Motion fail!" << std::endl;
return -1;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
auto id = impl->getMotionControl()->getExecId();
while (1) {
auto id1 = impl->getMotionControl()->getExecId();
if (id != id1) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
return 0;
}
// 设置力控移动距离
double movel_distance = 0.1;
// 设置传感器类型--内置
auto robot_name = cli->getRobotNames().front();
std::vector<double> sensor_pose = { 0, 0, 0, 0, 0, 0 };
cli->getRobotInterface(robot_name)
->getRobotConfig()
->selectTcpForceSensor("embedded");
// 设置传感器安装位姿
cli->getRobotInterface(robot_name)
->getRobotConfig()
->setTcpForceSensorPose(sensor_pose);
// 设置力控参数 mdk
std::vector<double> admittance_m = { 30.0, 30.0, 30.0, 6.0, 5.0, 4.0 };
std::vector<double> admittance_d = {
500.0, 500.0, 500.0, 30.0, 40.0, 15.0
};
std::vector<double> admittance_k = { 1000.0, 1000.0, 1000.0,
30.0, 30.0, 30.0 };
cli->getRobotInterface(robot_name)
->getForceControl()
->setDynamicModel(admittance_m, admittance_d, admittance_k);
// 设置目标
std::vector<bool> compliance = { true, true, true, true, true, true };
std::vector<double> target_wrench(6, 0.);
std::vector<double> speed_limits(6, 2.0);
cli->getRobotInterface(robot_name)
->getForceControl()
->setTargetForce(std::vector<double>(6, 0.), compliance, target_wrench,
speed_limits, TaskFrameType::NONE);
// 设置机械臂的速度比率
cli->getRobotInterface(robot_name)
->getMotionControl()
->setSpeedFraction(0.3);
// 获取机械臂当前位姿
auto pose =
cli->getRobotInterface(robot_name)->getRobotState()->getTcpPose();
/* 设置目标点 */
/* params1: 起始位姿（空间向量）*/
/* params2: 相对于起始位姿的姿态变化（空间向量）*/
auto pose1 =
cli->getMath()->poseTrans(pose, { 0., 0., movel_distance, 0., 0., 0. });
// 开启软浮动
if (!cli->getRobotInterface(robot_name)
->getForceControl()
->isSoftFloatEnabled()) {
cli->getRobotInterface(robot_name)
->getForceControl()
->softFloatEnable();
std::cout << "开启软浮动" << std::endl;
} else {
std::cout << "软浮动已开启" << std::endl;
}
// 移动到目标点
cli->getRobotInterface(robot_name)
->getMotionControl()
->moveLine(pose1, 1.2, 0.25, 0.025, 0);
waitArrivel(cli->getRobotInterface(robot_name));
// 关闭力控
cli->getRobotInterface(robot_name)->getForceControl()->softFloatDisable();
五、问题汇总
1.力控使用过程中触发保护停止，弹出力控制目标位置突然变化的弹窗
【问题排查及解决方案】
【1】按照第二步流程进行负载辨识，确认负载及传感器偏置已正确设置
【2】若第一步操作之后仍会出现弹窗，增大mdk中的d（阻尼）参数，降低力控过程中的运动速度
【3】如果上述两步操作之后仍会弹窗，拷贝日志，联系相关研发人员进行排查