Open even1024 opened 6 years ago
there is the steering_rate
branch, that introduces a steering speed parameter.
@croesmann we've tested it and it improves planning behavior for ackerman style vehicles! Would it be possible to merge that into the master? What could improve the behavior even more is if it remembers the steering angle between goals - that is, from my understanding, currently not implemented yet.
does it really work in all cases? I observed in some simulations that the (g2o) solver starts to freeze sometimes (so either convergence problems or non-smooth cost regions). But I investigated this just very barely up to now.
Hello. I have a tricycle robot with a front wheel actuation. Feedback also goes from the front wheel encoders as a steering angle and linear wheel speed. I am thinking to add front wheel support to teb_local_planner.
I decided to start with kinematic constraint modification like below:
double angle_diff = g2o::normalizetheta( conf2->theta() - conf1->theta() ); double dist = deltaS.norm(); double phi; if (cfg->trajectory.exact_arclength) phi = std::asin(cfg->robot.wheelbase 2sin(anglediff/2) / dist ); else phi = std::asin( cfg->robot.wheelbase angle_diff / dist ); _error[0] = fabs( ( cos(conf1->theta() +phi )+cos(conf2->theta() + phi ) ) deltaS[1] - ( sin(conf1->theta() + phi )+sin(conf2->theta() + phi ) ) * deltaS[0] );
Also It seems that I will need to modify steering speed constraints and maybe extractVelocity function. The wheel base is quite long in my case and it doesn't seem that steering constraint works well because odometry feedback have to be calculated to the rear axle. What do you think about the idea of adding front wheel actuation support and could you give some hints about it?