ActuationModelFloatingBaseThrusters() with tilted thrusters

[LorenzoBalandi]

Hello,
I am implementing a MPC for an hexarotor with tilted propellers (tilthex) using Crocoddyl, taking inspiration from the quadrotor examples.
To define the actuation model through the python interface, I use

    actuation = crocoddyl.ActuationModelFloatingBaseThrusters(state, thrusters)

where thrusters is a list of 6 crocoddyl.Thruster() defined as

thrusters = [
        crocoddyl.Thruster(
            pin.SE3(R[0], p[0]),
            c_t / c_f,
            crocoddyl.ThrusterType.CW,
        ),
       ...
       # similarly for the others alternating CW and CCW
       ...
]

R[i] and p[i] i=0,...,5 are orientations and positions of the thrusters, computed with a custom python function considering system parameters.

Now the problem is, I compare the allocation matrix (mapping thrusts to body wrench) obtained with Crocoddyl (actuation.Wthrust) with the one computed using another custom function and I obtain different results if the tilting angle is not zero.
I am pretty sure about the custom functions I am using for comparison because they have been extensively tested for simulations and experiments.

To investigate this behavior, I had a look at the code here from line 298, but it seems to me that the Crocoddyl function and the one I am using for comparison do the same operations to compute the allocation matrix, according to theory (see, for example, this paper, section 2.1), so I don't understand where the issue could be. I am not a Cpp expert, though.

Another question, more general, is how to understand the accessible variables to the ActuationModelFloatingBaseThrusters class. For example, I found that the allocation matrix is Wthrust only because I tried to access actuation.W_thrust_ after looking at the code, and I received a hint in the error message

AttributeError: 'ActuationModelFloatingBaseThrusters' object has no attribute 'W_thrust_'. Did you mean: 'Wthrust'?

In the documentation I don't find references to Wthrust, maybe I am looking in the wrong place?

For the moment I managed to proceed forward with my project using ActuationModelMultiCopterBase(state,alloc) because it allows to specify the allocation matrix, but that class is deprecated and I would like to solve the doubts written above.

Thanks,
Lorenzo

[cmastalli]

Now the problem is, I compare the allocation matrix (mapping thrusts to body wrench) obtained with Crocoddyl (actuation.Wthrust) with the one computed using another custom function and I obtain different results if the tilting angle is not zero.
I am pretty sure about the custom functions I am using for comparison because they have been extensively tested for simulations and experiments.

Can you share a snippet code of your custom function?

To investigate this behavior, I had a look at the code here from line 298, but it seems to me that the Crocoddyl function and the one I am using for comparison do the same operations to compute the allocation matrix, according to theory (see, for example, this paper, section 2.1), so I don't understand where the issue could be. I am not a Cpp expert, though.

If you compare our thrust-mapping matrix Wthrust with the cited paper, we are accounting for the total torque ("actuator and dragging forces") using ctorque. Apart from this, the rest remains the same. Moreover, be aware the Wthurst is mapping thrusters f_i to body wrenches.

Another question, more general, is how to understand the accessible variables to the ActuationModelFloatingBaseThrusters class. For example, I found that the allocation matrix is Wthrust only because I tried to access actuation.W_thrust_ after looking at the code, and I received a hint in the error message

I don't understand your question, as the answer is clearly explained in the error message. However, we bind members of our C++ classes in a way that makes them Pythonic.

In the documentation I don't find references to Wthrust, maybe I am looking in the wrong place?

The doxygen documentation in some classes is indeed missing at the moment. However, the Python documentation tends to be reasonably updated. Feel free to help us document classes.

[LorenzoBalandi]

Thank you for you reply.

Here is the code I used

from math import pi
import numpy as np
from numpy import matrix as mat
from casadi import cos, sin  # math

def axis_rot(axis, angle):
    """Compute the rotation matrix around a given axis (x, y, or z), angle in rad."""
    if axis == 'x':
        return mat([[1, 0, 0], [0, cos(angle), -sin(angle)], [0, sin(angle), cos(angle)]])
    elif axis == 'y':
        return mat([[cos(angle), 0, sin(angle)], [0, 1, 0], [-sin(angle), 0, cos(angle)]])
    elif axis == 'z':
        return mat([[cos(angle), -sin(angle), 0], [sin(angle), cos(angle), 0], [0, 0, 1]])

def GTMRP_props(n, l, alpha, beta, com=[0, 0, 0]):
    """
    Compute position and orientation of propellers in a Generically Tilted Multi-Rotor Platform.

    Inputs:
        n       Number of propellers
        l       Distance from propellers to CoM
        alpha   Alpha tilting angles in absolute value (rad)
        beta    Beta tilting angles in absolute value (rad)
        com     Position of the geometrical center of props wrt CoM

    Outputs:
        p       list of n positions (in body frame)
        R       list of n rotation matrices (in body frame)
    """
    p = [l * axis_rot('z', i * (pi / (n / 2))) @ mat('1; 0; 0') + mat(com).T for i in range(n)]
    R = [axis_rot('z', i * (pi / (n / 2))) @ axis_rot('x', (-1) ** i * alpha) @ axis_rot('y', beta) for i in range(n)]
    return p, R

# Allocation matrix for a GTMR
def GTMRP_matrix(R, p, c_f, c_t, sign=1):
    """
    Compute allocation matrices for a Generically Tilted Multi-Rotor Platform.

    Inputs:
        R       list of n (3x3) orientation matrices (one for each propeller)
        p       list of n (1x3) position vectors (one for each propeller)
        c_f     Propellers' force coefficient
        c_t     Propellers' torque coefficient
        sign    Rotation direction for 1st prop (-1:counter-clockwise, 1:clockwise !!!)

    Outputs:
        G_f: force allocation matrix
        G_t: torque allocation matrix
    """
    r = range(len(R))
    Riz = [R[i] @ mat('0;0;1') for i in r] # extract the last column (z axis) for each matrix
    # Riz is a list of z axes, Riz[i] is a 3x1 vector
    G_f = np.column_stack(Riz) # stack elements of Riz in a matrix 3xn
    G_t = np.column_stack([np.cross(p[i],Riz[i],0,0,0) + (c_t/c_f) * sign * (-1)**i * Riz[i] for i in r])
    return G_f, G_t

def compute_allocation(n, arm_length, alpha, beta, c_f, c_t, com=[0, 0, 0], sign=1):
    p, R = GTMRP_props(n, arm_length, np.deg2rad(alpha), np.deg2rad(beta), com)
    GF, GT = GTMRP_matrix(R, p, c_f, c_t, sign)
    return GF, GT

I didn't write these functions myself, they come from previous students at LAAS, but they should follow the cited paper.
To compute the allocation matrix G, I call

GF, GT = compute_allocation(n=6, arm_length=0.39, alpha=-20, beta=0, c_f=12.0e-4, c_tau=2.4e-5)
G = np.vstack((GF,GT))

while for using crocoddyl.ActuationModelFloatingBaseThrusters, I compute the lists of motors rotations and positions with

p, R = GTMRP_props(n=6, l=0.39, alpha=np.deg2rad(-20), beta=0)

and I use p and R as shown in the first post.

Regarding the question about documentation, my question was: how could I have found out before getting the hint that the mapping matrix computed by Crocoddyl is called Wthrust? Now I found out that one could use print(dir(crocoddyl.ActuationModelFloatingBaseThrusters)) or help(crocoddyl.ActuationModelFloatingBaseThrusters), but apart from this is there an official Python documentation?

In the near future I will use Crocoddyl to control the hexarotor with an arm attached (aerial manipulator similar to Borinot), so I can help with the documentation of the functions I will use where needed.