Problem about trajectory following not accurately

[BlueeHole]

When I use ego-planner on my quadrotor, a problem arises... The trajectory generated by ego-planner is calculated by mathematics method, and should be accurate, right ? But when I send the position, velocity, acceleration, yaw and yawdot to a real Quadrotor (using MAVROS and px4), the actual trajectory won't follow the planned traj very well. like this, Can this be explianed? (> <)

[USTfgaoaa]

@BlueeHole If the drone can't follow the traj properly, it's a problem of control not planning.

[BlueeHole]

@BlueeHole If the drone can't follow the traj properly, it's a problem of control not planning.

OK thank you ~ let me check it

[shubham-shahh]

@BlueeHole did you find a solution?

[shubham-shahh]

@BlueeHole If the drone can't follow the traj properly, it's a problem of control not planning.

Hi, Does the ego planner consider the current position of the quad before sending the next pos, vel and acc command? or it's open loop? If I want to make sure that the quad follows the trajactory, what controllers can I use? @bigsuperZZZX thanks

[TanmayJain03]

@BlueeHole If the drone can't follow the traj properly, it's a problem of control not planning.

Hi, Does the ego planner consider the current position of the quad before sending the next pos, vel and acc command? or it's open loop? If I want to make sure that the quad follows the trajactory, what controllers can I use? @bigsuperZZZX thanks

@shubham-shahh I too am facing the same issue where the start of the planned trajectory moves ahead while the drone is still stationary, and the tracker suddenly starts moving aggressively in a straight line directly towards the already leading trajectory's starting point till it catches up also not avoiding any obstacles during this. I am trying to make changes in ego_planner_fsm.cpp where I changed the start_pt in the planFromCurrentTraj() function to the position of the drone as observed in gazebo model states. Could you please share any lead as to how you tried to fix the problem, any help would be really appreciated. Regards

[bigsuperZZZX]

It's not difficult to fix this issue. You can compare the distance error between the real position from the drone odometry and the desired position from the current trajectory. If the distance error goes above a pre-defined threshold like 1m, then just start a new trajectory planning that takes the current drone odometry as the trajectory start state.

[shubham-shahh]

It's not difficult to fix this issue. You can compare the distance error between the real position from the drone odometry and the desired position from the current trajectory. If the distance error goes above a pre-defined threshold like 1m, then just start a new trajectory planning that takes the current drone odometry as the trajectory start state.

I find 2 possible problems with this approach, please correct me if I'm wrong

1) if the the trajectory error is always existent let's say 0.9m,the drone is either always leading or lagging the traj, during swarm operations, when inter drone collision avoidance is considered, it can cause issues because of this error unless there's a mechanism that communicates this error too

2) if the drone is in moving state, and if we trigger a replan, it'll consider the current pos of the drone and make a traj, but now the drone has moved a new position and again the error has risen and we'll keep triggering it

[shubham-shahh]

@BlueeHole If the drone can't follow the traj properly, it's a problem of control not planning.

Hi, Does the ego planner consider the current position of the quad before sending the next pos, vel and acc command? or it's open loop? If I want to make sure that the quad follows the trajactory, what controllers can I use? @bigsuperZZZX thanks

@shubham-shahh I too am facing the same issue where the start of the planned trajectory moves ahead while the drone is still stationary, and the tracker suddenly starts moving aggressively in a straight line directly towards the already leading trajectory's starting point till it catches up also not avoiding any obstacles during this. I am trying to make changes in ego_planner_fsm.cpp where I changed the start_pt in the planFromCurrentTraj() function to the position of the drone as observed in gazebo model states. Could you please share any lead as to how you tried to fix the problem, any help would be really appreciated. Regards

Changing the start_pt to drone Odom surely won't help since it won't be smooth, what autopilot are you using?

[bigsuperZZZX]

1. A consistent large error like 0.9m is of course not allowed. The controller should be adjusted before that.
2. Planning a new trajectory spends only about 1ms, so the moved position during that time can be neglected.

[shubham-shahh]

1. A consistent large error like 0.9m is of course not allowed. The controller should be adjusted before that.
2. Planning a new trajectory spends only about 1ms, so the moved position during that time can be neglected.

I agree, and as you mentioned this problem should be solved from the controller end but i was curious if there's a way by which we can add a mechanism to consider each drone's traj tracking error during swarm operations

[bigsuperZZZX]

If you can precisely model and predict the control error, then considering it will make sense.

[shubham-shahh]

If you can precisely model and predict the control error, then considering it will make sense.

I see, I'll look into it and update this thread

[TanmayJain03]

It's not difficult to fix this issue. You can compare the distance error between the real position from the drone odometry and the desired position from the current trajectory. If the distance error goes above a pre-defined threshold like 1m, then just start a new trajectory planning that takes the current drone odometry as the trajectory start state.

Thanks a lot for taking out the time to reply @bigsuperZZZX and @shubham-shahh, I could fix this issue following your suggestion. In the file ego_replan_fsm.cpp case EXEC_TRAJ in execFSMCallback() I added the following code

if ((odom_pos_ - info->start_pos_).norm() > 1)
      {
        changeFSMExecState(GEN_NEW_TRAJ, "Trajectory_Ran_Away");
        return;
      }

where my "odompos" topic is remapped to odometry received from gazebo.

I am using ardupilot autopilot for simulation. Regards.

[shubham-shahh]

It's not difficult to fix this issue. You can compare the distance error between the real position from the drone odometry and the desired position from the current trajectory. If the distance error goes above a pre-defined threshold like 1m, then just start a new trajectory planning that takes the current drone odometry as the trajectory start state.

Thanks a lot for taking out the time to reply @bigsuperZZZX and @shubham-shahh, I could fix this issue following your suggestion. In the file ego_replan_fsm.cpp case EXEC_TRAJ in execFSMCallback() I added the following code

if ((odom_pos_ - info->start_pos_).norm() > 1)
      {
        changeFSMExecState(GEN_NEW_TRAJ, "Trajectory_Ran_Away");
        return;
      }

where my "odompos" topic is remapped to odometry received from gazebo.

I am using ardupilot autopilot for simulation. Regards.

you can either keep this check in checkCollisionCallback, or you can check before replanning in replanning state, putting it without any state could possibly cause undefined behaviour