I am trying to establish the FRI connection for KUKA LBR iiwa. I know how to configure the FRI connection as there are example programs available in the Sunrise.Workbench.
A sample code is given below. My question is 'how to pass' the joint torque values (or joint position or wrench) to the controller using 'torqueOverlay' as mentioned in the code below. Since I could not find any documentation on this, it was quite difficult to figure out. Any sample code with explanation or any clues would be more than helpful.
JAVA code:
package com.kuka.connectivity.fri.example;
import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;
import com.kuka.connectivity.fri.ClientCommandMode;
import com.kuka.connectivity.fri.FRIConfiguration;
import com.kuka.connectivity.fri.FRIJointOverlay;
import com.kuka.connectivity.fri.FRISession;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.motionModel.PositionHold;
import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode;
/**
* Moves the LBR in a start position, creates an FRI-Session and executes a
* PositionHold motion with FRI overlay. During this motion joint angles and
* joint torques can be additionally commanded via FRI.
*/
public class LBRTorqueSineOverlay extends RoboticsAPIApplication
{
private Controller _lbrController;
private LBR _lbr;
private String _clientName;
@Override
public void initialize()
{
_lbrController = (Controller) getContext().getControllers().toArray()[0];
_lbr = (LBR) _lbrController.getDevices().toArray()[0];
// **********************************************************************
// *** change next line to the FRIClient's IP address ***
// **********************************************************************
_clientName = "127.0.0.1";
}
@Override
public void run()
{
// configure and start FRI session
FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName);
// for torque mode, there has to be a command value at least all 5ms
friConfiguration.setSendPeriodMilliSec(5);
friConfiguration.setReceiveMultiplier(1);
getLogger().info("Creating FRI connection to " + friConfiguration.getHostName());
getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |"
+ " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier());
FRISession friSession = new FRISession(friConfiguration);
FRIJointOverlay torqueOverlay = new FRIJointOverlay(friSession, ClientCommandMode.TORQUE);
// wait until FRI session is ready to switch to command mode
try
{
friSession.await(10, TimeUnit.SECONDS);
}
catch (final TimeoutException e)
{
getLogger().error(e.getLocalizedMessage());
friSession.close();
return;
}
getLogger().info("FRI connection established.");
// move to start pose
_lbr.move(ptp(Math.toRadians(90), Math.toRadians(-60), .0, Math.toRadians(60), .0, Math.toRadians(-60), .0));
// start PositionHold with overlay
JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(200, 200, 200, 200, 200, 200, 200);
PositionHold posHold = new PositionHold(ctrMode, 20, TimeUnit.SECONDS);
_lbr.move(posHold.addMotionOverlay(torqueOverlay));
// done
friSession.close();
}
/**
* main.
*
* @param args
* args
*/
public static void main(final String[] args)
{
final LBRTorqueSineOverlay app = new LBRTorqueSineOverlay();
app.runApplication();
}
}