I got hand guiding working without the hand guiding flange by using two switched and connecting them to the x11 port but now I want to connect it to ROS2 to get position and force data of the robot. I am using the lbr-fri-ros2 stack and using their sunrise code, but when adding the EMS for hand guiding mode, I can't run their program on my pendant. It seems like whenever I add the hand guiding inactive EMS, the pendant already has a program running before I can run my program. The play button on the pendant is also completely greyed out. I will attach my sunrise code here, any input is appreciated!
package lbr_fri_ros2;
import static com.kuka.roboticsAPI.motionModel.BasicMotions.positionHold;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.TimeoutException;
import java.util.Arrays;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
import com.kuka.roboticsAPI.motionModel.controlModeModel.*;
import com.kuka.connectivity.fastRobotInterface.*;
public class LBRServer extends RoboticsAPIApplication {
// members
private LBR lbr_;
private Controller lbr_controller_;
// 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 id = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Select the desired FRI send period [ms]:",
send_periods_
);
send_period_ = Integer.valueOf(send_periods_[id]);
getLogger().info("Send period set to: " + send_period_);
// remote IP address
id = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Select your remote IP address:",
client_names_
);
client_name_ = client_names_[id];
getLogger().info("Remote address set to: " + client_name_);
// control mode
id = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Select the desired FRI control mode: ",
control_modes_
);
CONTROL_MODE control_mode = CONTROL_MODE.values()[id];
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
id = getApplicationUI().displayModalDialog(
ApplicationDialogType.QUESTION,
"Select the desired FRI client command mode: ",
command_modes_
);
command_mode_ = ClientCommandMode.values()[id];
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() {
lbr_controller_ = (Controller) getContext().getControllers().toArray()[0];
lbr_ = (LBR) lbr_controller_.getDevices().toArray()[0];
// set FRI parameters
request_user_config();
// configure the FRI
configure_fri();
}
@Override
public void run() {
// run the FRI
lbr_.move(positionHold(control_mode_, -1, TimeUnit.SECONDS).addMotionOverlay(fri_overlay_));
return;
}
@Override
public void dispose() {
// close connection
getLogger().info("Disposing FRI session.");
fri_session_.close();
super.dispose();
}
/**
* main
*
* @param args
*/
public static void main(final String[] args) {
LBRServer app = new LBRServer();
app.runApplication();
}
}