Simplify code

We know that trajectory at least comprises the start state.
Thus, we don't need the sanity checks.

Author: rhaschke

core/src/solvers/cartesian_path.cpp

@@ -102,15 +102,14 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
 	    is_valid);
 #endif
 
-	if (!trajectory.empty()) {
-		result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));
-		for (const auto& waypoint : trajectory)
-			result->addSuffixWayPoint(waypoint, 0.0);
+	assert(!trajectory.empty());  // there should be at least the start state
+	result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
+	for (const auto& waypoint : trajectory)
+		result->addSuffixWayPoint(waypoint, 0.0);
 
-		trajectory_processing::IterativeParabolicTimeParameterization timing;
-		timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
-		                         props.get<double>("max_acceleration_scaling_factor"));
-	}
+	trajectory_processing::IterativeParabolicTimeParameterization timing;
+	timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
+	                         props.get<double>("max_acceleration_scaling_factor"));
 
 	return achieved_fraction >= props.get<double>("min_fraction");
 }

core/src/stages/move_relative.cpp

@@ -302,13 +302,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
 		const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
 
 		double distance = 0.0;
-		if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
-			if (use_rotation_distance) {
-				Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
-				distance = rotation.angle();
-			} else
-				distance = (reached_pose.translation() - link_pose.translation()).norm();
-		}
+		if (use_rotation_distance) {
+			Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
+			distance = rotation.angle();
+		} else
+			distance = (reached_pose.translation() - link_pose.translation()).norm();
 
 		// min_distance reached?
 		if (min_distance > 0.0) {