0
$\begingroup$

Rosanswers logo

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 of the robot arm via a scaling factor
  // of the maxiumum speed of each joint. Note this is not the speed of the end effector point.
  move_group.setMaxVelocityScalingFactor(0.1);

  // We want the Cartesian path to be interpolated at a resolution of 1 cm
  // which is why we will specify 0.01 as the max step in Cartesian
  // translation.  We will specify the jump threshold as 0.0, effectively disabling it.
  // Warning - disabling the jump threshold while operating real hardware can cause
  // large unpredictable motions of redundant joints and could be a safety issue

      moveit_msgs::RobotTrajectory trajectory;
      const double jump_threshold = 0.0;
      const double eef_step = 0.01;
      double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
      ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

  // Visualize the plan in RViz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

As you can see, x,y, and z coordinates have to be implemented by hand for each point.

Thanks in advance

Originally posted by Dan_escu on ROS Answers with karma: 11 on 2018-09-05

Post score: 1

Original comments

Comment by gvdhoorn on 2018-09-05:
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 ..

Comment by gvdhoorn on 2018-09-05:
.. 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.

Comment by Dan_escu on 2018-09-05:
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...

Comment by Dan_escu on 2018-09-05:
... 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?

Comment by gvdhoorn on 2018-09-06:
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 ..

Comment by fvd on 2018-09-07:
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.

Comment by gvdhoorn on 2018-09-07:
@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.

Comment by gvdhoorn on 2018-09-07:
re: related question: that is #q261368, which I already linked to in the first comment ;)

Comment by Dan_escu on 2018-09-07:
To use gvdhoorn's method, would a loop have to be implemented which reads each line and then applies each modification (create geometry_msgs, set x,y,z values)? I'm assuming this is necessary to avoid having to do it manually. Cheers

Comment by fvd on 2018-09-07:
Pretty much, yes: https://en.wikipedia.org/wiki/Foreach_loop

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

.. 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.

Originally posted by gvdhoorn with karma: 86574 on 2018-09-06

This answer was ACCEPTED on the original site

Post score: 2

Improve this answer
$\endgroup$

Your Answer

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