Cartesian Path Planning - fontysrobotics/ARMinor-2020-Opensource-Virtual-Learning-Environment-AROVLE GitHub Wiki

In this tutorial we will create a C++ package which will allow us to plan a cartesian path for the manipulator using an array of waypoints that will form this path. To store the points we will use a JSON file. Using a Json file will allow us to change the path without having to change the code or recompile it, which will form a kind of interface between our package and a third party application.

Create a catkin package called mov with roscpp dependency then edit the package.xml file and add the following:

<buildtool_depend>catkin</buildtool_depend>

<buildtool_depend>roscpp</buildtool_depend>

<build_depend>pluginlib</build_depend>

<build_depend>eigen</build_depend>

<build_depend>moveit_core</build_depend>

<build_depend>moveit_ros_planning</build_depend>

<build_depend>moveit_ros_planning_interface</build_depend>

<build_depend>moveit_ros_perception</build_depend>

<build_depend>interactive_markers</build_depend>

<build_depend>geometric_shapes</build_depend>

<build_depend>moveit_visual_tools</build_depend>

<build_depend>pcl_ros</build_depend>

<build_depend>pcl_conversions</build_depend>

<build_depend>rosbag</build_depend>

<build_depend>tf2_ros</build_depend>

<build_depend>tf2_eigen</build_depend>

<build_depend>tf2_geometry_msgs</build_depend>

<exec_depend>pluginlib</exec_depend>

<exec_depend>moveit_core</exec_depend>

<exec_depend>moveit_commander</exec_depend>

<exec_depend>moveit_fake_controller_manager</exec_depend>

<exec_depend>moveit_ros_planning_interface</exec_depend>

<exec_depend>moveit_ros_perception</exec_depend>

<exec_depend>interactive_markers</exec_depend>

<exec_depend>moveit_visual_tools</exec_depend>

<exec_depend>joy</exec_depend>

<exec_depend>pcl_ros</exec_depend>

<exec_depend>pcl_conversions</exec_depend>

<exec_depend>rosbag</exec_depend>

<exec_depend>tf2_ros</exec_depend>

<exec_depend>tf2_eigen</exec_depend>

<exec_depend>tf2_geometry_msgs</exec_depend>

Then edit the cmakelists.txt file by adding the following:

find_package(catkin REQUIRED

COMPONENTS

interactive_markers

moveit_core

moveit_visual_tools

moveit_ros_planning

moveit_ros_planning_interface

moveit_ros_perception

pluginlib

geometric_shapes

pcl_ros

pcl_conversions

rosbag

tf2_ros

tf2_eigen

tf2_geometry_msgs

)

find_package(Eigen3 REQUIRED)

find_package(Boost REQUIRED system filesystem date_time thread)

set(THIS_PACKAGE_INCLUDE_DIRS

doc/interactivity/include )

catkin_package(

LIBRARIES

INCLUDE_DIRS

CATKIN_DEPENDS

moveit_core

moveit_visual_tools

moveit_ros_planning_interface

interactive_markers

tf2_geometry_msgs

DEPENDS

EIGEN3

)

Create a c++ file in the package src directory and call it move.cpp

Last step creating a launch file with the following content:

<launch>

<node name="move_group_interface_tutorial" pkg="mov" type="move" respawn="false" output="screen">

</node>

</launch>

There are many different available libraries for C++ to parse JSON files. In this tutorial we will be using jsoncpp.so which is available by default in linux so it's not required to install or compile the library. In our example we'll be reading a premade JSON file. Inorder to setup the part of the code related to JSON we have to include the following libraries:

#include <iostream>

#include <jsoncpp/json/value.h>

#include <jsoncpp/json/json.h>

#include <fstream>

Then we will declare some variables to be used to read the poses from the file: A temp variable to hold the JSON values and parse them, then an array to store the parsed values.

geometry_msgs::Pose tempPose;

std::vector<geometry_msgs::Pose> waypoints;

A Json reader, value and an std::ifstream variable to read the file.

Json::Reader reader;

Json::Value root;

std::ifstream file("/home/i/catkin_ws/src/mov/src/points.json");

Then we will copy the file content to JSON value:

file >> root;

And finally we will create a loop to read all the objects in JSON file and parse them to C++ double values then store all those positions in an array which will be used later.

for (int i = 0; i < root.size(); i++) { //parse tempPose tempPose.position.x = root[i]["px"].asDouble(); tempPose.position.y = root[i]["py"].asDouble(); tempPose.position.z = root[i]["pz"].asDouble(); tempPose.orientation.x = root[i]["ox"].asDouble(); tempPose.orientation.y = root[i]["oy"].asDouble(); tempPose.orientation.z = root[i]["oz"].asDouble();

waypoints.push_back(tempPose); }

10.3 Cartesian path planning: To use the C++ interface include the following libraries:

#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit_msgs/DisplayRobotState.h> #include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/AttachedCollisionObject.h> #include <moveit_msgs/CollisionObject.h> #include <moveit_visual_tools/moveit_visual_tools.h> #include <rviz_visual_tools/rviz_visual_tools.h>

Then start the regular ros setup:

ros::init(argc, argv, "moveit_robot"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); spinner.start();

Initialize move group and visual tools:

rviz_visual_tools::RvizVisualToolsPtr visual_tools_;

static const std::string PLANNING_GROUP = "arm";

moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

const robot_state::JointModelGroup *joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("base_footprint"); visual_tools.deleteAllMarkers(); visual_tools_.reset(new rviz_visual_tools::RvizVisualTools("base_footprint", "/rviz_visual_markers"));

Then we parse the JSON file as we saw in the previous section Then create a trajectory to store the path in.

moveit_msgs::RobotTrajectory trajectory;

Create some variables to define the cartesian path constraints:

const double jump_threshold = 0.0; const double eef_step = 0.01;

Call the computeCartesianPath and pass the variables created earlier then use viausl_tools to visualize the results in rviz:

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);

visual_tools.publishPath(waypoints, rvt::RED, rvt::SMALL); for (std::size_t i = 0; i < waypoints.size(); ++i) { visual_tools.publishZArrow(waypoints[i], rviz_visual_tools::BLUE, rviz_visual_tools::SMALL, 0.1); } visual_tools.trigger();

Launch the robot_moveit demo.launch file Launch the mov move.launch package Add the markers topic in rviz and you will be able to visualize the cartesian path and the end effector state at each waypoint from the json file.

You can keep experimenting with cartesian path planning in the git repo github You can find a c++ file called circularPath.cpp which defines an array of waypoints that forms a circle which will result in the following path: