Compute separate position and rotation trajectories.

mav_trajectory_generation/src/segment.cpp

@@ -199,10 +199,6 @@ bool Segment::getSegmentWithSingleDimension(int dimension,
 
 bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append,
                                               Segment* new_segment) const {
-  if (time_ != segment_to_append.getTime()) {
-    LOG(WARNING) << "The appended segment needs the same segment time.";
-    return false;
-  }
   if (N_ == 0 || D_ == 0) {
     *new_segment = segment_to_append;
     return true;
@@ -215,14 +211,31 @@ bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append,
   // Get common polynomial order.
   const int new_N = std::max(segment_to_append.N(), N_);
   const int new_D = D_ + segment_to_append.D();
+
+  // Create temporary segments to scale polynomials if necessary.
+  Segment current_segment = *this;
+  Segment segment_to_append_temp = segment_to_append;
+
+  // Scale segment polynomials to the longer segment time.
+  const double new_time = std::max(time_, segment_to_append.getTime());
+  if (time_ < new_time){
+    for (int d = 0; d < D_; d++) {
+      current_segment[d].scalePolynomialInTime(time_ / new_time);
+    }
+  } else if (segment_to_append.getTime() < new_time) {
+    for (int d = 0; d < segment_to_append.D(); d++) {
+      segment_to_append_temp[d].scalePolynomialInTime(segment_to_append.getTime() / new_time);
+    }
+  }
+
   *new_segment = Segment(new_N, new_D);
 
   if (N_ == segment_to_append.N()) {
     for (int i = 0; i < new_D; i++) {
       if (i < D_) {
-        (*new_segment)[i] = polynomials_[i];
+        (*new_segment)[i] = current_segment[i];
       } else {
-        (*new_segment)[i] = segment_to_append[i - D_];
+        (*new_segment)[i] = segment_to_append_temp[i - D_];
       }
     }
   } else {
@@ -243,7 +256,7 @@ bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append,
     }
   }
 
-  new_segment->setTime(time_);
+  new_segment->setTime(new_time);
   return true;
 }
 

mav_trajectory_generation_example/include/mav_trajectory_generation_example/example_planner.h

@@ -21,7 +21,17 @@ class ExamplePlanner {
   // Plans a trajectory to take off from the current position and
   // fly to the given altitude (while maintaining x,y, and yaw).
   bool planTrajectory(const Eigen::VectorXd& goal_pos,
-                      const Eigen::VectorXd& goal_vel);
+                      const Eigen::VectorXd& goal_vel,
+                      mav_trajectory_generation::Trajectory* trajectory);
+
+  bool planTrajectory(const Eigen::VectorXd& goal_pos,
+                      const Eigen::VectorXd& goal_vel,
+                      const Eigen::VectorXd& start_pos,
+                      const Eigen::VectorXd& start_vel,
+                      double v_max, double a_max,
+                      mav_trajectory_generation::Trajectory* trajectory);
+
+  bool publishTrajectory(const mav_trajectory_generation::Trajectory& trajectory);
 
  private:
   ros::Publisher pub_markers_;

mav_trajectory_generation_example/src/example_planner.cc

@@ -38,7 +38,8 @@ void ExamplePlanner::setMaxSpeed(const double max_v) {
 // Plans a trajectory from the current position to the a goal position and velocity
 // we neglect attitude here for simplicity
 bool ExamplePlanner::planTrajectory(const Eigen::VectorXd& goal_pos,
-                                    const Eigen::VectorXd& goal_vel) {
+                                    const Eigen::VectorXd& goal_vel,
+                                    mav_trajectory_generation::Trajectory* trajectory) {
 
 
   // 3 Dimensional trajectory => through carteisan space, no orientation
@@ -104,10 +105,12 @@ bool ExamplePlanner::planTrajectory(const Eigen::VectorXd& goal_pos,
   opt.optimize();
 
   // get trajectory as polynomial parameters
-  mav_trajectory_generation::Trajectory trajectory;
-  opt.getTrajectory(&trajectory);
+  opt.getTrajectory(&(*trajectory));
 
+  return true;
+}
 
+bool ExamplePlanner::publishTrajectory(const mav_trajectory_generation::Trajectory& trajectory){
   // send trajectory as markers to display them in RVIZ
   visualization_msgs::MarkerArray markers;
   double distance =
@@ -120,9 +123,8 @@ bool ExamplePlanner::planTrajectory(const Eigen::VectorXd& goal_pos,
                                                &markers);
   pub_markers_.publish(markers);
 
-
   // send trajectory to be executed on UAV
-  mav_planning_msgs::PolynomialTrajectory4D msg;
+  mav_planning_msgs::PolynomialTrajectory msg;
   mav_trajectory_generation::trajectoryToPolynomialTrajectoryMsg(trajectory,
                                                                  &msg);
   msg.header.frame_id = "world";

mav_trajectory_generation_example/src/example_planner_6dof.cc

@@ -27,7 +27,7 @@ void ExamplePlanner::uavOdomCallback(const nav_msgs::Odometry::ConstPtr& odom) {
   // store current position in our planner
   tf::poseMsgToEigen(odom->pose.pose, current_pose_);
 
-  // store current vleocity
+  // store current velocity
   tf::vectorMsgToEigen(odom->twist.twist.linear, current_velocity_);
   tf::vectorMsgToEigen(odom->twist.twist.angular, current_angular_velocity_);
 }
@@ -38,62 +38,108 @@ void ExamplePlanner::setMaxSpeed(const double max_v) {
 }
 
 // Plans a trajectory from the current position to the a goal position and velocity
-// we neglect attitude here for simplicity
-bool ExamplePlanner::planTrajectory(const Eigen::VectorXd& goal_pos,
-                                    const Eigen::VectorXd& goal_vel) {
-
-  // 3 Dimensional trajectory =>
-  // 4 Dimensional trajectory => 
+bool ExamplePlanner::planTrajectory(
+    const Eigen::VectorXd& goal_pos, const Eigen::VectorXd& goal_vel,
+    mav_trajectory_generation::Trajectory* trajectory) {
+  assert(*trajectory);
+  trajectory->clear();
+
+  // 3 Dimensional trajectory => 3D position
+  // 4 Dimensional trajectory => 3D position + yaw
   // 6 Dimensional trajectory => through SE(3) space, position and orientation
   const int dimension = goal_pos.size();
+  const double v_max = 2.0;
+  const double a_max = 2.0;
+  const double ang_v_max = 2.0;
+  const double ang_a_max = 2.0;
+  bool success = false;
+
+  if (dimension == 6) 
+  {
+    mav_trajectory_generation::Trajectory trajectory_trans, trajectory_rot;
+
+    // Translation trajectory.
+    Eigen::Vector3d goal_position = goal_pos.head(3);
+    Eigen::Vector3d goal_lin_vel = goal_vel.head(3);
+    success = planTrajectory(
+        goal_position, goal_lin_vel, current_pose_.translation(), current_velocity_, v_max,
+        a_max, &trajectory_trans);
+
+    // Rotation trajectory.
+    Eigen::Vector3d goal_rotation = goal_pos.tail(3);
+    Eigen::Vector3d goal_ang_vel = goal_vel.tail(3);
+    Eigen::Vector3d current_rot_vec;
+    mav_msgs::vectorFromRotationMatrix(
+        current_pose_.rotation(), &current_rot_vec);
+    success &= planTrajectory(
+        goal_rotation, goal_ang_vel, current_rot_vec, current_angular_velocity_, ang_v_max,
+        ang_a_max, &trajectory_rot);
+
+    // Combine trajectories.
+    success &= trajectory_trans.getTrajectoryWithAppendedDimension(
+            trajectory_rot, &(*trajectory));
+    return success;
+  } 
+  else if (dimension == 3) 
+  {
+    success = planTrajectory(
+        goal_pos, goal_vel, current_pose_.translation(), current_velocity_, 
+        v_max, a_max, &(*trajectory));
+    return success;
+  } 
+  else if (dimension == 4) 
+  {
+    Eigen::Vector4d start_pos_4d, start_vel_4d;
+    start_pos_4d << current_pose_.translation(), mav_msgs::yawFromQuaternion((Eigen::Quaterniond)current_pose_.rotation());
+    start_vel_4d << current_velocity_, 0.0;
+    success = planTrajectory(
+        goal_pos, goal_vel, start_pos_4d, start_vel_4d, 
+        v_max, a_max, &(*trajectory));
+    return success;
+  } 
+  else 
+  {
+    LOG(WARNING) << "Dimension must be 3, 4 or 6 to be valid.";
+    return false;
+  }
+}
 
-  // Array for all waypoints and their constrains
+// Plans a trajectory from a start position and velocity to a goal position and velocity
+bool ExamplePlanner::planTrajectory(const Eigen::VectorXd& goal_pos,
+                                    const Eigen::VectorXd& goal_vel,
+                                    const Eigen::VectorXd& start_pos,
+                                    const Eigen::VectorXd& start_vel,
+                                    double v_max, double a_max,
+                                    mav_trajectory_generation::Trajectory* trajectory) {
+  assert(*trajectory);
+  const int dimension = goal_pos.size();
+  // Array for all waypoints and their constraints
   mav_trajectory_generation::Vertex::Vector vertices;
 
   // Optimze up to 4th order derivative (SNAP)
   const int derivative_to_optimize =
       mav_trajectory_generation::derivative_order::SNAP;
 
   // we have 2 vertices:
-  // Start = current pose and twist
-  // end = desired pose and twist
+  // start = desired start vector
+  // end = desired end vector
   mav_trajectory_generation::Vertex start(dimension), end(dimension);
 
   /******* Configure start point *******/
-  // set start point constraints to current position and set all derivatives to zero
-  Eigen::VectorXd current_pose_vector(6);
-  Eigen::Vector3d current_rot_vec;
-  mav_msgs::vectorFromRotationMatrix(current_pose_.rotation(), &current_rot_vec);
-  current_pose_vector << current_pose_.translation(), current_rot_vec;
-  start.makeStartOrEnd(current_pose_vector,
-                       derivative_to_optimize);
-
-  // set start point's velocity to be constrained to current velocity
-  Eigen::VectorXd current_twist(6);
-  current_twist << current_velocity_, current_angular_velocity_;
+  start.makeStartOrEnd(start_pos, derivative_to_optimize);
   start.addConstraint(mav_trajectory_generation::derivative_order::VELOCITY,
-                      current_twist);
-
-  // add waypoint to list
+                      start_vel);
   vertices.push_back(start);
 
-
   /******* Configure end point *******/
   // set end point constraints to desired position and set all derivatives to zero
-  end.makeStartOrEnd(goal_pos,
-                     derivative_to_optimize);
-
-  // set start point's velocity to be constrained to current velocity
+  end.makeStartOrEnd(goal_pos, derivative_to_optimize);
   end.addConstraint(mav_trajectory_generation::derivative_order::VELOCITY,
                     goal_vel);
-
-  // add waypoint to list
-  vertices.push_back(end);
+vertices.push_back(end);
 
   // setimate initial segment times
   std::vector<double> segment_times;
-  const double v_max = 2.0;
-  const double a_max = 2.0;
   segment_times = estimateSegmentTimes(vertices, v_max, a_max);
 
   // Set up polynomial solver with default params
@@ -112,9 +158,14 @@ bool ExamplePlanner::planTrajectory(const Eigen::VectorXd& goal_pos,
   opt.optimize();
 
   // get trajectory as polynomial parameters
-  mav_trajectory_generation::Trajectory trajectory;
-  opt.getTrajectory(&trajectory);
+  opt.getTrajectory(&(*trajectory));
+  trajectory->scaleSegmentTimesToMeetConstraints(v_max, a_max);
+
+  return true;
+}
+
 
+bool ExamplePlanner::publishTrajectory(const mav_trajectory_generation::Trajectory& trajectory){
   // send trajectory as markers to display them in RVIZ
   visualization_msgs::MarkerArray markers;
   double distance =

mav_trajectory_generation_example/src/example_planner_6dof_node.cc

@@ -49,7 +49,9 @@ int main(int argc, char** argv) {
     ros::spinOnce();  // process a few messages in the background - causes the uavPoseCallback to happen
   }
 
-  planner.planTrajectory(pose, twist);
+  mav_trajectory_generation::Trajectory trajectory;
+  planner.planTrajectory(pose, twist, &trajectory);
+  planner.publishTrajectory(trajectory);
   ROS_WARN_STREAM("DONE. GOODBYE.");
 
   return 0;

mav_trajectory_generation_example/src/example_planner_node.cc

@@ -40,7 +40,9 @@ int main(int argc, char** argv) {
     ros::spinOnce();  // process a few messages in the background - causes the uavPoseCallback to happen
   }
 
-  planner.planTrajectory(position, velocity);
+  mav_trajectory_generation::Trajectory trajectory;
+  planner.planTrajectory(position, velocity, &trajectory);
+  planner.publishTrajectory(trajectory);
   ROS_WARN_STREAM("DONE. GOODBYE.");
 
   return 0;