Attitude tracking with aerial robots

[LorenzoBalandi]

Hi,

I am using Crocoddyl to control a fully actuated hexarotor.
The goal is to track a reference state trajectory.
The geometric path for the reference trajectory of the CoM is a planar circle, tracked with a 5th order polynomial motion law with null initial velocities and accelerations.
The reference attitude is zero roll and pitch, and a 360 deg rotation for the yaw, also defined with a 5th order polynomial as shown in the following image, where the yaw (green line) is converted in RPY angles even if it is passed as a quaternion (qx,qy,qz,qw) in the code.

In other words, the body x axis has to point always outside the circle.
The model I am using is loaded from a urdf exported from a CAD.

I setup and solve the OCP with parameters

T = 200 # shooting nodes
dt = 2e-2 # discretization time
Tsim_s = dt*T # total sim time (seconds)

and here is the robot behavior (the red arm is the x body axis, red cylinders indicate the motor thrusts)

2025-06-09.16-18-34.mp4

While crossing yaw=pi, there is an unwanted "flip".
Here is the plot of position (xyz) and attitude quaternion (qx,qy,qz,qw).

The OCP has only two costs: input regularization and full state tracking

# Create cost terms
# Control regularization cost
uResidual = crocoddyl.ResidualModelControl(state, nu)
uRegCost = crocoddyl.CostModelResidual(state, uResidual)
# State regularization cost
xActivation = crocoddyl.ActivationModelWeightedQuad(
    np.array([1e3]*3 + [1e1,1e1,1e3] + [1e0,1e0,1e0,1e0,1e0,1e0])
)

# Create the running models 
# (the terminal model is not treated explicitly but it's the last element of the running models list)
runningModels = []
for t in range(T + 1):
    runningCostModel = crocoddyl.CostModelSum(state, nu)
    # Add costs
    # runningCostModel.addCost("stateReg", xRegCost, 1e-1) # we could weight only the velocities
    runningCostModel.addCost("uReg", uRegCost, 1e-3)

    if t != T:
        xResidual = crocoddyl.ResidualModelState(state, traj[t,:], nu)
        xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
        runningCostModel.addCost("xReg", xRegCost, 1.0)
    else:
        xResidual = crocoddyl.ResidualModelState(state, traj[-1,:], nu)
        xRegCost = crocoddyl.CostModelResidual(state, xActivation, xResidual)
        runningCostModel.addCost("xReg", xRegCost, 1.0)

    # Define constraints
    constraints = crocoddyl.ConstraintModelManager(state, nu)
    if t != 0:
        constraints.addConstraint("state_bound", state_constraint)
    constraints.addConstraint("u_bound", u_bound)

    # Create Differential action model
    running_DAM = crocoddyl.DifferentialActionModelFreeFwdDynamics(
        state, actuation, runningCostModel, constraints
    )
    # Apply Euler integration
    running_model = crocoddyl.IntegratedActionModelEuler(running_DAM, dt)
    runningModels.append(running_model)

# Create the shooting problem
problem = crocoddyl.ShootingProblem(x0, runningModels[:-1], runningModels[-1])

where traj[t,:] is the reference state trajectory at node t, defined as the concatenation of position, attitude quaternion, linear velocity, angular velocity.

I formulate the problem in this way because I have a full reference state to track and I am equally interested in all the trajectory points.
Then, I use mim_solvers to solve the problem, but I had the same issue using Crocoddyl's FDDP, so I think the issue is not in the solver. The only constraint considered are the input bounds.

Interestingly, if I solve the same tracking problem with MPC with the same weights, in a simulation where at each loop I initialize the OCP with the current state obtained via integration with Pinocchio, the trajectory is well tracked (probably because the horizon is too limited to plan complex motions such as the flip)

2025-06-09.16-34-44.mp4

I am also developing a simulation in Gazebo with the same reference trajectory and problem formulation and also with MPC I have unstable behaviors when the yaw is far from 0 deg, suggesting that there is something wrong with the attitude.

Any idea of what could be the problem?

[cmastalli]

Hi @LorenzoBalandi,

The reported results have nothing to do with FDDP or any solver, but how Lie algebra works. When you're crossing yaw=pi, there is no singularity thanks to our Lie approach; however, your closest point is that flip. This doesn't happen when you start integrating from a different initial state, as encountered in your MPC results.

One workaround for trajectory optimization would be to provide an initial guess where "the vehicle is rotated as expected". I am saying this because you're likely using the initial state to warm-start the entire trajectory, right?

Remembering our previous discussion, would you be able to let me know what actuation model you're using? Please consider visiting us as we're developing some very cool estimation approaches where you could contribute. As the main Crocoddyl developers, we have and are developing many advanced features that won't be public soon.

[LorenzoBalandi]

Hi Carlos,

yes I am indeed warm starting the entire trajectory with the initial state (zero position + unit quaternion).
If I warm start using the reference trajectory the behavior is similar to the MPC case (no flip).

Looking at the velocity plots, I have another doubt, which maybe I should have clarified later.
In which reference frame is the state expressed by default? Usually, in aerial robotics, position and linear velocity are expressed in world frame, while angular velocity is in body frame. That's why in the equation of motion the linear part of the body wrench (expressed in body frame) is rotated from body to world (see, for example Eq. (2) here).
However, to my understanding Crocoddyl and Pinocchio require to use a unique frame. For example ResidualModelFrameVelocity() requires a single reference frame pin.LOCAL or pin.WORLD.

As actuation model, I am still using the deprecated ActuationModelMultiCopterBase() both with the hexarotor and the hexarotor+arm (which I am now porting to a Gazebo simulation which uses our plugins for SITL).
It could be interesting to see other features useful for aerial manipulation (in particular, after the free flight we will explore contacts with the environment).

[cmastalli]

We follow Featherstone's approach; therefore, the base velocities are expressed in the body frame. Have you had a chance to try using our new actuation model? I am keen to sort out this situation.

We are working on several improvements, including contacts and advanced solvers that work with different floating-point numbers (practical for software embedding and efficiency). Unfortunately, I can't tell you more than this. However, these are examples of what we have already published, but have not made publicly available:

1. https://youtu.be/nDOxTgeK_5o
2. https://youtu.be/RBohdOhgbWw

[LorenzoBalandi]

Thanks, I fixed the velocity so now the simulation works in Gazebo, minimizing the implementation effort for experiments.

For the actuation model you mean ActuationModelFloatingBaseThrusters? If so, no I didn't look further into it. It may be one of the next steps.
The videos seems interesting!