private void performMotion(JointPosition position) {
getLogger().info("Moving to position #" + (++positionCounter));
PTP mainMotion = new PTP(position).setJointVelocityRel(joggingVelocity);
lbr_iiwa.move(mainMotion);
getLogger().info("Moving to current position from negative direction");
JointPosition position1 = new JointPosition(lbr_iiwa.getJointCount());
for (int i = 0; i < lbr_iiwa.getJointCount(); ++i) {
position1.set(i, position.get(i) - sideOffset);
}
PTP motion1 = new PTP(position1).setJointVelocityRel(joggingVelocity);
lbr_iiwa.move(motion1);
lbr_iiwa.move(mainMotion);
// Wait a little to reduce robot vibration after stop.
ThreadUtil.milliSleep(2500);
// Send the command to safety to trigger the measurement
sendSafetyCommand();
getLogger().info("Moving to current position from positive direction");
JointPosition position2 = new JointPosition(lbr_iiwa.getJointCount());
for (int i = 0; i < lbr_iiwa.getJointCount(); ++i) {
position2.set(i, position.get(i) + sideOffset);
}
PTP motion2 = new PTP(position2).setJointVelocityRel(joggingVelocity);
lbr_iiwa.move(motion2);
lbr_iiwa.move(mainMotion);
// Wait a little to reduce robot vibration after stop
ThreadUtil.milliSleep(2500);
// Send the command to safety to trigger the measurement
sendSafetyCommand();
}
public void run() {
lbr_iiwa_7_R800_1.move(ptpHome());
Frame myFrame = new Frame(0, 0, 0, 0, 0, 0);
}
public void run() {
PositionMastering mastering = new PositionMastering(lbr_iiwa);
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]);
if (!isMastered) {
getLogger()
.warn(
"Axis with axisId "
+ axisId[i]
+ " is not mastered, therefore it cannot be referenced");
}
allAxesMastered &= isMastered;
}
// We can move faster, if operation mode is T1
if (OperationMode.T1 == lbr_iiwa.getOperationMode()) {
joggingVelocity = 0.4;
}
if (allAxesMastered) {
getLogger().info("Perform position and GMS referencing with 5 positions");
// Move to home position
getLogger().info("Moving to home position");
lbr_iiwa.move(ptpHome().setJointVelocityRel(joggingVelocity));
// 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.
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");
lbr_iiwa.move(ptpHome().setJointVelocityRel(joggingVelocity));
}
}
