Planning and Executing movement on real robot via Move Group Interface

[Nicholas Adrian]

I am trying to control a 6 DOF robotic arm with stepper motor actuator controlled with arduino. So far I have managed to simulate the arm and move it in rviz by giving the end coordinate using move group interface. But i do not know how to link this simulated movement to a real robot movement.

I've seen a lot of documentation where people drag the simulated arm around in rviz and click on "plan and execute" to move the real robot. However, since I'd like the robot to move to a certain coordinate, I use move group interface to do so instead of manually dragging the arm in simulation. But by doing so, I'm unable to use the "plan and execute" button.

My understanding of ROS is still far from perfect , thus I'd appreciate it if anyone can help to point me in the right direction.

[Shawn Schaerer]

Drag and then plan and execute works for my custom robot.

not sure why this is not working for you.

Can you get the robot to move to pre-defined positions using MoveIt + RVIZ?

[William Lehman]

I have a custom robot using Arduino and 3 servo motors to control the arm from Moveit and ROS.  I wrote a simple node and used ROS serial to connect it to Moveit or manual control with sliders in ROS.  The plan and execute work fine.  The interface can support up to 12 joints.  I did not have to change Moveit.  Are you using ROS serial?

[Nicholas Adrian]

Drag + plan works for my simulated robot (I have not finished building the physical robot arm). But what I would like to do eventually with the real arm is to give an end/target coordinate and then plan and execute it instead of dragging and then plan and execute it. So the question is how do I get moveit to register an input (end coordinate) as the end state in rviz so that I can eventually plan and execute to the real robot arm?

[Nicholas Adrian]

I understand that rosserial is for letting the arduino communicate in the ROS system (publish and subscribe). How does your node work? Does it subscribe to an input coordinate and then publish the required angle to the arduino?

[William Lehman]

The Fake Robot Controller in Moveit sends out Joint States (angles of joints) and the node receives these and converts them to Brazen Messages which are sent ROS serial. Rosserial sends the Brazen message to the Ardunio.  The ardunio is a node as well and takes the joint angle from the Brazen Message and sets motor to that angle.  In my case it is a RC Servo Motor in your case a Stepper Motor.

I have a post Mar 3 DIY Robot Arm........

Link to post:

https://groups.google.com/forum/?utm_source=digest&utm_medium=email/#!topic/moveit-users/1GBTgAJLYpQ

If you email me at tec.teach...@gmail.com I can send you the associated files/source for Ardunio and the message conversion node.

[Nicholas Adrian]

So if i understand your explanation correctly, after you drag the simulated robot in rviz, moveit will calculate the required angle. Then after clicking 'plan and execute', fake robot controller will publish Join States topic. And we can then send this Joint State to arduino.

Now what I'm trying to do is to publish an end coordinate --> let moveit do the calculation (and maybe show it on rviz) --> plan and execute to my robot. How do I do this exactly? Because while I can get the robot to simulate the movement to the end coordinate, the simulated robot doesnt stay at the end state and thus I cannot 'plan and execute'.

[Shawn Schaerer]

If you want to have the joint values sent down to your arduino, then take a look at ROS Control, specifically ROS Control boilerplate (a code template that you use to code your robot).

MoveIt would talk to ROSControl and it would communicate with your robot.

[Acorn Pooley]

I think what you are looking for is the move group interface. It is a programming interface that lets you do from a program what you are now able to do from rviz: define a goal Cartesian pose and have MoveIt generate a joint trajectory and send it to the robot. The move group interface is available in c++ and python. This is what you use to tell MoveIt what to do. (This is also what the rviz plugin uses to tell MoveIt what to do.)

Telling MoveIt how to control your robot is what the other emails in this thread are talking about: ROS serial, fake robot controller, ROS control, etc

Cheers
Acorn

[Nicholas Adrian]

@shawn thank you for this! I keep hearing about ros control and for the longest time have been confused as to how ros control play a part in the system. I'll look into this

[Nicholas Adrian]

@acorn I actually am implementing move group interface and have been able to subscribe to a Cartesian goal topic. MoveIt generated the trajectory successfully. So I guess the next step for me is to implement ROS control to pass the joint states to arduino through rosserial (as mentioned by shawn)?

[Shawn Schaerer]

post your questions, I have been working with ROS Control and MoveIt for the last year so I have a good handle on it

[Nicholas Adrian]

I read somewhere that once Moveit computes a path for you, it will publish JointTrajectory to FollowJointTrajectory action server.

Is this another alternative to using ros control or are they the same thing? What is FollowJointTrajectory action server exactly? Also, are JointState and JointTrajectory the same thing?

On 2 April 2016 at 08:59, Acorn Pooley <ac...@bcorn.com> wrote:

Yup.

Or if you don't need that much complexity check out the FakeRobotController which sends joint trajectory messages. Write a node (ROS program) that receives those messages and moves your robot to the requested position.

[Acorn Pooley]

Joint State is, by convention, where the robot is now (the current state of the robot). The robot controller publishes this to tell MoveIt where the robot is. Robot State Publisher uses it to publish tf frames which allow rviz to display the robot in its current pose.

Follow Joint Trajectory is the command telling the robot where to go. This is publisher by the fake robot controller in MoveIt.

There are (at least) 2 ways to write a robot controller:

1
You can write a robot controller which receives the FollowJointTrajectory messages and moves the robot to that location, and publishes the Joint State message to tell everyone where the robot is now. MoveIt will use the fake robot controller to publish these FollowJointTrajectory messages which your controller subscribes to.

2
You can replace the fake robot controller in MoveIt with your own plugin which controls the robot.

The decision is up to you. Is you want to use ROS messages and don't want to write a plugin then do 1. If you want to write your controller as a plugin, or want to communicate to your controller using done mechanism other than ROS messages (or use a different ROS message) then do 2. It is probably simpler to do 1 for a simple robot. For  high performance robots that expect to receive commands at a very regular high rate (e.g 1kHz) there are reasons to prefer 2 (This is also something ROS control is designed to help with).

Acorn

[William Lehman]

Hi Great Information from Shawn and Acorn!  I did method 1 and modified Fake Robot Controller Send Trajectory routine to send a Brazen message to ROS Serial and the Ardunio (node).  I forgot to send it too you.  Advantage of using Fake Robot Controller was I could delay slightly the trajectory going to the Robot to slow trajectory down and not have robot move too fast.  You can to get your ardunio side worked out first use conversion node and rosserial with Moveit unmodified fake robot controller to test ardunio node.  Of course my robot is so simple it is one way communication to command the motors with no feedback to PC needed.

The conversion node is good to use with ROS to calibrate your robot with Joint State Publisher with GUI set to true.  That way you can use sliders.  All robot arms have errors and custom ones the errors are unknown.

The reason I keep mentioning brazen message is messages discussed so far use float64 for joint states and ardunio with rosserial only supports float32.  My solution was to write a custom message for ardunio on both ROS side of fence for nodes running on PC and on ardunio side of fence.

Note if you choose to use rosserial with ROS control the message issue will still exist.  I am updating code this weekend and perhaps monday to use floats.  You may be done before that.  In any case I will let you know when I made the change to the message, arduino node, fake robot controller manager.  If you need it I will put it in a git repository so you get the whole package.  Don't know what I was thinking using email.  Old guy from punch card days.

Best to all of you --- GREAT posts!

[Nicholas Adrian]

My move group interface (attached) contains the pose goal for the arm. When I run it, I can see the movement being visualised on Rviz. However i cant find a topic which contains the information on the required joint state for my robot arm. I checked /move_group/fake_controller_joint_state as you have suggested, but nothing was printed on the terminal. I have also attached the rqt_graph after running the move group interface. What am I missing?

[William Lehman]

Did you monitor /joint_states in rqt?  Fake controller generates publishes /joint_states topic when it gets a trajectory.

[William Lehman]

I was incorrect.  Don't use rqt. I should of suggested checking with this command:

 rostopic echo /move_group/fake_controller_joint_states

It should show something if the fake controller received a trajectory.  Sendtrajectory is where the moveit_fake_controller_manager.cpp publishes to the /move_group/fake_controller_joint_state.

[Nicholas Adrian]

I already did that to check /move_group/fake_controller_joint_states and /joint_states, but there is nothing on both

[William Lehman]

If you are run demo.launch software you can plan a robots motion which will show the robot moving to the goal position but that is just planned.

To see /move_group/fake_controller_joint_states you need to press the Execute button in the demo software to actually move the robot to that goal position.

To do this with code see Move_group_interface (C++) tutorial:

Moving to a pose goal

Moving to a pose goal is similar to the step above except we now use the move() function. Note that the pose goal we had set earlier is still active and so the robot will try to move to that goal. We will not use that function in this tutorial since it is a blocking function and requires a controller to be active and report success on execution of a trajectory.

/* Uncomment below line when working with a real robot*/
/* group.move() */

[Nicholas Adrian]

It worked now! Thank you! I was looking at the rqt_graph (attached) and I'm particularly curious at something: what did /move_group_interface publish to /move_group so that /move_group can publish /move_group/fake_controller_joint_state? 

[Spyros_G]

Hello everyone,

I have already moved the simulated robot in Gazebo. In that case, if I want to move the actual robot, what do i need to do exactly ?

If I understand how this works correctly, I would guess that I have to subscribe to a topic which contains the effort output, send this to my robot for execution, and publish the joint state values I receive from the encoders to the appropriate topic.

I am already using the joint_trajectory_controller implementing the FollowJointTrajectory action. But it is unclear to me what is the next step.

Thank you all!

[William Lehman]

The answer to your question depends upon the type of hardware and robot you have.  You need a interface from the PC to the robot controller hardware.  The robot controller is connected to the actuators and sensors on you robot.   I created my own very simple robot and robot controller using a serial interface to the PC. What do you have in the way of robot and robot controller?

[9492...@qq.com]

在 2016年4月2日星期六 UTC+8上午1:02:01，Shawn Schaerer写道：

Hello , Shawn.I am a student from china ,I have just learned ROS-I and  have a abb-irb120 robot. I learn that the moveit can plan the path of robot in ROS. Are there other manual to learn moveit? Now , I learn MOVEIT from these website---http://moveit.ros.org/documentation/tutorials/ .  The API DOC is a good manual ,but I can't understand how to use it.  Thank you very much!

[Spyros_G]

Hello William,

thanks for the reply. I already have a node that can subscribe to a topic, receive effort commands and send them to my hardware for execution. The same node can receive joint positions from the encoders and publish them to another topic. Is that what you mean by an "interface from the PC to the robot controller hardware"?

[William Lehman]

You also need a robot controller for moveit like ROS Control, Simple Robot Controller or Fake Robot Controller.  You will have to write change/write code for any of these options.  I used the Fake Robot Controller but my robot was as simple as you can get with no sensors on the robot.  If you go back through this thread this is discussed with pro and cons.  The processing in your node can also be moved to the Robot Controller code to make system more simple.

[Shawn Schärer]

I used ROS control and ROS control boilerplate as a starting point.

That simplified my development.

I would try that route

Sent from my iPhone

[Spyros_G]

I already have ros_control integrated inside gazebo. I also use joint_trajectory_controller. Just to be clear, when you say controller you mean a "node" that reads an error in general and produces an effort command ? Because I think I might have misunderstood something there.

Again, thank you all for the advices.

[William Lehman]

Have you tried to move the robot in demo software or with move_group_interface and it moves in simulator but not real robot?

[Spyros_G]

Hello,

yes the robot moves perfectly in gazebo, but not in real hardware. But that might be because I have not yet found the right topic to get the effort command and send it to the robot. Does it sound like it should be moving the way I am describing it ? Or the whole method is mistaken ?

[William Lehman]

Sounds like you are on the right path but need to isolate why the actual robot is not moving.  This brings to mind many possibilities.  Some simple things to check off the list:

your roslaunch has correct parameters and topics.  Example fake_execution parameter should be false since you are not using the fake robot controller.  Also I imagine you need a config file for your controller like fake_controllers.yaml.  Actually you might consider using demo launch for test purposes only.

in the fake controller it is a routine called send_trajectory where you want to send commands to your robot to make it move.  I imagine you have a send_trajectory or equivalent in ros_control?  Might consider putting a command in your send_trajectory routine like     ROS_INFO("WE ARE DOING IT"); so you can see it on terminal when you do a "go" in moveit group or execute in demo software.

Just a note you are doing a "go" or execute button to make actual robot move since planning will also move robot in simulator.

If none of this helps feel free to ask other questions.  I have found in many years of work even the best technical people benefit bouncing problems off other people.

[Spyros_G]

The fake execution is disabled like you mentioned. The config file for my controller is also set up.

I am using either the execute button in the rviz plugin or the  group.move() command when using a script to set the desired pose. Is there any difference ?

Once again thank you for your help, it is much appreciated.

[William Lehman]

See post apr 10 of this thread there is a difference. Execute does a go()

[Spyros_G]

Hello William,

sorry for the late response, I was working on a different project for the last month. Your suggestions, always appreciated.

I managed to move the robot pretty simply, by taking the effort commands from the /joint_states topic and sending them to the actuator, like I mentioned above.

The movement however, is not similar with the one simulated and I have located some potential problems:

1) There is no position feedback from the real robot to moveit. This is a problem that I have not yet been able to resolve, how am I going to send the encoder position of the real robot back to moveit instead of the position feedback that is currently being sent back by the simulated robot in Gazebo.

2) I also have the feeling that I have misunderstood the function of the FollowJointTrajectory controller, and I have not found much information on what it does exactly. Could it be used as a PID controller for example ? I am guessing not, but I would appreciate a clarification on its actual purpose.

Kind regards
Spyros

[William Lehman]

The code I used was for a simple robot that did not need to send feedback, there were no encoders.  So I used a topic to send trajectory to robot over ros serial to ardunio.

But I am writing a version now where I use a service call to the ardunio from the ros controller to ros serial and the ardunio.  Unfortunately I am still working on that. Doing a service call is similar to doing topic.  There are two serializes and deserializes on in the ros_lib message side of the house.  One for the request and the other for the response.  I will try and get that done quickly but it might l a day or two.

If your in a rush you use brazenbot.msg and service call example in ros_lib for guidance.  You can also bypass ros altogethere by doing direct read and writes from serial line.  Best regards

[Spyros_G]

I finally achieved it by modifying the existing joint_states topic. I restructured the message adding the position from the encoders rather than the position from the simulation. Your suggestion were very useful though.

Any ideas on my second point above?

Thank you very much!

[Nicholas Adrian]

May I know how is your setup like? How many joints does your robot arm has? What kind of actuator are you using? And are you using arduino to operate the actuators?

The reason I'm asking is because I'm having problem sending /fake_controller_joint_states of 6 joints to my arduino through rosserial

[Spyros_G]

I am working with a single joint just to get familiar with the software. Also I have never used rosserial so I can not tell you if this is where the problem lies.

I am not using arduino because it was rather complicated to read the encoder input. I use another mc called Tiva and I send the position via UDP to ROS.