basic speed adaption for cartesian paths

src/SampleConstraintPlanner.cpp

@@ -11,8 +11,101 @@
 #include <moveit_msgs/CollisionObject.h>
 
 #include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
 
 int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "move_group_interface_tutorial");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
+    ros::Duration(5.0).sleep();
+    std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
+    //moveit::planning_interface::MoveGroup group("arm_gp");
+    moveit::planning_interface::MoveGroupInterface group("panda_arm");
+
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+
+    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
+    moveit_msgs::DisplayTrajectory display_trajectory;
+
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    group.setStartStateToCurrentState();
+
+    std::vector<geometry_msgs::Pose> waypoints;
+
+    geometry_msgs::Pose target_pose = group.getCurrentPose().pose;
+    target_pose.position.x += 2.0;
+    target_pose.position.y += 5.0;
+    target_pose.position.z -= 2.0;
+    waypoints.push_back(target_pose);
+
+    /*target_pose.position.y -= 0.0;
+    waypoints.push_back(target_pose);
+
+    target_pose.position.z -= 0.08;
+    target_pose.position.y += 0.0;
+    target_pose.position.x -= 0.0;
+    waypoints.push_back(target_pose);*/
+
+    moveit_msgs::RobotTrajectory trajectory_msg;
+    group.setPlanningTime(10.0);
+
+    double fraction = group.computeCartesianPath(waypoints,
+                                                 0.01,  // eef_step
+                                                 0.0,   // jump_threshold
+                                                 trajectory_msg, false);
+    // The trajectory needs to be modified so it will include velocities as well.
+    // First to create a RobotTrajectory object
+    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
+
+    // Second get a RobotTrajectory from trajectory
+    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
+
+    // Third create a IterativeParabolicTimeParameterization object
+    trajectory_processing::IterativeParabolicTimeParameterization iptp;
+    // Fourth compute computeTimeStamps
+    bool success = iptp.computeTimeStamps(rt);
+    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
+    // Get RobotTrajectory_msg from RobotTrajectory
+    rt.getRobotTrajectoryMsg(trajectory_msg);
+    // Check trajectory_msg for velocities not empty
+    //std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
+
+    // adjust velocities, accelerations and time_from_start
+    for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
+    {
+        trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(5);
+        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
+        {
+            if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
+                trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 5;
+            //trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
+            }
+        }
+
+        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
+        {
+            trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
+                        sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
+        }
+    }
+
+    //std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
+
+    plan.trajectory_ = trajectory_msg;
+    ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
+    sleep(5.0);
+
+    group.execute(plan);
+
+    ros::shutdown();
+    return 0;
+}
+/*int main(int argc, char** argv)
 {
     ros::init(argc, argv, "CONSTRAINT PLANNER");
     ros::NodeHandle node_handle;
@@ -104,4 +197,4 @@ int main(int argc, char** argv)
 
     ros::shutdown();
     return 0;
-}
\ No newline at end of file
+}*/
\ No newline at end of file