Skip to content

Commit

Permalink
Merge pull request #22 from fredRocs/fri-2.5
Browse files Browse the repository at this point in the history
LBRServer FRI 2.x added. Thank you @fredRocs
  • Loading branch information
mhubii authored Feb 26, 2024
2 parents d107aa3 + 2a4c733 commit 4b440e8
Showing 1 changed file with 162 additions and 0 deletions.
162 changes: 162 additions & 0 deletions server_app/LBRServer.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,162 @@
package application;

import static com.kuka.roboticsAPI.motionModel.BasicMotions.positionHold;

import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;
import javax.inject.Inject;
import java.util.Arrays;

import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.sensitivity.LBR;
import com.kuka.sensitivity.controlmode.CartesianImpedanceControlMode;
import com.kuka.sensitivity.controlmode.JointImpedanceControlMode;
import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
import com.kuka.roboticsAPI.uiModel.IApplicationUI;
import com.kuka.roboticsAPI.motionModel.controlModeModel.*;
import com.kuka.fri.FRIChannelInformation;
import com.kuka.fri.FRIConfiguration;
import com.kuka.fri.FRISession;
import com.kuka.fri.IFRISessionListener;
import com.kuka.fri.FRIJointOverlay;
import com.kuka.fri.common.ClientCommandMode;



public class LBRServer extends RoboticsAPIApplication {
// members
@Inject private LBR lbr_;
@Inject private IApplicationUI applicationUi;

// control mode
private enum CONTROL_MODE {
POSITION_CONTROL,
JOINT_IMPEDANCE_CONTROL,
CARTESIAN_IMPEDANCE_CONTROL;
}

// convert enum to string array, see https://stackoverflow.com/questions/13783295/getting-all-names-in-an-enum-as-a-string
public static String[] getNames(Class<? extends Enum<?>> e) {
return Arrays.toString(e.getEnumConstants()).replaceAll("^.|.$", "").split(", ");
}

// FRI parameters
private String client_name_;
private String[] client_names_ = {"172.31.1.148", "192.170.10.1"};
private int send_period_;
private String[] send_periods_ = {"1", "2", "5", "10"}; // send period in ms

private FRIConfiguration fri_configuration_;
private FRISession fri_session_;
private FRIJointOverlay fri_overlay_;

private AbstractMotionControlMode control_mode_;
private String[] control_modes_ = getNames(CONTROL_MODE.class);
private ClientCommandMode command_mode_;
private String[] command_modes_ = getNames(ClientCommandMode.class);


// methods
public void request_user_config() {
// send period
int selectedButtonIndex = applicationUi.displayModalDialog(
ApplicationDialogType.QUESTION,
"Select the desired FRI send period [ms]:",
send_periods_
);
send_period_ = Integer.valueOf(send_periods_[selectedButtonIndex]);
getLogger().info("Send period set to: " + send_period_);
// remote IP address
selectedButtonIndex = applicationUi.displayModalDialog(
ApplicationDialogType.QUESTION,
"Select your remote IP address:",
client_names_
);
client_name_ = client_names_[selectedButtonIndex];
getLogger().info("Remote address set to: " + client_name_);

// control mode
selectedButtonIndex = applicationUi.displayModalDialog(
ApplicationDialogType.QUESTION,
"Select the desired FRI control mode: ",
control_modes_
);

CONTROL_MODE control_mode = CONTROL_MODE.values()[selectedButtonIndex];
switch (control_mode) {
case POSITION_CONTROL:
control_mode_ = new PositionControlMode();
break;
case JOINT_IMPEDANCE_CONTROL:
control_mode_ = new JointImpedanceControlMode(200, 200, 200, 200, 200, 200, 200);
break;
case CARTESIAN_IMPEDANCE_CONTROL:
control_mode_ = new CartesianImpedanceControlMode();
break;
}
getLogger().info("Control mode set to: " + control_mode.name());
// command mode
selectedButtonIndex = applicationUi.displayModalDialog(
ApplicationDialogType.QUESTION,
"Select the desired FRI client command mode: ",
command_modes_
);
command_mode_ = ClientCommandMode.values()[selectedButtonIndex];
getLogger().info("Client command mode set to: " + command_mode_.name());
}
public void configure_fri() {
fri_configuration_ = FRIConfiguration.createRemoteConfiguration(lbr_, client_name_);
fri_configuration_.setSendPeriodMilliSec(send_period_);
getLogger().info("Creating FRI connection to " + fri_configuration_.getHostName());
getLogger().info(
"SendPeriod: " + fri_configuration_.getSendPeriodMilliSec() + "ms |"
+ " ReceiveMultiplier: " + fri_configuration_.getReceiveMultiplier()
);
fri_session_ = new FRISession(fri_configuration_);
fri_overlay_ = new FRIJointOverlay(fri_session_, command_mode_);
fri_session_.addFRISessionListener(new IFRISessionListener() {
@Override
public void onFRISessionStateChanged(FRIChannelInformation friChannelInformation) {
getLogger().info("Session State change " + friChannelInformation.getFRISessionState().toString() );
}
@Override
public void onFRIConnectionQualityChanged(FRIChannelInformation friChannelInformation) {
getLogger().info("Quality change signalled "+friChannelInformation.getQuality());
getLogger().info("Jitter "+friChannelInformation.getJitter());
getLogger().info("Latency "+friChannelInformation.getLatency());
}
});
// try to connect
try {
fri_session_.await(10, TimeUnit.SECONDS);
} catch (final TimeoutException e) {
getLogger().error(e.getLocalizedMessage());
return;
}
getLogger().info("FRI connection established.");
}
@Override
public void initialize() {
// set FRI parameters
request_user_config();
// configure the FRI
configure_fri();
}

@Override
public void run() {
// run the FRI
lbr_.getFlange().move(positionHold(control_mode_, -1, TimeUnit.SECONDS).addMotionOverlay(fri_overlay_));
return;
}
@Override
public void dispose() {
getLogger().info("Close connection to client");

if (null != fri_session_)
{
fri_session_.close();
}
}

}

0 comments on commit 4b440e8

Please sign in to comment.