Using MoveIt! to follow a path based on a set of Cartesian coordinates

asked 2018-09-05 06:46:23 -0500

Dan_escu gravatar image

Hi,

I have created a 3D path on MATLAB and have exported the x,y and z coordinates into a .txt file after performing an animated plot. This file contains three columns (one for each set of coordinates) where each column contains the coordinates in the appropriate order to be able to plot this continuous path.

Even though MoveIt! is generally used for planning a path from point A to B, my aim is to import this path which I have already generated so that MoveIt! simply has to follow this path. The robot controller I will be using has similar specs to the Panda robot used in the MoveIt! tutorials, which is one of the reasons why I'm set on using this software, aswell as the ease of use and straightforward integration with ROS (I'm a beginner).

The Cartesian path planner present in the Move Group C++ interface seems to be the closest method I could find to do this, however when using this planner each waypoint has to be inserted by hand by editing the move_group_interface_tutorial.cpp file. This is far from ideal as the path I want to use has ~1000 points. So I'm wondering if there's some sort of way of importing this .txt file into MoveIt! and then have it follow this path using the Panda robot. This seems like a very straightforward thing to do which is why I'm confused as to how I haven't found a solution to this issue after searching through every feature of MoveIt!.

If this can't be done with a .txt file then I would appreciate knowing if it can be done with any other kind of file containing a set of ordered coordinates (which can be exported from MATLAB). If this cannot be done at all then I'm open to any other method for doing something like this, or if there's some sort of function I could use in the .cpp file to scan through each point in the coordinate file.

I'm using ROS Kinetic on Ubuntu 16.04.5.

Here is the code for computing a Cartesian path with the Move Group interface (isolated from the tutorial):

// Cartesian Paths

  // ^^^^^^^^^^^^^^^
  // You can plan a Cartesian path directly by specifying a list of waypoints
  // for the end-effector to go through. Note that we are starting
  // from the new start state above.  The initial pose (start state) does not
  // need to be added to the waypoint list but adding it can help with visualizations
  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(start_pose2);

  geometry_msgs::Pose target_pose3 = start_pose2;

  target_pose3.position.z -= 0.2;
  waypoints.push_back(target_pose3);  // down

  target_pose3.position.y -= 0.2;
  waypoints.push_back(target_pose3);  // right

  target_pose3.position.z += 0.2;
  target_pose3.position.y += 0.2;
  target_pose3.position.x -= 0.2;
  waypoints.push_back(target_pose3);  // up and left

  // Cartesian motions are frequently needed to be slower for actions such as approach and retreat
  // grasp motions. Here we demonstrate how to reduce the speed ...
(more)
edit retag flag offensive close merge delete

Comments

1

Related question: #q261368.

This is all possible,but:

This seems like a very straightforward thing to do which is why I'm confused as to how I haven't found a solution to this

MoveIt is a software framework, not a user-oriented robot programming application. It can be done, but ..

gvdhoorn gravatar image gvdhoorn  ( 2018-09-05 06:50:48 -0500 )edit
1

.. you'll have to open the file in your own code, read the contents, convert it to the representation expected by computeCartesianPath(..), feed it to computeCartesianPath(..) and then request execution the result.

Opening files and reading them is not necessarily MoveIt specific though.

gvdhoorn gravatar image gvdhoorn  ( 2018-09-05 06:52:04 -0500 )edit

Hi, thanks for the quick response, I found the related question before but the answer didn't go into as much detail as I needed, hence my question. Could you be a bit more specific about how this can be done ie. what the representation that computeCartesianPath(..) expects is...

Dan_escu gravatar image Dan_escu  ( 2018-09-05 07:09:12 -0500 )edit

... How to 'feed' it to computeCartesianPath(..) etc. I have very little experience in coding so this may be an issue. If you feel like this is out of topic (more coding related) then do you know where I could find any guides to doing this?

Dan_escu gravatar image Dan_escu  ( 2018-09-05 07:11:54 -0500 )edit

I do feel this is somewhat out-of-scope for this forum, as most of it comes down to reading from your text file.

The representation that computeCartesianPath(..) expects is exactly the same as it is in the example code you show: std::vector<geometry_msgs::Pose>.

'feeding it' is also just ..

gvdhoorn gravatar image gvdhoorn  ( 2018-09-06 01:41:28 -0500 )edit
1

.. calling move_group.computeCartesianPath(..) as you are already doing.

My suggestion:

  1. open the file
  2. read line by line
  3. for each line:
    1. create a geometry_msgs::Pose
    2. set it's x, y and z to the values from the current line
    3. add it to the vector

done.

gvdhoorn gravatar image gvdhoorn  ( 2018-09-06 01:42:55 -0500 )edit

Related question. I would add that it should be possible to plan each motion and then concatenate the trajectories and retime them. But that's worth a pull request, in my opinion.

fvd gravatar image fvd  ( 2018-09-07 07:50:00 -0500 )edit

@fvd: of course. That's certainly possible. However, you would still give MoveIt hundreds of mini-trajectories to plan for. Besides not being very efficient, I'd be interested to see what the "quality" of the resulting cumulative trajectory is, instead of planning through a single one.

gvdhoorn gravatar image gvdhoorn  ( 2018-09-07 08:11:30 -0500 )edit