Rosanswers logo


I'm writing my own cartesian controller for my Powerball arm following this tutorials. After a few days working with it I get a correct control and now, my next step is get a correct movement without collisions (in a prefixed environment) and auto-collisions.

As you know, there is wizard in the arm_navigation package that allows to generate automatically the kinematics services and so on related with the arm movement. I executed it and generated the files with success. After a study of the generated files and the packages involved, I concluded that the correct way to avoid the collisions is the use of the "trajectory_filter_server" node. My idea is call this service (trajectory_filter_server/filter_trajectory_with_constraints) every time that I make a position increment, and obtain from this a answer containing if the movement is valid or not.

However, after several test, I haven't be able to make a correct call to the service. Anyone can tell me which are the necessary and essential parameters to correctly launch the service? I "only" filled the stamp, frame_id, trajectory.joint_names, trajectory.points, robot_state.start_state.name, robot_state.start_state.position and the group_name.

[Added 13/08/2012]

When debugging with GDB I observed that when I call the service it fails in the line 84 of the spline_smoother_utils.cpp ( spline_segment = spline.segments.back(); ).

  if(time >= segment_end_time)
    ROS_DEBUG("Did not find spline segment corresponding to input time: %f",time);
    segment_time = segment_end_time - segment_start_time;
    spline_segment = spline.segments.back();
    return true;

I think that all the necessaries services are launched because I get before the next output:

[INFO] [1344842134.839221386, 118.688000000]: Successfully connected to planning scene action server for /trajectory_filter_server

[INFO] [1344842134.846775320, 118.768000000]: waitForService: Service [/trajectory_filter_server/filter_trajectory_with_constraints] is now available.

[INFO] [1344842134.851041043, 118.799000000]: Sending Planning Scene....

[INFO] [1344842134.851441300, 118.813000000]: Reverting planning scene to default.

[INFO] [1344842134.856230667, 118.827000000]: Planning scene sent.

[INFO] [1344842134.859664450, 118.857000000]: Initialized

[INFO] [1344842134.859908300, 118.864000000]: Selecting planning group 0

[INFO] [1344842134.861566525, 118.864000000]: Planning group selected.

P.S: Do you think that my "idea" is correct or exist any node more appropriately for this purpose.

Originally posted by mbj on ROS Answers with karma: 197 on 2012-08-09

Post score: 1


1 Answer 1


Rosanswers logo

Hi there!

I don't know, how the trajectory filter ensures collision-free motions, but I recommend the use of the planning_environment for collision checking.

Have a look at the tutorials, e.g. "Checking collisions for a given robot state". But be warned, as noted in the overview section of planning_environment, major changes have been made for Electric. I think, the tutorial is only valid for Diamondback.

You could have a look at this piece of code. It should be a working example of collision checking under Electric.

In both the tutorial and the linked code collision checking is done for the robot state, which is defined by the joint states you need to provide. I think that should work for your use case (check the robot state after each increment).

Now, if you are working on Fuerte, I think the Electric version should work (or only minor changes have to be applied). At least that is my personal experience so far regarding changes in arm_navigation for Fuerte.

Originally posted by bit-pirate with karma: 2062 on 2012-08-13

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by mbj on 2012-08-14:
Hi bit-pirate. Thank you very much for the answer. It has given me a different view on how to solve the problem. However, I haven't solved it yet.

Comment by mbj on 2012-08-14:
Firstly, I tried to incorporate the code on the update() method of my controller, but I was obtaining always the same result: "[WARN] Incomplete robot state in setPlanningScene" & "[ERROR] Setting planning scene state to NULL".

Comment by mbj on 2012-08-14:
Worried about the update frequency was troubling the call of the service, I designed a simple program that only call the service once. The result is the same. Any idea? Thanks again.

Comment by bit-pirate on 2012-08-15:
Which piece of code generates the warning and error above? Please make sure that states for all joints of your robot are published (check the "/joint_states" topic for example).

Comment by bit-pirate on 2012-08-15:
Have a look at this discussion: http://answers.ros.org/question/40632/moving-custom-robot-arm-to-pose-goal/?comment=41547#comment-41547. Might be useful for you as well.

Comment by mbj on 2012-08-16:
Hi, I had seen those threads yesterday, however, thanks for the answers. : ) I didn't get any solution so I opened a new thread dedicated to the problem: http://answers.ros.org/question/41564/impossible-to-set-a-planningscene/.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.