Hello,
I am currently working with a LBR iiwa on an application that has the goal to stir and mix some powder in order to make it go through a sieve.
For this purpose I have prepared an application containing the computations for implementing some sort of planetary motion as can be seen from the graph below.
I am also integrating a CartesianSineImpedanceControlMode in the movement.
Here is the code:
@Inject
private LBR lbr;
final static double[] startingPosition = new double [] { 0, Math.toRadians(65), 0, -Math.toRadians(80), 0, Math.toRadians(35), 0};
final static int numTot = 630;
final static double omega1 = 0.01;
final static int radius1 = 90;
final static double omega2 = 0.1;
final static int radius2 = 60;
@Override
public void initialize() {
lbr = getContext().getDeviceFromType(LBR.class);
}
@Override
public void run() {
PTP ptpStartingPos = ptp(startingPosition);
lbr.move(ptpStartingPos);
ptpStartingPos.setJointVelocityRel(0.25);
Frame startingFrame = lbr.getCurrentCartesianPosition(lbr.getFlange());
Spline planetarySpline = createPlanetarySpline(startingFrame).setJointJerkRel(1).setCartVelocity(900);
planetarySpline.setJointVelocityRel(0.70);
planetarySpline.setMode(CartesianSineImpedanceControlMode.createDesiredForce(CartDOF.Z, 5, 500));
while(true){
lbr.move(planetarySpline);
}
}
private Spline createPlanetarySpline (Frame centerFrame){
SplineMotionCP<?>[] spMotions = new SplineMotionCP<?>[numTot];
for(int t=1; t<=numTot; t++) {
double teta = ((omega1*t));
double phi = ((omega2*t));
double x = (radius1 * Math.cos(teta)) + (radius2 * Math.cos(phi));
double y = (radius1 * Math.sin(teta)) + (radius2 * Math.sin(phi));
double z = centerFrame.getZ();
Frame f = new Frame(centerFrame).setX(x).setY(y).setZ(z);
spMotions[t-1] = spl(f);
}
Spline completeSpline = new Spline(spMotions);
return completeSpline;
}
Display More
As can be seen, the Spline is computed once at the start of the application.
The problem that I am facing is that:
- before starting the movement, the robot takes several seconds planning the spline.
- for the same reason, the robot takes also several seconds to restart the movement (inside the while(true))
Can anyone advice me on how to the make the movement smoother? What am I doing wrong?
Is there any other way to make the robot execute the same motion "forever" other than using a while(true) ?
P.S.: I tried switching the spl to a lin but that only made things worse.
Thanks in advance to anyone who will reply
-- Vale