diff --git a/config/config.yaml b/config/config.yaml
index 1c84f392d70e116356398ac6185f8bd6233310d0..48ae64d65812c1e8799d614911e26da7afea9511 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -1,7 +1,8 @@
panda_mqtt_connector:
server: "tcp://localhost:1883"
- robot_speed_factor: .15
+ robot_speed_factor: .95
robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
+ default_trajectory: "square" # "square" or "circle", everything else is ignored
topics:
robotConfig: "robotconfig"
dataConfig: "dataconfig"
diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 9404808bc650c3584d0014c9a77330cedca68a7a..beae1b83d32c2bf5e5f866603a8c54223f83aad9 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -18,41 +18,44 @@
namespace PlannerState {
- std::vector <geometry_msgs::Pose> raw_trajectory;
+ std::vector<geometry_msgs::Pose> raw_trajectory;
TrajectoryUtil traj_util;
const double default_velocity = 1.0;
const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH;
+ const bool useCircle = true;
bool isLooping = true;
}
-void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
+void
+constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, double y_dimension) {
- moveit_msgs::CollisionObject plate;
+ moveit_msgs::CollisionObject plate;
- plate.header.frame_id = "panda_link0";
- plate.id = "plate";
+ plate.header.frame_id = "panda_link0";
+ plate.id = "plate";
- plate.primitives.resize(1);
- plate.primitives[0].type = 1u;
- plate.primitives[0].dimensions.resize(3);
- plate.primitives[0].dimensions[0] = x_dimension;
- plate.primitives[0].dimensions[1] = y_dimension;
- plate.primitives[0].dimensions[2] = 0.1;
+ plate.primitives.resize(1);
+ plate.primitives[0].type = 1u;
+ plate.primitives[0].dimensions.resize(3);
+ plate.primitives[0].dimensions[0] = x_dimension;
+ plate.primitives[0].dimensions[1] = y_dimension;
+ plate.primitives[0].dimensions[2] = 0.1;
- plate.primitive_poses.resize(1);
- plate.primitive_poses[0].position.x = 0;
- plate.primitive_poses[0].position.y = 0;
- plate.primitive_poses[0].position.z = -0.1;
+ plate.primitive_poses.resize(1);
+ plate.primitive_poses[0].position.x = 0;
+ plate.primitive_poses[0].position.y = 0;
+ plate.primitive_poses[0].position.z = -0.1;
- plate.operation = plate.ADD;
+ plate.operation = plate.ADD;
- collision_objects.push_back(plate);
+ collision_objects.push_back(plate);
}
-void initCircularTrajectory(double radius, geometry_msgs::Point origin, int granularity) {
+void loadCircularTrajectory(std::vector<geometry_msgs::Pose> &waypoints, double radius, geometry_msgs::Point origin,
+ int granularity) {
double step_size_rad = 2 * M_PI / granularity;
@@ -71,125 +74,68 @@ void initCircularTrajectory(double radius, geometry_msgs::Point origin, int gran
nextPose.position.y = origin.y + radius * cos(next_rad);
nextPose.position.z = origin.z;
- PlannerState::raw_trajectory.push_back(nextPose);
- std::cout << "added: " << nextPose << std::endl;
+ waypoints.push_back(nextPose);
}
}
-void
-initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group, bool useDefault,
- bool useCircularDefault) {
-
- geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
-
- if (useCircularDefault) {
- geometry_msgs::Point origin;
- origin.z = .3;
- initCircularTrajectory(0.6, origin, 20);
- return;
- }
-
- if (!PlannerState::traj_util.initWaypoints(PlannerState::raw_trajectory, node_handle, current_pose) && useDefault) {
- // choose a default trajectory
- 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_2.position.z = 0.6;//0.583;
- target_pose_2.position.y = -0.6;//63;
- target_pose_2.position.x = 0;//-0.007;
- PlannerState::raw_trajectory.push_back(target_pose_2);
+bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeHandle &node_handle,
+ geometry_msgs::Pose &initialPose) {
- target_pose_3.position.z = 0.6;//0.691;
- target_pose_3.position.y = -0.032;
- target_pose_3.position.x = -0.607;
- PlannerState::raw_trajectory.push_back(target_pose_3);
+ // regardless if the loading fails, the trajectory is no longer "new" after calling this method
+ node_handle.setParam("new_trajectory_available", false);
- target_pose_4.position.z = 0.6;
- target_pose_4.position.y = 0.6;//0.509;
- target_pose_4.position.x = 0;//0.039;
- PlannerState::raw_trajectory.push_back(target_pose_4);
+ std::vector<double> x_values;
+ std::vector<double> y_values;
+ std::vector<double> z_values;
+
+ // check if no trajectory is configured
+ if (!node_handle.getParam("trajectory_pos_x", x_values) ||
+ !node_handle.getParam("trajectory_pos_y", y_values) ||
+ !node_handle.getParam("trajectory_pos_z", z_values)) {
+ return false;
+ } else {
+ for (int i = 0; i < x_values.size(); i++) {
+ geometry_msgs::Pose pose = initialPose;
+ pose.position.x = x_values.at(i);
+ pose.position.y = y_values.at(i);
+ pose.position.z = z_values.at(i);
+ waypoints.push_back(pose);
+ }
- target_pose_1.position.z = group->getCurrentPose().pose.position.z;
- target_pose_1.position.y = group->getCurrentPose().pose.position.y;
- target_pose_1.position.x = group->getCurrentPose().pose.position.x;
- PlannerState::raw_trajectory.push_back(target_pose_1);
+ return true;
}
}
-bool updateRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface *group) {
-
- geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
- PlannerState::raw_trajectory.clear();
- node_handle.setParam("new_trajectory_available", false);
-
- return PlannerState::traj_util.initWaypoints(PlannerState::raw_trajectory, node_handle, current_pose);
-}
-
-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;
-
- // start at beginning of circular movement
- tf2::Quaternion orientation; // the RPY constructor is deprecated
- orientation.setRPY(0, M_PI, 0);
-
- geometry_msgs::Pose target_pose_1;
- target_pose_1.orientation = tf2::toMsg(orientation);
- target_pose_1.position.x = 0;
- target_pose_1.position.y = .6;
- target_pose_1.position.z = .3;
+void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
- trajectory_to_init_pose.push_back(target_pose_1);
- group.setPlanningTime(10.0);
+ bool newTrajectoryAvailable = false;
+ node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
- moveit_msgs::RobotTrajectory trajectory;
- double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);
+ if (newTrajectoryAvailable) {
+ geometry_msgs::Pose current_pose = group.getCurrentPose().pose;
+ PlannerState::raw_trajectory.clear();
- moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
- cartesian_plan.trajectory_ = trajectory;
- group.execute(cartesian_plan);
-}
+ if (!loadWaypoints(PlannerState::raw_trajectory, node_handle, current_pose)) {
+ ROS_ERROR_STREAM("Unable to load new trajectory!");
+ }
+ }
-void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) {
+ if (PlannerState::raw_trajectory.empty()) {
+ return;
+ }
for (int i = 0; i < PlannerState::raw_trajectory.size(); i++) {
- bool newTrajectoryAvailable = false;
node_handle.getParam("new_trajectory_available", newTrajectoryAvailable);
node_handle.getParam("loop_trajectory", PlannerState::isLooping);
- if (newTrajectoryAvailable) {
- if (!updateRawTrajectory(node_handle, &group)) {
- break;
- }
- i = 0;
- }
-
- // wake up if we have trajectory
- while (PlannerState::raw_trajectory.empty()) {
- if (newTrajectoryAvailable) {
- updateRawTrajectory(node_handle, &group);
- i = 0;
- break;
- }
- }
-
- double velocity = 0.0;
- std::string planning_mode;
+ double velocity;
if (!node_handle.getParam("robot_speed_factor", velocity)) {
velocity = PlannerState::default_velocity;
}
+ std::string planning_mode;
if (!node_handle.getParam("robot_planning_mode", planning_mode)) {
planning_mode = PlannerState::default_planning_mode;
}
@@ -197,15 +143,18 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo
ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity);
moveit::planning_interface::MoveGroupInterface::Plan plan;
- PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity);
- group.execute(plan);
+
+ if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8,
+ 0.8, velocity)) {
+ group.execute(plan);
+ } else {
+ ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings.");
+ }
// make sure the robot moves in an infinite circle
if (i == (PlannerState::raw_trajectory.size() - 1)) {
if (PlannerState::isLooping) {
i = -1;
- } else {
- return;
}
}
}
@@ -220,27 +169,67 @@ int main(int argc, char **argv) {
spinner.start();
// wait for robot init of robot_state_initializer
- ros::Duration(5.0).sleep();
+ ros::Duration(3.0).sleep();
ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
- node_handle.setParam("tud_planner_ready", true);
// Initialize start state of robot and target trajectory.
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- //moveRobotToInitialState(node_handle);
- initRawTrajectory(node_handle, &group, true, true);
node_handle.getParam("loop_trajectory", PlannerState::isLooping);
+ std::string defaultTrajectory = "<none>";
+ node_handle.getParam("default_trajectory", defaultTrajectory);
+
+
+ geometry_msgs::Pose base_pose = group.getCurrentPose().pose;
+
+ if (loadWaypoints(PlannerState::raw_trajectory, node_handle, base_pose)) {
+ ROS_INFO_STREAM("Ignoring default waypoints because there is a trajectory defined by parameters");
+ } else if (defaultTrajectory == "circle") {
+ ROS_INFO_STREAM("loading default trajectory 'circle'");
+ geometry_msgs::Point origin;
+ origin.z = .3;
+ loadCircularTrajectory(PlannerState::raw_trajectory, 0.6, origin, 20);
+ } else if (defaultTrajectory == "square") {
+ ROS_INFO_STREAM("loading default trajectory 'square'");
+ // choose a default trajectory
+ base_pose.position.x = 0.4;
+ base_pose.position.y = 0.4;
+ base_pose.position.z = 0.3;
+ geometry_msgs::Pose target_pose_1 = base_pose;
+ geometry_msgs::Pose target_pose_2 = base_pose;
+ geometry_msgs::Pose target_pose_3 = base_pose;
+ geometry_msgs::Pose target_pose_4 = base_pose;
+
+ PlannerState::raw_trajectory.push_back(target_pose_1);
+
+ target_pose_2.position.y *= -1;
+ PlannerState::raw_trajectory.push_back(target_pose_2);
+
+ target_pose_3.position.x *= -1;
+ target_pose_3.position.y *= -1;
+ PlannerState::raw_trajectory.push_back(target_pose_3);
+
+ target_pose_4.position.x *= -1;
+ PlannerState::raw_trajectory.push_back(target_pose_4);
+ } else {
+ ROS_WARN_STREAM("not loading a default trajectory! provided trajectory: " + defaultTrajectory);
+ }
+
// add the ground plate
std::vector<moveit_msgs::CollisionObject> collision_objects;
constructPlate(collision_objects, 5.0, 5.0);
planning_scene_interface.applyCollisionObjects(collision_objects);
- // execute the trajectory which consists of single waypoints to allow distinct planning
- doMotion(node_handle, group);
+ while (ros::ok()) {
+
+ // execute a trajectory
+ doMotion(node_handle, group);
+
+ ros::spinOnce();
+ }
- ros::shutdown();
return 0;
}
diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp
index 88c70d7600fb4d8a1f08ec36883f2fc80f1a012b..ad9e8c7277a99f330382267270859b424f0c4262 100644
--- a/src/util/TrajectoryUtil.cpp
+++ b/src/util/TrajectoryUtil.cpp
@@ -61,6 +61,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
} else if (pathType == FLUID_PATH) {
if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
ROS_ERROR("Computation of fluid path has failed.");
+ ROS_ERROR_STREAM("Path was motion to:\n" << targetPose);
}
} else {
ROS_ERROR("Invalid type of path provided.");
@@ -68,35 +69,11 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte
}
manipulateVelocity(plan, motionSpeedFactor);
-}
-
-bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
- geometry_msgs::Pose &initialPose) {
- std::vector<double> x_values;
- std::vector<double> y_values;
- std::vector<double> z_values;
-
- // check if no trajectory is configured
- if (!node_handle.getParam("trajectory_pos_x", x_values) ||
- !node_handle.getParam("trajectory_pos_y", y_values) ||
- !node_handle.getParam("trajectory_pos_z", z_values)) {
- return false;
- } else {
- for (int i = 0; i < x_values.size(); i++) {
- geometry_msgs::Pose pose = initialPose;
- pose.position.x = x_values.at(i);
- pose.position.y = y_values.at(i);
- pose.position.z = z_values.at(i);
- waypoints.push_back(pose);
- }
-
- node_handle.setParam("new_trajectory_available", false);
- return true;
- }
+ return true;
}
void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) {
- ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << "joints");
+ ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << " joints");
for (auto &point : plan.trajectory_.joint_trajectory.points) {
point.time_from_start *= 1 / velocity;
for (double &v : point.velocities) {
diff --git a/src/util/TrajectoryUtil.h b/src/util/TrajectoryUtil.h
index 15ea8fbf601612887990c8d730f2acfce030615f..e978c7a3c1445132a2e043d694a25dd232fd7165 100644
--- a/src/util/TrajectoryUtil.h
+++ b/src/util/TrajectoryUtil.h
@@ -27,12 +27,9 @@ public:
const std::string &pathType, double maxVelocityFactor,
double maxAccelerationFactor, double motionSpeedFactor);
- bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle,
- geometry_msgs::Pose &initialPose);
-
private:
- void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
+ static void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity);
};
#endif //PANDA_SIMULATION_TRAJECTORYUTIL_H
\ No newline at end of file