Hello,
I use Cartesian Impedance mode to take landmarks and to perform classic movements in a more secure manner.
During movement, even with the strongest impedance (5000 for xyz and 300 for abc + all damping to 1), there is always a slight difference between the position I want to reach and the one the robot is trying to reach. Even pushing it with his hand to help him reach that point he tries to go back to the offset point.
I noticed that the offset is still on the base-head axis. It shifts towards the base when it goes down and away from it when it goes up.
I do have a tool on the robot and I defined it in the project.
I use the movement functions in the following way:
For the moment I regulate this offset by an offset of a few mm on the x and y axes.
Is this problem "normal", is it a physical problem of the robot?
Is there a solution to fix? Maybe an error in my code?
Many thanks in advance
package application;
import static com.kuka.roboticsAPI.motionModel.BasicMotions.lin;
import javax.inject.Inject;
import javax.inject.Named;
import libraries.Impedance_Bank;
import libraries.Position_Bank;
import libraries.Security;
import com.kuka.common.ThreadUtil;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.geometricModel.Frame;
import com.kuka.roboticsAPI.geometricModel.Tool;
import com.kuka.roboticsAPI.motionModel.HandGuidingMotion;
import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
/**
* Implementation of a robot application.
* <p>
* The application provides a {@link RoboticsAPITask#initialize()} and a
* {@link RoboticsAPITask#run()} method, which will be called successively in
* the application lifecycle. The application will terminate automatically after
* the {@link RoboticsAPITask#run()} method has finished or after stopping the
* task. The {@link RoboticsAPITask#dispose()} method will be called, even if an
* exception is thrown during initialization or run.
* <p>
* <b>It is imperative to call <code>super.dispose()</code> when overriding the
* {@link RoboticsAPITask#dispose()} method.</b>
*
* @see UseRoboticsAPIContext
* @see #initialize()
* @see #run()
* @see #dispose()
*/
public class Landmark_by_hand extends RoboticsAPIApplication {
@Inject
private LBR robot;
private Security security = new Security();
private Position_Bank positionBank = new Position_Bank();
private Impedance_Bank impedanceBank = new Impedance_Bank();
@Inject
@Named("ToolTest1")
private Tool ToolTest1;
private HandGuidingMotion motion = new HandGuidingMotion().setCartVelocityLimit(100);
private boolean handGuidingState = false;
private int dialogWindow;
@Override
public void initialize() {
// initialize your application here
ToolTest1.attachTo(robot.getFlange());
impedanceBank.initialize();
motion.setJointLimitViolationFreezesAll(false);
}
@Override
public void run() {
security.Mode(robot,"slowMoov");
getLogger().info("ESM is set for slowMoov");
robot.move(positionBank.getHandGuidingStartPose().setJointVelocityRel(0.02));
Frame frame2= robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"));
ToolTest1.detach();
while (handGuidingState == false){
security.Mode(robot,"handGuiding");; // controller will only monitor the hand guiding switch
getLogger().info("ESM is set for handGuiding");
robot.move(motion);
security.Mode(robot,"slowMoov");
getLogger().info("ESM is set for slowMoov");
dialogWindow = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION, "Restart Hand Guiding ?", "Yes", "No");
if (dialogWindow == 1)
{
handGuidingState = true;
}
}
ToolTest1.attachTo(robot.getFlange());
security.Mode(robot,"slowMoov");
// Wait a little to reduce robot vibration after stop.
ThreadUtil.milliSleep(1000);
Frame holeFrame = robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"));
Frame frame=holeFrame.copy();
getLogger().info("holeFrame : " + holeFrame);
getLogger().info("frame : " + frame);
getLogger().info("starting loop");
int i =1;
for (i=1;i<=4;i++){
getLogger().info("Going out of the hole");
frame.setZ(frame.getZ()+30).setY(frame.getY()+5).setX(frame.getX()-10);
getLogger().info("frame : " + frame);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10).setMode(impedanceBank.getImpedanceForTouchMoov()));
// Wait a little to reduce robot vibration after stop.
ThreadUtil.milliSleep(500);
getLogger().info("OUT IMPEDANCE n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP")));
getLogger().info("OUT IMPEDANCE n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"),holeFrame));
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10));
// Wait a little to reduce robot vibration after stop.
ThreadUtil.milliSleep(500);
getLogger().info("OUT NO IMPEDANCE n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP")));
getLogger().info("OUT NO IMPEDANCE n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"),holeFrame));
frame.setZ(holeFrame.getZ()+100);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame2 = robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"));
frame2.setZ(150).setY(137);
ToolTest1.getFrame("/TCP").move(lin(frame2).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame2.setX(584);
ToolTest1.getFrame("/TCP").move(lin(frame2).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame2.setZ(50).setAlphaRad(Math.toRadians(90)).setBetaRad(Math.toRadians(0)).setGammaRad(Math.toRadians(180));
ToolTest1.getFrame("/TCP").move(lin(frame2).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame2.setZ(15);
ToolTest1.getFrame("/TCP").move(lin(frame2).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
// Wait a little to reduce robot vibration after stop.
ThreadUtil.milliSleep(1000);
frame2.setZ(150);
ToolTest1.getFrame("/TCP").move(lin(frame2).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame.setZ(holeFrame.getZ()+100);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame.setZ(holeFrame.getZ()+25);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(40).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame.setZ(holeFrame.getZ()+15).setY(holeFrame.getY()-5).setX(holeFrame.getX()+5);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10).setMode(impedanceBank.getImpedanceForSafeMoov()));
frame.setZ(holeFrame.getZ()+9);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10));
frame.setZ(holeFrame.getZ()+8);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10));
frame.setZ(holeFrame.getZ()-5);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10).setMode(impedanceBank.getImpedanceForTouchMoov()));
getLogger().info("IN n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP")));
getLogger().info("IN n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"),holeFrame));
}
frame.setZ(-20);
ToolTest1.getFrame("/TCP").move(lin(frame).setCartVelocity(10).setMode(impedanceBank.getImpedanceForTouchMoov()));
getLogger().info("OUT n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP")));
getLogger().info("OUT n°:" +(i)+ robot.getCurrentCartesianPosition(ToolTest1.getFrame("/TCP"),holeFrame));
}
}
Display More