Robot control task space controller

[kekeblom]

Implements a ros control integration for the task space controller. Have not tested this yet.

I based this off of the split_ros branch where the ros code was split up into a separate package. Maybe those changes should be merged first and I can the rebase this branch onto that one.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[kekeblom]

I managed to figure out what is wrong with the gazebo simulation. There were two issues. One has to do with the paths to the mesh files. They had a model:// prefix instead of package://<ros_package>. This was for the bullet simulation which has no understanding of ros packages. The second issue, is that colors are not used from the meshes. These had to be added to the urdf for gazebo to show the links as non-transparent.

The problem now, is that the model will no longer load in pybullet.

One solution I have in mind, would be to write a script which generates the bullet urdf from the gazebo one by replacing the paths, and add these generated files to .gitignore. This way, we only have one urdf to maintain. The downside being we need to run a script when first cloning the repository or when making changes to the transform script.

[grizzi]

Could we add that as an argument to the xacro and run xacro whenever loading the robot.

[kekeblom]

Yeah that's actually a good idea. I'll try to do that.

[ethzasl-jenkins]

Test FAILed.

[kekeblom]

added the parameter to the panda xacro. Problem is I'm now no longer able to run the python scripts to develop a processing step to run that also through xacro, as I can't install the packages. Will have to figure that out.

Btw, should we disable jenkins until we have time to fix it? That fail screaming butler is starting to enter my dreams :laughing:

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[kekeblom]

I timed the code, and it seems it is running super fast at least on my machine. I would have to go and time it on the actual platform to see if performance is actually the limiting factor.

It is quite stochastic though, which makes me think the actually limiting factor is scheduling and not computation itself.

[grizzi]

I was expecting it to be quite fast. Probably the issue is in the initialization rather than in the advance loop. There must be a switch from whatever default controller to the one we want to test. I can imagine this switch might happen in more than a ms, and trigger the error

[ethzasl-jenkins]

Test FAILed.

[grizzi]

Is this the latest version tested on the robot? If this is the case I would merge it

[kekeblom]

I didn't try the template magic yet, but I can do that today. I'll let you know if it runs.

[ethzasl-jenkins]

Test FAILed.

[ethzasl-jenkins]

Test FAILed.

[kekeblom]

I tried the controller on the real robot. And it runs.

I added a interactive marker so that the target can be set while the controller is running. It's not the prettiest thing, but it's better than nothing.

The marker works in simulation, but on the platform, it doesn't. Maybe we can merge it and figure it out in a separate pull request.

Here is what I found out so far. Let me know if you know what might be causing the problem.

The messages from rviz get published without any problems. I can verify this using rostopic echo /target_pose/feedback. It prints all the messages as they are set. However, the callback does not get called. If I run rostopic info /target_pose/feedback, the franka control node shows up as a subscriber, as it should.

I first thought this might be due to franka control not running a ros::spin() loop to process messages, so I tried spinning up an ros::AsyncSpinner. I then read the source code for the marker server and it has an option to spin it's own thread for processing messages. I tried that too, but to no effect.

I'm currently out of ideas, but maybe diving into the franka_ros source code could give some ideas.

[grizzi]

Can this be related to the network setup? the difference between robot and simulation is that rviz is running on the operator machine right? Additionally, it could be that the franka control node namespaces the control manager which in turns is namespacing the marker service callback? I havent seen the code, so I am just popping out stuff from my head. I will check the code soon and let you know if I find something.

EDIT: you said the franka control node shows as a subscriber, so the last point I mentioned should be ok.

[ethzasl-jenkins]

Test PASSed.

[kekeblom]

Could be related to the network setup.

I checked the namespaces as this was also one of my original ideas. I couldn't see it subscribing to another namespace. In that case, it would also not show up in rostopic info as a subscriber.

[grizzi]

I am trying to run this in simulation right now and I do not see the marker. Before I move forward, are all the changes you made puhsed here?

[kekeblom]

Yes they should be there. Can you see the marker in the rviz left panel?

[grizzi]

Yes I can see it!

[kekeblom]

I've seen a bug, where you might have to toggle the checkbox on the marker visibility before it becomes visible. Not sure what causes that. Additionally, maybe the marker is hard to see? Maybe we should make it red or something?

It also only becomes visible after the controller is loaded. Make sure to load the controller first.

[grizzi]

Yeah. I would also make the marker a complete 6d marker and initialize it at the current robot pose. Right now I am working in making the simulation more usable. I added a simple script to automatically unpause gazebo after some time to avoid the initial freaking behavior and I also noticed that when switching controller (from the trajectory following to the task space sim), the sim is not started which is weird. I am also debugging this. Calling the init controller service instead works but in the meantime the arm was not controlled and it freaks out again.

[ethzasl-jenkins]

Test PASSed.

[kekeblom]

Looks super good!

[grizzi]

If this looks good to you right now, I would merge. I see some modifications feature that we need but I will add them to the project.

[kekeblom]

Yeah, I think it's ready to merge. Tested it in gazebo and it works super well.