Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

RPM Motor Mapping #196

Open
zcase opened this issue Mar 15, 2024 · 6 comments
Open

RPM Motor Mapping #196

zcase opened this issue Mar 15, 2024 · 6 comments
Labels
question Further information is requested

Comments

@zcase
Copy link

zcase commented Mar 15, 2024

@JacopoPan I am looking into using the RL training that uses the RPM setting. I was curious how the motors map to that of the RPM output? Does index 0 of the output array correspond to that of the prop0 in the URDF file?

Also I have seen things out there showing different configurations for motor layouts like found here:

https://ardupilot.org/copter/docs/connect-escs-and-motors.html

How does this relate to what you have or is it like a stated above that index 0 = prop0, index 1 = prop1, ..., index N =propN?

Thank you!

@JacopoPan JacopoPan added the question Further information is requested label Mar 16, 2024
@JacopoPan
Copy link
Member

Hi @zcase the mapping between the action and PyBullet/the URDF is in line 702 of BaseAviary, index i is the i-th link the the URDF file

for i in range(4):
p.applyExternalForce(self.DRONE_IDS[nth_drone],
i,
forceObj=[0, 0, forces[i]],
posObj=[0, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self.CLIENT
)

You are correct the different standards are use by different autopilots, e.g. in BetaAviary the remapping is

remapped_input = np.array([[
action[i][2], # 0
action[i][1], # 1
action[i][3], # 2
action[i][0] # 3

@zcase
Copy link
Author

zcase commented Mar 18, 2024

@JacopoPan This is great thank you. Do you by chance have any links to the documents that discuss what you did for the Crazyflie drone and the beta flight stuff?

@JacopoPan
Copy link
Member

JacopoPan commented Mar 19, 2024

The Python binding for the Crazyflie firmware are discussed in this technical report https://arxiv.org/pdf/2308.16743.pdf
For Betaflight, I simply used the SITL target https://github.com/betaflight/betaflight (changing the UDP ports it uses for multiple drones) and the position controller from https://github.com/tii-racing/drone-racing-dataset

@zcase
Copy link
Author

zcase commented May 10, 2024

@JacopoPan The links you provided have been helpful, but I think I may be confusing myself and am looking for a little guidance.

Based off of what you said, the action space is:

action = [prop0_link_action, prop1_link_action, prop2_link_action, prop3_link_action]

To make sure I'm on the right page here is what I have found regarding the crazyflie motors (left) and the mapping of the action space to it (on the right)

source (https://www.bitcraze.io/documentation/system/platform/cf2-components/)

For Betaflight motors, I found the following:

source (https://betaflight.com/docs/wiki/configurator/motors-tab)

image

source (https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/)

Crazyflie: (URDF, Motors, Action Mapping
URDF prop0_link (x=+, y=-) = M2 = rl action index 0
URDF prop1_link (x=-, y=-) = M3 = rl action index 1
URDF prop2_link (x=-, y=+) = M4 = rl action index 2
URDF prop3_link (x=+, y=+) = M1 = rl action index 3

QUESTION 1
Assuming action = [prop0_link_action, prop1_link_action, prop2_link_action, prop3_link_action] and based on the source code and URDF files, how does the following map out this way?:

Crazyflie + Betaflight: (BM = Betaflight Motor) (CM = Crazyflie motor)
URDF prop0_link (x=+, y=-) = CM2 = BM1 = rl action index 2
URDF prop1_link (x=-, y=-) = CM3 = BM3 = rl action index 1
URDF prop2_link (x=-, y=+) = CM4 = BM4 = rl action index 3
URDF prop3_link (x=+, y=+) = CM1 = BM2 = rl action index 0

The following image is the shows the Betaflight motor layout with its actions in the YELLOW As while in Red 1-4 are the Crazyflie motors and A0-A3 are the Crazyflie actions on their own (without Betaflight)

I would have assumed you might only swap the actions for motors 1 and 2.

QUESTION 2
I'm not understanding how that mapping is working with the Crazyflie when using the Betaflight, which is making it hard to map my action space to an ardupilot X frame drone in the ENU reference frame. (ENU like the rest of the simulation env)

Ardupilot has this motor mapping

source (https://ardupilot.org/copter/docs/connect-escs-and-motors.html)

QUESTION 3
Does the ordering of the prop_links need to be same? For my ardupilot drone the prop_links are in a different order.

I really appreciate your help with this. It is probably something really silly that I'm missing. I just can't see it for some reason.

@JacopoPan
Copy link
Member

Hi @zcase

I think there are at least 2 misunderstandings here

@zcase
Copy link
Author

zcase commented May 13, 2024

@JacopoPan,

Ok that helps a bit with the URDF stuff but not sure I see the relation to the motor mapping and the actions.

You mentioned:

the mapping between the action and PyBullet/the URDF is in line 702 of BaseAviary, index i is the i-th link the the URDF file

Is this just for the Crazyflie base training?

Crazyflie: (URDF, Motors, Action Mapping)
URDF prop0_link (x=+, y=-) = M2 = rl action index 0
URDF prop1_link (x=-, y=-) = M3 = rl action index 1
URDF prop2_link (x=-, y=+) = M4 = rl action index 2
URDF prop3_link (x=+, y=+) = M1 = rl action index 3

Based on your latest response, I have updated the Racer Mapping here:

Racer/Betaflight: (URDF, Motors, Action Mapping)
URDF prop0_link (x=+, y=+) = M2 = rl action index 2
URDF prop1_link (x=-, y=+) = M4 = rl action index 1
URDF prop2_link (x=-, y=-) = M3 = rl action index 3
URDF prop3_link (x=+, y=-) = M1 = rl action index 0

Racer/Betaflight Ordered to match +- CrazyFlie Ordering above: (Starting at bottom right and going counterclockwise
URDF prop3_link (x=+, y=-) = M1 = rl action index 0
URDF prop2_link (x=-, y=-) = M3 = rl action index 3
URDF prop1_link (x=-, y=+) = M4 = rl action index 1
URDF prop0_link (x=+, y=+) = M2 = rl action index 2

But for the racer urdf and action mapping, why isn't the action mapping just:

  • prop0 = index 0
  • prop1 = index 1
  • prop2 = index 2
  • prop3 = index 3

Instead we get:

  • prop0 = index 2
  • prop1 = index 1
  • prop2 = index 3
  • prop3 = index 0

Is this because like you mentioned:

the action space of BetaAviary is in CTBR (collective thrust and body rates),

And if we want the action space to represent the RPMs we need convert it to the global ENU space or to the body?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
question Further information is requested
Projects
None yet
Development

No branches or pull requests

2 participants