Safety and Realibity measures when using Cartesian Impedance Controller

[anubhav-dogra]

Hi Matthias,

I have been using Cartesian Impedance Contoller with iiwa_ros and also using CI_trajectory_controller/reference_pose or cartesian trajectory planner to send the commands to the robot. The functioning is very impressive, thanks to you. I want to gain confidence for regular use of the same functionality without actively keeping my awareness to press Emergency stop :p , just in case robot behaves weirdly. Are their any reliability and safety measures which I can set to keep the robot in confidence even when working very close to human? There are some safety configurations which I have set up in the KUKA controller such as joint limits, max. cartesian velocities, etc. Can you recommend additional measures please. Sometimes what happens that,

1. FRI connection would break while running and restarting the application gives some JAVAlang error. This makes the robot totally in torque control mode with zero stiffness. Due to the errors in load data detemination (multiple trials/ additionally due to have some wires going down from the end-effector), robot does tries to move faster.
2. Sending the reference_poses to the robot, sometimes robot tries to move very fast. Parameters which Im using are as follows: {delta_tau_max: 1.0
   filtering:
   nullspace_config: 0.1
   pose: 0.3
   stiffness: 0.1
   wrench: 0.5 }
3. What will happen if the IK solution is not available at certain poses? In simulation the robot moves very fast in these situations. Can you please shed some light over it? I tried to check the IK using iiwa_tools over the path computed by cartesin_trajecotry_generator, but Im getting mirror solutions for every next pose. (provided the seed value as per the joint values of the last pose).

[matthias-mayr]

Are their any reliability and safety measures which I can set to keep the robot in confidence even when working very close to human?

As described in the Readme we limit the torques the robot arm can apply. Right now we configured 20 Nm. However it would make sense to reduce it a bit more for joint 6 & 7. Overall what these limits allow you to do is to just grab the robot arm and move it around, independently of its commands.

In addition the velocities are limited to 1 rad/s in the same file.

FRI connection would break while running and restarting the application gives some JAVAlang error. This makes the robot totally in torque control mode with zero stiffness. Due to the errors in load data detemination (multiple trials/ additionally due to have some wires going down from the end-effector), robot does tries to move faster.

The FRI connection should not break. For us it runs for hours on 500 Hz. On 1000 Hz I do not have that extended experience. I have not fully figured out the mechanics behind the controller, driver & FRI when restarting a connection. Right now we typically restart everything until someone does so.

This makes the robot totally in torque control mode with zero stiffness.

I do not understand this. What typically happens then is that the connection breaks and the brakes are engaged. Under totally in torque control mode with zero stiffness I would understand that it's free floating (zero stiffness). However, this is not a behavior I have seen before.

Sending the reference_poses to the robot, sometimes robot tries to move very fast.

Depending on how far the reference poses are away, the robot could still move fast. You applied pose filtering of 0.3, meaning that (roughly) after 1 seconds the reference pose would be 0.3*new_pose + 0.7*old_pose. If the new pose if 1.5m away that's still about 0.5m.

What will happen if the IK solution is not available at certain poses? In simulation the robot moves very fast in these situations. Can you please shed some light over it? I tried to check the IK using iiwa_tools over the path computed by cartesin_trajecotry_generator, but Im getting mirror solutions for every next pose. (provided the seed value as per the joint values of the last pose).

What happens with the reference poses is that they "pull" the robot towards them as you would with your hand. It would end up in some sort of equilibrium.

but Im getting mirror solutions for every next pose.

I do not understand this. Can you elaborate?

[anubhav-dogra]

Thanks for replying on the above queries.

I do not understand this. What typically happens then is that the connection breaks and the brakes are engaged. Under totally in torque control mode with zero stiffness I would understand that it's free floating (zero stiffness). However, this is not a behavior I have seen before.

In our case, breaks doesnt apply, FRIOverlayGripper Application just stops without any error (green light turns off), it still updates the robot state on the terminal (if verbose:true), and robot is in pure torque mode (free floating).

Depending on how far the reference poses are away, the robot could still move fast. You applied pose filtering of 0.3, meaning that (roughly) after 1 seconds the reference pose would be 0.3*new_pose + 0.7*old_pose. If the new pose if 1.5m away that's still about 0.5m.

Thanks for the clearification here. I get it now. However, can you please suggest something about the pose tracking, (if our pose is varying continuously) ? I tried with both cartesian_trajectory_generator as well as directly sending varying pose to /reference_pose in CI controller, But I found that the robot motion is a bit jerky over the path. This can also be verified through commanded_torque plots here

I do not understand this. Can you elaborate?

This generally happens when the distance between the goal and the current pose is larger. The IK solutions over the path are not consistent (Giving different solutions for e.g., elbow up or elbow down solution). Maybe I need to apply a different algorithm rather than using ik_server from iiwa_tools

[matthias-mayr]

In our case, breaks doesnt apply, FRIOverlayGripper Application just stops without any error (green light turns off), it still updates the robot state on the terminal (if verbose:true), and robot is in pure torque mode (free floating).

Interesting. The only that the controller could play in this is that it could potentially take too long to calculate new torques and slow down the FRI loop. However for us it works at 1kHz when running a realtime kernel. An easy test for this would be to set the control frequency to 333 or even 200 Hz. Even though, iiwa_ros is probably the better place for an issue, does it print anything on the Smartpad? It also makes sense to check the log as some warnings and error messages disappear.

Thanks for the clearification here. I get it now. However, can you please suggest something about the pose tracking, (if our pose is varying continuously) ? I tried with both cartesian_trajectory_generator as well as directly sending varying pose to /reference_pose in CI controller, But I found that the robot motion is a bit jerky over the path. This can also be verified through commanded_torque plots here

My explanation for plot is that the cartesian_trajectory_generator received new poses about twice per second. What it does in this case is that it starts a new trajectory with zero velocity and the given configured acceleration every time it receives a new pose. So in it's current state, the motion generator is not intended to get pose updates in the middle of a robot motion.

If you want to send small incremental updates to your poses you can configure pose filtering of the controller and just send the new poses directly. If you want to run a mix of small updates and larger ones, you can also write a small ROS node that just rate-limits the changes to some 10cm a second or so.

This generally happens when the distance between the goal and the current pose is larger. The IK solutions over the path are not consistent (Giving different solutions for e.g., elbow up or elbow down solution). Maybe I need to apply a different algorithm rather than using ik_server from iiwa_tools

The Cartesian trajectory generator does not have any notion of the robot kinematics. It just sees the current end-effector pose and the target pose, calculates the trajectory and publishes the end-effector reference poses. If you want to have more control over what's happening, you can use a motion planning library like MoveIt and send those trajectories as joint-space trajectories to the controller. If you have configured some nullspace stiffness, you can expect the arm to follow those trajectories including the elbow movement.

If you know your goal joint configuration, another option is to send it "manually" to the controller for nullspace control. Bear in mind that there would be path deviations if the FK pose of that configuration is not the same a the current pose of the end-effector. However, this is exactly the strategy that we apply in our RL-experiments when we want to reset the robot. We have some joint configuration should be in, we send the FK-pose to the Cartesian trajectory generator and set this joint configuration for the nullspace. Here's the (super ugly) script for that.

[anubhav-dogra]

My explanation for plot is that the cartesian_trajectory_generator received new poses about twice per second. What it does in this case is that it starts a new trajectory with zero velocity and the given configured acceleration every time it receives a new pose. So in it's current state, the motion generator is not intended to get pose updates in the middle of a robot motion.

If you want to send small incremental updates to your poses you can configure pose filtering of the controller and just send the new poses directly. If you want to run a mix of small updates and larger ones, you can also write a small ROS node that just rate-limits the changes to some 10cm a second or so.

Sounds like a solution in this case. I have tried sending poses directly to impedance controller as reference_pose and have found the similar behaviour. May be mixing up the small and large updates would be a better appraoch.

However, this is exactly the strategy that we apply in our RL-experiments when we want to reset the robot. We have some joint configuration should be in, we send the FK-pose to the Cartesian trajectory generator and set this joint configuration for the nullspace.

Understood ! Thanks for clarifications.

[anubhav-dogra]

Hi Matthias,

Have you noticed large variation (2 to 3 N) in the cartesian wrench (computed using Jacobian) when the robot moves in cartesian space?
Also, For an example, when the robot is at initial pose lets say A, the wrench in Z direction of the tool would be (for example), 0.35 N, and when the robot is moved to position B, the wrench would be + or - 2.2 N. If I perturb the end-effector a bit, it might reduce to a certain level. Can you understand why is this so? is it due to the stiffness model or friction in the joints? is KUKA controller is not properly compensating the gravity terms (meaning the wrench in Z should always be 0) ?

Im a bit confused in this. Wanna shed some light over this please ?

[matthias-mayr]

Have you noticed large variation (2 to 3 N) in the cartesian wrench (computed using Jacobian) when the robot moves in cartesian space?

I assume that you mean the measured wrench that was computed with the external joint torques and the Jacobian. Let's say that I am not surprised at all. From conversations with other iiwa owners and own experiences, the KUKA tool calibration leaves some space for improvements. E.g. our robots have an estimated link mass that is slightly too high and they start to lift when having 0 stiffness. There is also no zeroing option like with the UR FT sensor. However you could implement such a feature in the FT calculation and hope that the external torque errors are locally stable.

As written here, I talked to a guy from Stanford at the last IROS and he implemented his own calibration on top of KUKA's. He said that for his experiments this was necessary to get rid of movements when sending 0 torques.

I also know another person who was interested in the same issue and to whom I sent the Stanford guy's contact data. I haven't heard back since then, but I can connect you if you want.

[matthias-mayr]

I assume that this is answered. Feel free to re-open it if there's anything else.