Loading... ```java package application; import static com.kuka.roboticsAPI.motionModel.BasicMotions.*; import javax.inject.Inject; import com.kuka.common.ThreadUtil; import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; import com.kuka.med.controllerModel.MedController; import com.kuka.roboticsAPI.controllerModel.sunrise.ISunriseRequestService; import com.kuka.roboticsAPI.controllerModel.sunrise.api.SSR; import com.kuka.roboticsAPI.controllerModel.sunrise.api.SSRFactory; import com.kuka.roboticsAPI.controllerModel.sunrise.connectionLib.Message; import com.kuka.roboticsAPI.controllerModel.sunrise.positionMastering.PositionMastering; import com.kuka.roboticsAPI.deviceModel.JointPosition; import com.kuka.roboticsAPI.deviceModel.LBR; import com.kuka.roboticsAPI.deviceModel.OperationMode; import com.kuka.roboticsAPI.motionModel.PTP; /** * This application can be used as template for LbrMed Position and GMS Referencing. * The safety needs exactly 10 measurements to perform a successful GMS Referencing. * The time between two measurements must be less than 15 seconds. */ public class PositionAndGMSReferencing extends RoboticsAPIApplication { private MedController medController; @Inject private LBR lbrMed; private final static double sideOffset = Math.toRadians(5); // 侧向运动的弧度偏移5° private static double joggingVelocity = 0.2; // 定义相对运动速度 private final static int axisId[] = {0, 1, 2, 3, 4, 5, 6}; // 定义各个轴的编号 private final static int GMS_REFERENCING_COMMAND = 2; // GMS力矩传感器安全命令 private final static int COMMAND_SUCCESSFUL = 1; private int positionCounter = 0; // 位置计数 从0开始 public void initialize() { medController = (MedController) lbrMed.getController(); // register the application as robot state listener } public void run() { PositionMastering mastering = new PositionMastering(lbrMed); // 定义mastering这个变量是lbrMed的mastering boolean allAxesMastered = true; for (int i = 0; i < axisId.length; ++i) { // Check if the axis is mastered - if not, no referencing is possible boolean isMastered = mastering.isAxisMastered(axisId[i]); // isAxisMastered()返回的是布尔类型 True/False if (!isMastered) { getLogger().warn("Axis with axisId " + axisId[i] + " is not mastered, therefore it cannot be referenced"); } allAxesMastered &= isMastered; } // 如果操作模式为T1手动的情况下可以快一点 if (OperationMode.T1 == lbrMed.getOperationMode()) { joggingVelocity = 0.4; } if (allAxesMastered) // 如果前面allAxesMastered &= isMastered;最终结果为True时，进行下一步move { getLogger().info("Perform position and GMS referencing with 5 positions"); // 运动到Home Position getLogger().info("Moving to home position"); lbrMed.move(ptpHome().setJointVelocityRel(joggingVelocity)); // 以joggingVelocity的速度运动到ptpHome() // In this example 5 positions are defined, though each one // will be reached from negative and from positive axis // direction resulting 10 measurements. The safety needs // exactly 10 measurements to perform the referencing. // vscode会显示angdeg，这是vscode自己加的注释，表示以°为单位的角度的意思，不用管它 performMotion(new JointPosition(Math.toRadians(0.0), Math.toRadians(16.18), Math.toRadians(23.04), Math.toRadians(37.35), Math.toRadians(-67.93), Math.toRadians(38.14), Math.toRadians(-2.13))); performMotion(new JointPosition(Math.toRadians(18.51), Math.toRadians(9.08), Math.toRadians(-1.90), Math.toRadians(49.58), Math.toRadians(-2.92), Math.toRadians(18.60), Math.toRadians(-31.18))); performMotion(new JointPosition(Math.toRadians(-18.53), Math.toRadians(-25.76), Math.toRadians(-47.03), Math.toRadians(-49.55), Math.toRadians(30.76), Math.toRadians(-30.73), Math.toRadians(20.11))); performMotion(new JointPosition(Math.toRadians(-48.66), Math.toRadians(24.68), Math.toRadians(-11.52), Math.toRadians(10.48), Math.toRadians(-11.38), Math.toRadians(-20.70), Math.toRadians(20.87))); performMotion(new JointPosition(Math.toRadians(9.01), Math.toRadians(-35.00), Math.toRadians(24.72), Math.toRadians(-82.04), Math.toRadians(14.65), Math.toRadians(-29.95), Math.toRadians(1.57))); // Move to home position at the end getLogger().info("Moving to home position"); lbrMed.move(ptpHome().setJointVelocityRel(joggingVelocity)); } } private void performMotion(JointPosition position) // 定义一个parameter为7个轴角度的方法performMotion { getLogger().info("Moving to position #" + (++positionCounter)); // 在run()方法中每次positionCounter都会+1 PTP mainMotion = new PTP(position).setJointVelocityRel(joggingVelocity); // mainMotion为主运动位置 lbrMed.move(mainMotion); // Med运动到mainMotion getLogger().info("Moving to current position from negative direction"); // getJointCount()获取关节数7，然后JointPosition(7)生成一个JointPosition(__, __, __, __, __, __, __)里面轴角度为空的轴位置 // 然后利用for循环依次设定这个新的position1的轴角度。把空填满 JointPosition position1 = new JointPosition(lbrMed.getJointCount()); for (int i = 0; i < lbrMed.getJointCount(); ++i) { position1.set(i, position.get(i) - sideOffset); // 依次按照position的每个轴减5°形成position1 } PTP motion1 = new PTP(position1).setJointVelocityRel(joggingVelocity); // motion1为侧向运动位置position1 lbrMed.move(motion1); // 移动到motion1 lbrMed.move(mainMotion); // 移动到mainMotion // 停止后暂停，以消除机械臂的震动 ThreadUtil.milliSleep(2500); // Send the command to safety to trigger the measurement sendSafetyCommand(); //发送安全命令，下面定义了这个方法 // +5°反方向侧向运动，方式同上 getLogger().info("Moving to current position from positive direction"); JointPosition position2 = new JointPosition(lbrMed.getJointCount()); for (int i = 0; i < lbrMed.getJointCount(); ++i) { position2.set(i, position.get(i) + sideOffset); } PTP motion2 = new PTP(position2).setJointVelocityRel(joggingVelocity); lbrMed.move(motion2); lbrMed.move(mainMotion); // Wait a little to reduce robot vibration after stop ThreadUtil.milliSleep(2500); // Send the command to safety to trigger the measurement sendSafetyCommand(); } // 暂时不知道什么意思，明天可以问赵工 private void sendSafetyCommand() { ISunriseRequestService requestService = (ISunriseRequestService) (medController.getRequestService()); SSR ssr = SSRFactory.createSafetyCommandSSR(GMS_REFERENCING_COMMAND); Message response = requestService.sendSynchronousSSR(ssr); int result = response.getParamInt(0); if (COMMAND_SUCCESSFUL != result) { getLogger().warn("Command did not execute successfully, response = " + result); } } } ``` 最后修改：2023 年 09 月 21 日 © 允许规范转载 赞 0 如果觉得我的文章对你有用，请随意赞赏
