-
Notifications
You must be signed in to change notification settings - Fork 214
ActuationModelFloatingBaseThrusters() with tilted thrusters #1375
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
All reactions
Replies: 2 comments 3 replies
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.
All reactions
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.
All reactions
-
👍 1
This line looks buggy:
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])
I believe it should be
G_t = np.column_stack([np.cross(p[i],Riz[i]) + (c_t/c_f) * sign * (-1)**i * Riz[i] for i in r])
I would like to provide more realistic examples of aerial manipulation. Would you like to help us include your robot inside example-robot-data?
Finally, we already have some examples of aerial manipulation with our advanced solvers. We haven't yet released in Crocoddyl, but if you are interested, we can schedule a meeting to discuss your needs and potential collaboration paths.
Screen.Recording.2025年04月28日.at.18.55.47.mp4
All reactions
Hi,
I'm not sure that line is the problem. np.cross() expects vectors of dimension (3,), but p[i] and Riz[i] are (3,1), so specifying axisa=0, axisb=0, axisc=0 avoids a dimension error and creates a 3x1 vector suitable to be stacked with np.column_stack().
The following image depicts the body frame considered for the hexarotor.
image
The allocation matrix I obtain with zero tilting angle, with both the functions above and crocoddyl.ActuationModelFloatingBaseThrusters is
[[ 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00]
[ 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00 0.000e+00]
[ 1.000e+00 1.000e+00 1.000e+00 1.000e+00 1.000e+00 1.000e+00]
[ 0.000e+00 3.377e-01 3.377e-01 4.776e-17 -3.377e-01 -3.377e-01]
[-3.900e-01 -1.950e-01 1.950e-01 3.900e-01 1.950e-01 -1.950e-01]
[ 2.000e-02 -2.000e-02 2.000e-02 -2.000e-02 2.000e-02 -2.000e-02]]
reasoning colmn-wise, the i-th column is the contribution of the i-th motor to the body wrench. This matrix is coherent because in this case the vehicle is underactuated (similar to borinot) and cannot exert forces in xy , so the first two lines are zero while all the thrust contributes to z.
If I change the tilting angle to -20 deg (as in the real robot), the linear part of the allocation matrix is the same with both the functions above and crocoddyl, but the rotational parts are respectively
[ 0.00e+00 3.11e-01 3.11e-01 4.40e-17 -3.11e-01 -3.11e-01]
[-3.60e-01 -1.80e-01 1.80e-01 3.60e-01 1.80e-01 -1.80e-01]
[ 1.52e-01 -1.52e-01 1.52e-01 -1.52e-01 1.52e-01 -1.52e-01]]
and
[ 0.000e+00 3.318e-01 3.318e-01 4.692e-17 -3.318e-01 -3.318e-01]
[-3.832e-01 -1.916e-01 1.916e-01 3.832e-01 1.916e-01 -1.916e-01]
[ 1.879e-02 -1.879e-02 1.879e-02 -1.879e-02 1.879e-02 -1.879e-02]]
The signs are ok, there is something wrong in the amplitudes.
by further changing the tilting angle to -90 (extreme case), the rotational allocation matrix I obtain with crocoddyl is
[ 0.000e+00 3.204e-01 3.204e-01 4.531e-17 -3.204e-01 -3.204e-01]
[-3.700e-01 -1.850e-01 1.850e-01 3.700e-01 1.850e-01 -1.850e-01]
[ 1.225e-18 -1.225e-18 1.225e-18 -1.225e-18 1.225e-18 -1.225e-18]]
which In my opinion is not correct because the motors are maximally contributing to the torque in z, and the last line shouldn't be zero.
How did you obtain the video on the right?
It could be good to include our aerial manipulator in example-robot-data! At the moment we have the tilthex platform + 2 dof Bolt leg (soon 3 dof). we have the urdf model with meshes from the CAD. Since it is still an ongoing work, we have to discuss and finalize it.
I am definitely interested in aerial manipulation, I am currently working on the full body control of our plarform using crocoddyl + mim_solvers. In the next months I will visit NYU with prof. Righetti to extend our framework to manage contacts, so I foresee a large use of crocoddyl.
It will be interesting to discuss about this.
All reactions
Hi,
thanks to @skleff1994, we found that the solution was simply to update the Crocoddyl version to >=3.
All my previous comments were using version 2.1.0, I tried in another conda environment with version 3.0.0 and the two allocation matrices match.
I'm not sure what the problem was, because it seems there were no changes to that actuation model since version 2.1.0, but at least now the problem is solved.