Hi Chris thank you for sharing your project!!

Actually is really simple thanks to you. the demo from moveit already has a controller. If you update random goal and click on plane and execute button, moveit will publish all the joint positions on a topic called “/movegroup/fake_trajectorycontroller .

I just write a simple python subscriber to receive data from the topic and send command to the Arduino.

In a terminal I launch the demo.launch file from ar2_moveit_config package

In an other terminal I launch my script:

-load all the global variables you define in your source file

-load the serial connection

-calibrate all motors with calRobotAll() function

-move in home position (the same as you can see in moveit)

Then I have a simple loop and I read joint values from the fake trajectory topic, convert them from radian to degree and send them to the Arduino through your MoveNew() function.

It is important to update joint limits in the Urdf file and modified negangllim posinglimit global variables in order to match the real AR2 position and the moviet AR2 position.

I can make a tutorial and share it with my code!

It’s really easy to run if you have ros install

Hi! Thank you for your interest. Concerning the tuto and the code I have not finished yet, I need more time to write the tutorial and finished ros_control integration. I would prefer release clean code. But if you give me an email adress I can send you the code and the first version of the tutorial. Your feedback will be very usefull to me.