矩形防护测试，横移有bug

MPC/loaded_mpc.cpp

@@ -113,9 +113,9 @@ class FG_eval_half_agv {
       }
 
 
-      if(m_statu.size()==2+8){
+      if(m_statu.size()==2+12){
         //与障碍物的距离
-        for(int i=0;i<4;++i) {
+        for(int i=0;i<6;++i) {
           double ox=m_statu[2+i*2];
           double oy=m_statu[2+i*2+1];
           for (int t = 0; t < N; ++t) {
@@ -125,7 +125,7 @@ class FG_eval_half_agv {
              * */
             CppAD::AD<double> ra=(ox-vars[nx+t])*CppAD::cos(vars[nth+t]) -(oy-vars[ny+t])*CppAD::sin(vars[nth+t]);
             CppAD::AD<double> rb=(ox-vars[nx+t])*CppAD::sin(vars[nth+t]) +(oy-vars[ny+t])*CppAD::cos(vars[nth+t]);
-            fg[1 + nobs + N*i+t] = CppAD::pow(ra/obs_w,8)+CppAD::pow(rb/obs_w,8);
+            fg[1 + nobs + N*i+t] = CppAD::pow(ra/obs_w,8)+CppAD::pow(rb/obs_h,8);
           }
         }
       }
@@ -237,9 +237,13 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
    * 障碍物是否进入 碰撞检测范围内
    */
   float distance=Pose2d::distance(obs_relative_pose_,Pose2d(0,0,0));
-  if(distance<5){
-    n_constraints = N * (5+4);
+  bool find_obs=false;
+  if(distance<20){
+    //printf(" mpc find obs ,w=%f,h=%f\n",obs_w_,obs_h_);
+    find_obs=true;
   }
+  if(find_obs) n_constraints = N * (5+6);
+
   Dvector constraints_lowerbound(n_constraints);
   Dvector constraints_upperbound(n_constraints);
   for (int i = 0; i < n_constraints; i++)
@@ -261,8 +265,8 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
   }
 
   // 与障碍物保持距离的约束
-  if(distance<5) {
-    for (int i = nobs; i < nobs + 4*N; ++i) {
+  if(find_obs) {
+    for (int i = nobs; i < nobs + 6*N; ++i) {
       constraints_lowerbound[i] = 1.0;
       constraints_upperbound[i] = 1e19;
     }
@@ -289,10 +293,12 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
   }
 
   Eigen::VectorXd statu_velocity(2);
-  if(distance<5){
-    statu_velocity=Eigen::VectorXd(2+8);
-    std::vector<Pose2d> vertexs=Pose2d::generate_rectangle_vertexs(obs_relative_pose_,2.5,1.3);
+  if(find_obs){
+    statu_velocity=Eigen::VectorXd(2+12);
+
+    std::vector<Pose2d> vertexs=Pose2d::generate_rectangle_vertexs(obs_relative_pose_,obs_w_*2,obs_h_*2);
     for(int i=0;i<vertexs.size();++i){
+      //std::cout<<"vetex:"<<vertexs[i]<<std::endl;
       statu_velocity[2+i*2]=vertexs[i].x();
       statu_velocity[2+i*2+1]=vertexs[i].y();
     }
@@ -328,7 +334,7 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
   {
     printf(" mpc failed statu : %d  input: %.4f  %.5f(%.5f) relative:(%f,%f) \n",solution.status,line_velocity,
            wmg,angular,obs_relative_pose_.x(),obs_relative_pose_.y());
-    if (solution.status == CppAD::ipopt::solve_result<Dvector>::invalid_number_detected)
+    if (solution.status == CppAD::ipopt::solve_result<Dvector>::local_infeasibility)
       return no_solution;
     return failed;
   }
@@ -348,9 +354,9 @@ MpcError LoadedMPC::solve(Trajectory trajectory, Pose2d target,Eigen::VectorXd s
     if (solve_angular < 0) correct_angular = -correct_angular;
   }
   ///-----
-  printf("input : %.4f  %.5f(%.5f)   output : %.4f  %.5f(%.5f)  ref : %.3f relative:(%f,%f) time:%.3f\n",
+  printf("input : %.4f  %.5f(%.5f)   output : %.4f  %.5f(%.5f)  ref : %.3f obs relative:(%f,%f) time:%.3f\n",
          line_velocity,wmg,angular,solve_velocity,solve_angular,correct_angular,
-         ref_velocity,targetPoseInAGV.x(),targetPoseInAGV.y(),time);
+         ref_velocity,obs_relative_pose_.x(),obs_relative_pose_.y(),time);
   if(solve_velocity>=0 && solve_velocity<min_velocity_)
     solve_velocity=min_velocity_;
   if(solve_velocity<0 && solve_velocity>-min_velocity_)

MPC/navigation.cpp

@@ -702,8 +702,8 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   Trajectory optimize_trajectory;
   Trajectory selected_trajectory;
   NavParameter::MPC_parameter mpc_parameter=parameter_.x_mpc_parameter();
-  double obs_w=1.35;
-  double obs_h=0.75;
+  double obs_w=0.8;
+  double obs_h=1.5;
 
   if (directY == true) {
     //将车身朝向旋转90°，车身朝向在(-pi,pi]
@@ -711,12 +711,15 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
     if (statu[2] > M_PI)
       statu[2] -= 2 * M_PI;
     mpc_parameter=parameter_.y_mpc_parameter();
-    obs_w=0.75;
-    obs_h=1.35;
+    obs_w=1.5;
+    obs_h=0.8;
   }
 
   Pose2d brother = timedBrotherPose_.Get();
-  Pose2d relative = Pose2d::relativePose(brother.rotate(brother.x(),brother.y(),M_PI/2), pose.rotate(pose.x(),pose.y(),M_PI/2));//计算另一节在当前小车坐标系下的坐标
+  Pose2d relative = Pose2d::relativePose(brother, pose);
+  if(directY)
+    relative = Pose2d::relativePose(brother.rotate(brother.x(),brother.y(),M_PI/2), pose.rotate(pose.x(),pose.y(),M_PI/2));//计算另一节在当前小车坐标系下的坐标
+  std::cout<<"pose:"<<pose<<",  brother:"<<brother<<", relative:"<<relative<<std::endl;
   if (move_mode_ == Monitor_emqx::ActionMode::eMain)
     relative = Pose2d(-1e8, -1e8, 0);
 
@@ -732,8 +735,9 @@ bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& o
   //std::cout<<" traj size %d  %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
   if(ret==no_solution) {
     printf(" mpc solve no solution set v/w = 0\n");
-    out[0]=0;
-    out[1]=0;
+    out.clear();
+    out.push_back(0);
+    out.push_back(0);
   }
   predict_traj_.reset(optimize_trajectory, 1);
   selected_traj_.reset(selected_trajectory, 1);

MPC/pose2d.cpp

@@ -90,8 +90,8 @@ Pose2d Pose2d::rotate(float theta)const
 }
 
 Pose2d Pose2d::rotate(float ox,float oy,float theta)const{
-  Pose2d translate(x()-ox,y()-oy,theta);//平移
-  Pose2d rotated=rotate(theta);
+  Pose2d translate(x()-ox,y()-oy,m_theta);//平移
+  Pose2d rotated=translate.rotate(theta);
   Pose2d ret(rotated.x()+x(),rotated.y()+y(),rotated.theta());
   return ret;
 }

MPC/pose2d.h

@@ -116,10 +116,13 @@ class Pose2d
 
     static std::vector<Pose2d> generate_rectangle_vertexs(const Pose2d& pose,float w,float h){
       std::vector<Pose2d> poses;
-      poses.push_back(pose*Pose2d(w/2,h/2,0));
-      poses.push_back(pose*Pose2d(w/2,-h/2,0));
-      poses.push_back(pose*Pose2d(-w/2,h/2,0));
-      poses.push_back(pose*Pose2d(-w/2,-h/2,0));
+      //std::cout<<" obs:"<<pose<<", oprator* "<<pose*Pose2d(w/2,h/2,pose.theta())<<",wh:"<<w<<","<<h<<std::endl;
+      poses.push_back(pose*Pose2d(w/2,h/2,pose.theta()));
+      poses.push_back(pose*Pose2d(w/2,-h/2,pose.theta()));
+      poses.push_back(pose*Pose2d(-w/2,h/2,pose.theta()));
+      poses.push_back(pose*Pose2d(-w/2,-h/2,pose.theta()));
+      poses.push_back(pose*Pose2d(-w/2,0,pose.theta()));
+      poses.push_back(pose*Pose2d(w/2,0,pose.theta()));
       return poses;
     }
 

config/navigation.prototxt

@@ -34,13 +34,13 @@ x_mpc_parameter
 {
 	shortest_radius:26
 	dt:0.1
-	acc_velocity:1
+	acc_velocity:1.5
 	acc_angular:10
 }
 y_mpc_parameter
 {
 	shortest_radius:24.5
 	dt:0.1
-	acc_velocity:1
+	acc_velocity:1.5
 	acc_angular:10
 }

config/navigation_main.prototxt

@@ -33,13 +33,13 @@ x_mpc_parameter
 {
 	shortest_radius:26
 	dt:0.1
-	acc_velocity:1
+	acc_velocity:1.0
 	acc_angular:10
 }
 y_mpc_parameter
 {
 	shortest_radius:24.5
 	dt:0.1
-	acc_velocity:1
+	acc_velocity:1.0
 	acc_angular:10
 }