-
Notifications
You must be signed in to change notification settings - Fork 21
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #22 from fredRocs/fri-2.5
LBRServer FRI 2.x added. Thank you @fredRocs
- Loading branch information
Showing
1 changed file
with
162 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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(); | ||
} | ||
} | ||
|
||
} |