Arm moving unexpectedly when starting Cartesian Compliance/force controllers

[mbs-tahir]

I have integrated an XARM6 with Cartesian Controllers and Robotiq Force Torque Sensor FT300. The problem I am facing is the arm is moving unexpectedly and sometimes crashing into the pallete. For this reason I have set smaller values for error_scale and PD. I have tried tuning the parameters alot but nothing helped this startup issue. I have recorded a some videos and a rosbag here. In controller folder is the initial behavior and in parameter tuning is the video for smaller values of PD and error_scale. The rosbag is for the same video i.e. parameter tuning. Any idea on how this problem can be solved? To me I think something is wrong with the force torque sensor values? May be I need to zero them at the start of controller for reference purpose? Any ideas will highly be appreciated.

[stefanscherzinger]

Hi @mbs-tahir

Thanks for sharing the videos! That's always helpful to get a feeling for the issue. I'm not completely sure why this happens.

May be I need to zero them at the start of controller for reference purpose?

Yes, definitely. I almost always experienced highly non-zero force-torque measurements on most robotic sensors when switching on the manipulator and I developed the habit to zero them as the first thing I do when activating any of the force-based cartesian_controllers.

It's a good Idea to combine a real sensor with the simulation for initial tests btw. Could you zero the sensor and report if the effect still happens?

For this reason I have set smaller values for error_scale and PD. I have tried tuning the parameters alot but nothing helped this startup issue

That's a little strange and speaks against high initial force-torque values because their effect would be strongly limited by decreasing the error_scale. You could also set error_scale = 0.0 (best via dynamic reconfigure in rqt) before switching on any of the controllers and see if it still happens.

[mbs-tahir]

Hi @stefanscherzinger

Thank you so much for your reply and helping out.

Could you zero the sensor and report if the effect still happens?

It worked!

Robotiq FT300 have a service, so after calling:

rosservice call /robotiq_ft_sensor_acc  "command_id: 8 command: ''" 

and

rosservice call /cartesian_compliance_controller/signal_taring "{}"

Than initiating the controller the robot never moves and it works fine.

I have one more question about controller behavior and configuration. When an external force is applied the robot reacts against this applied force and after force removal maintains its position. In my use case scenario the robot has to move the end effector on a surface which shouldn't be smooth. For example human arm touching from one end to another which can have different up and downs from one point to another.

Here in sensitive_touch you can see a video which eventually more or less is the desired the behavior. The end effector will be something soft like a brush etc.

So the robot behavior is fully opposite I want the robot to get back from an uneven terrain however the robot exerts force on that. Can you give some points by following which I can have this expected behavior?

[stefanscherzinger]

Hi @mbs-tahir

It worked!

I'm glad to hear that!

Can you give some points by following which I can have this expected behavior?

The sensitive touch use case that you describe is certainly challenging with an industrial robot. The force-torque measurements will suffice, but the robot control frequency is relatively low and will require slow motion for stability and safety reasons. Moving up and down a human arm sounds to me like following a path compliantly and being especially compliant in the orthogonal (pushing downwards to the human arm) dimension. You could try setting a low stiffness in that direction and apply an additional pushing force downward of few newtons, for instance z = 10 N. You'll need some careful testing, but I'm excited to sew how that goes!

I want the robot to get back from an uneven terrain however the robot exerts force on that.

That should be doable with a low stiffness in that direction and an additional downward force as described above. Force reference setpoints are just overplayed with the virtual spring. If you have a low virtual stiffness, the applied target forces will compensate for the uneven terrain. The challenge is that this will work relatively slowly, though. You might also want to mitigate that the end-effector gets stuck somehow in the tissue of the sweater or similar by designing a suitable brush (no sharp edges, possibly a round underside?).

[mbs-tahir]

Hi @stefanscherzinger

Apologies for late reply as I was trying to plan trajectory and move the arm because with cartesian_controllers the robot moves directly the position with a Jerk. Although I was able to do so but trajectory tracking was taking some time so started with MOVEIT integration. I belive this should work. As described here the controller namespaces are quite important. So I setup the robot in same way. But it didn't worked. I have recorded a video here in moveit_integration/xarm_moveit_self_collision you can see a video where a normal operation of the robot can be seen first. Secondly by setting up the joint_to_cartesian_controller, joint_to_cartesian_controller/joint_trajectory_controller and at the end cartesian_compliance_controller results in resource error.

Can you guide where the configuration is not set up accordingly?

What I understand from these three controllers is as follows(Please correct me if I am wrong):

• joint_to_cartesian_controller will be a bridge between move_group and joint_to_cartesian_controller/joint_trajectory_controller. It will output the relevant information to cartesian_compliance_controller which in the end should move the robot.

Thank you so much for your help in the issues.

Edit 01

After checking the controllers and removing redundant ones and taking care of namespaces I was able to launch all the controllers. This time while joint_trajectory_controller was up, starting cartesian_compliance_controller didn't make any issue. But when I start cartesian_compliance_controller the robot goes into self collision mode. After investigating a little bit I saw the output topic(target_frame) of joint_to_cartesian_controller which should be subscribed by cartesian_compliance_controller is publishing zero all the time. Due to which the robot is going to self collision as zero is the robot base and this state can't be achieved.

I also tested target_frame without starting cartesian_compliance_controller with plans from MoveIt which should be reflected in this topic but it still is zero.

Can you please share any insights on this issue on what I possibly could be doing wrong?

Launch and yaml file along with the recorded video can be found here on the same link inside moveit_integration/xarm_moveit_self_collision.

Edit 02

After playing a little bit with the controllers I have got things working a little bit but still having some issues. Here are the details.

Actually joint_to_cartesian_controller initially is publishing all zeros by default. So this controllers has to unload, reload and start(Only start and stop doesn't help). In this way correct TCP pose is published by this controller. Because joint_to_cartesian_controller is reloaded and started so joint_to_cartesian_controller/joint_trajectory_controller gets unloaded so I need to reload and start joint_to_cartesian_controller/joint_trajectory_controller again. After that at the end I load and start cartesian_compliance_controller with which things seems to work. I set all this up with MoveIt and it seems to work a little bit at-least the robot moves.

Now coming to the problem: In setting up, loading and unloading controllers sometimes joint_to_cartesian_controller I think gets reset and starts publishing zeros and in case this happens while starting cartesian_compliance_controller the robot goes to self collision in order to move to zero position. The other thing which also happened is while the normal operation was going on. I mean the robot was working with MoveIt after completion of a goal the robot goes to self collision because of zero publishing from joint_to_cartesian_controller. I have spent quite some time on it but it still persists and I don't have any clue so any insights on this issue will be really appreciated.

Apologies for so much questions and being annoying.

[stefanscherzinger]

Hi @mbs-tahir Thanks for posting your progress on this!

It's some time that I have used this feature (turning joint trajectories into a moving target for the cartesian_controllers). I remember, though, that they were not entirely robust towards arbitrary controller changes. I ran into freezes sometimes and wanted to fix this in #41 .

Concerning the zeros that get published in the target frame from time to time: That's definitely incorrect behavior and a severe safety risk. I want to fix this, but need to reproduce this for debugging in a simple setup. Thanks to @matthias-mayr for spotting the dangling robot chain. Maybe that's somehow related.

@mbs-tahir Could you try again on the improve moveit compatibility branch?

I'll rethink this architecture. The joint_to_cartesian_controller should always publish the current robot pose as default. Something seems to initialize incorrectly.

---

Edit: In case you use Moveit just for creating some motion and do not require real planning, maybe this feature is interesting for you? You can easily specify Cartesian trajectories and publish the moving target as geometry_msgs/PoseStamped for the cartesian_controllers.

[stefanscherzinger]

Update: The joint_to_cartesian_controller already updates its internal representation in starting, so the published target should be valid, provided the robot driver fills those buffers with meaningful values.

I'll try to reproduce the publishing of zeros somehow.

Maybe also #78 is related to this issue.

[stefanscherzinger]

@mbs-tahir

You might try again on the improve-moveit-compatibility branch. I rebased onto #78.

[mbs-tahir]

Hi @stefanscherzinger

Apologies for the late reply as I was trying to solve the problem and tune the paramteres. I did tried with #78 but it didn't solved the issue. Than I tried rebuilding the code with #41 and it worked out. Now the real TCP points are published insteas of zero's at the start.

The sensitive touch use case that you describe is certainly challenging with an industrial robot. The force-torque measurements will suffice, but the robot control frequency is relatively low and will require slow motion for stability and safety reasons. Moving up and down a human arm sounds to me like following a path compliantly and being especially compliant in the orthogonal (pushing downwards to the human arm) dimension. You could try setting a low stiffness in that direction and apply an additional pushing force downward of few newtons, for instance z = 10 N. You'll need some careful testing, but I'm excited to sew how that goes!

That should be doable with a low stiffness in that direction and an additional downward force as described above. Force reference setpoints are just overplayed with the virtual spring. If you have a low virtual stiffness, the applied target forces will compensate for the uneven terrain. The challenge is that this will work relatively slowly, though. You might also want to mitigate that the end-effector gets stuck somehow in the tissue of the sweater or similar by designing a suitable brush (no sharp edges, possibly a round underside?).

Now things seems to work. So I will tune the parameters and integrate the above mentioned comments and update you.

Once again thank you so much for the great work and continuous support.

[stefanscherzinger]

@mbs-tahir

I tried rebuilding the code with https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/pull/41 and it worked out. Now the real TCP points are published insteas of zero's at the start.

Great! Then I guess it was the dangling reference fix.

Once again thank you so much for the great work and continuous support.

You're welcome! :)

[mbs-tahir]

You could try setting a low stiffness in that direction and apply an additional pushing force downward of few newtons, for instance z = 10 N.

@stefanscherzinger I tried setting low stiffness and published a constant push in upward direction but still its not that compliant. Even I tried with 1N force in z-axis. Just to confirm I need to publish to cartesian_compliance_controller/target_wrench right?

Moreover to add here I am not planning in the frame of ft_sensor but in link_eef but they both are same. For reference I have recorded a bag file and a video which can be found here in controller_parameter_tuning. Any comments/guidelines will highly be appreciated.

[stefanscherzinger]

I need to publish to cartesian_compliance_controller/target_wrench right?

I'm not sure of your current remappings. If you didn't remap anything, then it looks correct. You could do a rostopic list | grep target_wrench and check how many target_wrench are there and if they are connected.

but still its not that compliant.

Your error_scale seems very low. Could you carefully increase that a little bit? Your system could be much more responsive, the only limiting factor would be oscillations in contact. Unfortunately, that's a classic trade-of. You might also lower your trans_x and trans_y stiffness a little bit. The rotational stiffness is also quite high.

I updated the exemplary compliance controller config yesterday after testing a little on a UR5e (it's for ROS2, but that should be similar). Maybe you want to try that. But start with your small error_scale = 0.02 and gradually increase that because that's hugely different from robot to robot.

[mbs-tahir]

Thanks fort he prompt response. I got the point and will try that suggestions. One more thing which I noticed is whenever the controllers starts and external force is published cartesian_compliance_controller/target_wrench the robot moves a little bit in the direction of motion either upward if positive negative force in z-axis or downward in the other case. Is it normal behavior if so can you please explain why does this happen?

[stefanscherzinger]

Is it normal behavior if so can you please explain why does this happen?

Yes, that's normal behavior. The compliance controller computes the overall net force, consisting of target_wrench, ft_sensor_wrench and wrenches induced by offsets to the target_pose. This behavior is a design choice. I like its simplicity and the possibility to do some force control in directions orthogonal to the nominal motion. The fact that the controller's stiffness balances the target_wrench until an equilibrium is established, is fine for most use cases.