diff --git a/src/SampleTimedCartesianPlanner.cpp b/src/SampleTimedCartesianPlanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf8d1cff3e77c9798f01a7bb34704a077f584eb1
--- /dev/null
+++ b/src/SampleTimedCartesianPlanner.cpp
@@ -0,0 +1,142 @@
+//
+// Created by sebastian on 31.03.20.
+//
+#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 <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * allows time/velocity-constraint planning for cartesian paths
+ */
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "move_group_interface_tutorial");
+ ros::NodeHandle node_handle;
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ // wait for robot init of robot_state_initializer
+ std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
+ ros::Duration(5.0).sleep();
+ std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
+ node_handle.setParam("tud_planner_ready", true);
+
+ // Visualization Setup
+ namespace rvt = rviz_visual_tools;
+ moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+ visual_tools.deleteAllMarkers();
+ visual_tools.loadRemoteControl();
+
+ Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
+ text_pose.translation().z() = 1.75;
+ visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
+
+ // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+ visual_tools.trigger();
+
+ 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;
+
+ // initial pose of the robot
+ geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
+
+ // waypoints for a circle
+ geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_4 = group.getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_5 = group.getCurrentPose().pose;
+
+ // specification of the circles waypoints
+ target_pose_1.position.z = start_pose.position.z - 0.4;
+ target_pose_1.position.y = start_pose.position.y;
+ target_pose_1.position.x = start_pose.position.x + 0.5;
+ waypoints.push_back(target_pose_1);
+
+ target_pose_2.position.z = 0.6;//0.583;
+ target_pose_2.position.y = -0.6;//63;
+ target_pose_2.position.x = 0;//-0.007;
+ waypoints.push_back(target_pose_2);
+
+ target_pose_3.position.z = 0.6;//0.691;
+ target_pose_3.position.y = -0.032;
+ target_pose_3.position.x = -0.607;
+ waypoints.push_back(target_pose_3);
+
+ target_pose_4.position.z = 0.6;
+ target_pose_4.position.y = 0.6;//0.509;
+ target_pose_4.position.x = 0;//0.039;
+ waypoints.push_back(target_pose_4);
+
+ target_pose_5.position.z = target_pose_1.position.z;
+ target_pose_5.position.y = target_pose_1.position.y;
+ target_pose_5.position.x = target_pose_1.position.x;
+ waypoints.push_back(target_pose_5);
+
+ moveit_msgs::RobotTrajectory trajectory_msg;
+ group.setPlanningTime(10.0);
+
+ double fraction = group.computeCartesianPath(waypoints,0.01,0.0,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");
+ rt.getRobotTrajectoryMsg(trajectory_msg);
+
+ //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*=(2);
+ 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) /= 2;
+ //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);
+
+ group.execute(plan);
+
+ ros::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/src/TimedCartesianPlanner.cpp b/src/TimedCartesianPlanner.cpp
index e2552ec5c1217e5cb5a64e34c64e0adc17045604..cca05c9e50a8fb15698c9e28c87107fad4d8db42 100644
--- a/src/TimedCartesianPlanner.cpp
+++ b/src/TimedCartesianPlanner.cpp
@@ -4,33 +4,86 @@
#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 <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
-/**
- * allows time/velocity-constraint planning for cartesian paths
- */
+std::vector<geometry_msgs::Pose> raw_trajectory;
+
+const double default_velocity = 1.0;
+
+bool isInitialized = false;
+
+void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface* group)
+{
+ geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
+ geometry_msgs::Pose target_pose_4 = group->getCurrentPose().pose;
+
+ target_pose_1.position.z = 0.6;//0.583;
+ target_pose_1.position.y = -0.6;//63;
+ target_pose_1.position.x = 0;//-0.007;
+ raw_trajectory.push_back(target_pose_1);
+
+ target_pose_2.position.z = 0.6;//0.691;
+ target_pose_2.position.y = -0.032;
+ target_pose_2.position.x = -0.607;
+ raw_trajectory.push_back(target_pose_2);
+
+ target_pose_3.position.z = 0.6;
+ target_pose_3.position.y = 0.6;//0.509;
+ target_pose_3.position.x = 0;//0.039;
+ raw_trajectory.push_back(target_pose_3);
+
+ target_pose_4.position.z = group->getCurrentPose().pose.position.z;
+ target_pose_4.position.y = group->getCurrentPose().pose.position.y;
+ target_pose_4.position.x = group->getCurrentPose().pose.position.x;
+ raw_trajectory.push_back(target_pose_4);
+}
+
+void moveRobotToInitialState(ros::NodeHandle node_handle)
+{
+ ROS_INFO("Moving to initial pose of trajectory.");
+ moveit::planning_interface::MoveGroupInterface group("panda_arm");
+
+ ros::Publisher display_publisher = node_handle.
+ advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
+ group.setStartStateToCurrentState();
+
+ std::vector<geometry_msgs::Pose> trajectory_to_init_pose;
+ geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
+
+ target_pose_1.position.z = group.getCurrentPose().pose.position.z - 0.4;
+ target_pose_1.position.y = group.getCurrentPose().pose.position.y;
+ target_pose_1.position.x = group.getCurrentPose().pose.position.x + 0.5;
+ trajectory_to_init_pose.push_back(target_pose_1);
+ group.setPlanningTime(10.0);
+
+ moveit_msgs::RobotTrajectory trajectory;
+ double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);
+
+ moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
+ cartesian_plan.trajectory_ = trajectory;
+ group.execute(cartesian_plan);
+}
+
int main(int argc, char **argv)
{
- ros::init(argc, argv, "move_group_interface_tutorial");
+ ros::init(argc, argv, "timed_cartesian_planner");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
- std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
ros::Duration(5.0).sleep();
- std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
+ ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
node_handle.setParam("tud_planner_ready", true);
- // Visualization Setup
+ // Visualization Setup.
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
@@ -39,82 +92,80 @@ int main(int argc, char **argv)
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
+ // Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm");
+ moveRobotToInitialState(node_handle);
+ initRawTrajectory(node_handle, &group);
- 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;
+ for(int i = 0; i < raw_trajectory.size(); i++)
+ {
+ double velocity = 0.0;
- geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
- geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
+ if(!node_handle.getParam("robot_speed_factor", velocity))
+ {
+ velocity = default_velocity;
+ }
- target_pose_1.position.z = start_pose.position.z - 0.2;
- target_pose_1.position.y = start_pose.position.y + 0.2;
- target_pose_1.position.x = start_pose.position.x + 0.2;
- waypoints.push_back(target_pose_1);
+ std::cout << ">>>>>>>>>>>>>>>>> starting planing step " << i << " (speed: " << velocity << ")" << std::endl;
- geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
- target_pose_2.position.z = start_pose.position.z + 0.2;
- target_pose_1.position.y = start_pose.position.y - 0.2;
- target_pose_2.position.x = start_pose.position.x - 0.2;
- waypoints.push_back(target_pose_2);
+ std::vector<geometry_msgs::Pose> single_traj;
+ single_traj.push_back(raw_trajectory.at(i));
- moveit_msgs::RobotTrajectory trajectory_msg;
- group.setPlanningTime(10.0);
+ moveit::planning_interface::MoveGroupInterface group("panda_arm");
+ moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
+ ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
- // 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");
+ moveit::planning_interface::MoveGroupInterface::Plan plan;
+ group.setStartStateToCurrentState();
- // Second get a RobotTrajectory from trajectory
- rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
+ moveit_msgs::RobotTrajectory trajectory_msg;
+ group.setPlanningTime(10.0);
- // 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");
- rt.getRobotTrajectoryMsg(trajectory_msg);
+ double fraction = group.computeCartesianPath(single_traj,0.01,0.0,trajectory_msg, false);
+ robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
+ rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
- //std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
+ // 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");
+ rt.getRobotTrajectoryMsg(trajectory_msg);
+ plan.trajectory_ = trajectory_msg;
+ ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)",fraction * 100.0);
- // 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*=(2);
- for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
+ //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++)
{
- if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
- trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
- //trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
+ double new_tfs = 1 / velocity;
+ trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
+ 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) *= velocity;
+ }
+ }
+
+ 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;
+
+ group.execute(plan);
- for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
+ if(i == (raw_trajectory.size() -1))
{
- trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
- sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
+ i = -1;
}
}
- //std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
- plan.trajectory_ = trajectory_msg;
- ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
-
- group.execute(plan);
-
ros::shutdown();
return 0;
}
\ No newline at end of file