Hello,
I am trying to work with the KUKA iiwa 14 820, my goal is to apply an impedance controller through FRI, that will be modified (so i don't want to use the cartesian impedance mode of the robot).
The problem i am having is that the measurements of joint external torques (which i need for my control scheme) are not zero when the robot is standing still with no external forces applied. There is a difference between applied torque (due to the gravity compensation) and the measured torque (should be gravity + some friction ?), and this difference goes almost completely as an external torque. An example:
Measured: -0.923347 54.783570 -5.389097 -19.635456 -1.095186 -0.747938 -0.024364
Commanded: -0.000000 52.627036 -5.599072 -19.337630 -1.030718 -0.749381 -0.011041
External: -0.924499 2.163937 0.219936 -0.301189 -0.067499 0.003105 -0.012845
This differences change with the robot posture (so not just a simple bias that could be erased). I clarify again, all this measurements are done when the robot is not moving and nothing is applying forces to it.
The scheme that i am using right now is (code at the end):
- In the robot controller side (java): a joint impedance control mode with a position hold, stiffness and damping are set to zero for all the joints.
- In the client side (FRI, C++): Applying zero torque to each joint (for now, since i need to solve this issue) and setting joint poisition as actual joint position to avoid the delta error
So i am thinking that maybe the gravity compensation performed by the robot (since it is done by the hold method + FRI itself) is not really accurate, but enough to allow mechanical friction to avoid movements, and thats the external effort that i am getting (although in that case, friction would also be badly estimated)
I know external torques are estimated from the torque measured, applied, and dynamics, but this difference seems to much to me. When I estimate external cartesian forces from te joint external torques I get values up to 3 N when no forces are really applied.
I would really appreciate any ideas or corrections if I am making some mistakes,
Martin
Java robot side:
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.");
// start PositionHold with overlay
JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
ctrMode.setMaxJointSpeed(1.2, 1.2, 1.2, 1.2, 1.2, 1.2, 1.2);
ctrMode.setDamping(0.0,0.0,0.0,0.0,0.0,0.0,0.0);
PositionHold posHold = new PositionHold(ctrMode, 80, TimeUnit.SECONDS);
_lbr.move(posHold.addMotionOverlay(torqueOverlay));
// done
friSession.close();
}FRI client side
void LBRTorqueSineOverlayClient::command()
{
// In command(), the joint values have to be sent. Which is done by calling
// the base method.
LBRClient::command();
double jointPos[LBRState::NUMBER_OF_JOINTS];
double jointTorques[LBRState::NUMBER_OF_JOINTS];
double jointCommandedTorques[LBRState::NUMBER_OF_JOINTS];
double jointExternalTorques[LBRState::NUMBER_OF_JOINTS];
memcpy(jointTorques, robotState().getMeasuredTorque(), LBRState::NUMBER_OF_JOINTS * sizeof(double));
memcpy(jointCommandedTorques, robotState().getCommandedTorque(), LBRState::NUMBER_OF_JOINTS * sizeof(double));
memcpy(jointExternalTorques, robotState().getExternalTorque(), LBRState::NUMBER_OF_JOINTS * sizeof(double));
memcpy(jointPos, robotState().getMeasuredJointPosition(), LBRState::NUMBER_OF_JOINTS * sizeof(double));
// Check for correct ClientCommandMode.
if (robotState().getClientCommandMode() == TORQUE)
{
_torques[0] = 0.0;
_torques[1] = 0.0;
_torques[2] = 0.0;
_torques[3] = 0.0;
_torques[4] = 0.0;
_torques[5] = 0.0;
_torques[6] = 0.0;
std::cout << "\n" << "Measured"<< jointTorques[0] << " " << jointTorques[1] << " " << jointTorques[2] << " " << jointTorques[3] << " " << jointTorques[4] << " " << jointTorques[5] << " " << jointTorques[6] << "\n";
std::cout << "Commanded" <<jointCommandedTorques[0] << " " << jointCommandedTorques[1] << " " << jointCommandedTorques[2] << " " << jointCommandedTorques[3] << " " << jointCommandedTorques[4] << " " << jointCommandedTorques[5] << " " << jointCommandedTorques[6] << "\n";
std::cout << "External"<< jointExternalTorques[0] << " " << jointExternalTorques[1] << " " << jointExternalTorques[2] << " " << jointExternalTorques[3] << " " << jointExternalTorques[4] << " " << jointExternalTorques[5] << " " << jointExternalTorques[6] << "\n";
// Set superposed joint torques.
robotCommand().setJointPosition(jointPos);
robotCommand().setTorque(_torques);
}
}
