slim71
June 18, 2023, 5:39pm
1
Hi there,
I’m currently using a drone model which contains a pair of landing legs, which are simply connected via revolute joints to the main body.
Is there any way to make them start in the down position at the start of the Gazebo simulation, and then make them raise later?
Seeing how rotors are controlled, my guess is that I need either an ESC or better yet a servo for each of them… But:
I don’t know any Gazebo plugin to control servos
Even managing with an ESC, how do I make them start with an initial value and change automatically after takeoff?
Any help is appreciated!
EDIT: forgot to mention that I’ve also thought about using a SIM_GZ_EC_FUNCn parameter with value 400 (landing gear) or 201-208 (servos), but could not understand how to use those in simulations
1 Like
Benja
June 18, 2023, 9:56pm
2
Hi @slim71
That’s a nice model!
To have PX4 able to control you landing gear you can take a look at the rc_cessna
model:
First, from the model.sdf
of the cessna you can see that the JointPositionController
is used to control the control surfaces.
PX4-Autopilot/Tools/simulation/gz/models/rc_cessna/model.sdf at 3303323971f02cff6eb2fc029562bc4309b69b5d · PX4/PX4-Autopilot · GitHub
You can use the same to control your joints (there are a lot of other parameter that you can set for that plugin, see gz-sim/src/systems/joint_position_controller/JointPositionController.hh at 001dd9b829eb8e8b4465d87c6c70ccf4ee2e6b6a · gazebosim/gz-sim · GitHub
Next, you have to tell the control allocation module to use that servo, again, use the cessna airframe file as reference: PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna at 3303323971f02cff6eb2fc029562bc4309b69b5d · PX4/PX4-Autopilot · GitHub
The most important parameters are SIM_GZ_SV_FUNC*
(replace *
with the servo number-1
, therefore if you set <sub_topic>servo_0</sub_topic>
in the sdf file, you need to modify SIM_GZ_SV_FUNC1
. Refer to the SIM_GZ_*
parameters for their uses. SIM_GZ_SV_FUNC*
can assume many values, see Actuator Configuration and Testing | PX4 User Guide (main)
There is landing gear among them, but I don’t know if it is supported by the multicopter logic. You can use RC AUX
then, or Offboard Acutator Set
this is the right direction but you also need to tell gazebo, using the sdf file, how to use the data coming from PX4
slim71
June 19, 2023, 10:44am
3
First, from the model.sdf
of the cessna you can see that the JointPositionController
is used to control the control surfaces.
PX4-Autopilot/Tools/simulation/gz/models/rc_cessna/model.sdf at 3303323971f02cff6eb2fc029562bc4309b69b5d · PX4/PX4-Autopilot · GitHub
You can use the same to control your joints (there are a lot of other parameter that you can set for that plugin, see gz-sim/src/systems/joint_position_controller/JointPositionController.hh at 001dd9b829eb8e8b4465d87c6c70ccf4ee2e6b6a · gazebosim/gz-sim · GitHub
that’s what I was missing!
it seems easy enough now, I’ll try it as soon as I can and see if it works!
man, I still have a loooong way to go with these tools… Thank you very much!
1 Like
slim71
June 19, 2023, 5:12pm
4
Thanks a lot @Benja !
I’ve managed to make the legs retract or deploy depending on the takeoff/landing phase!
I’ve just added the plugin to the sdf model
<plugin filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>left_leg_joint</joint_name>
<sub_topic>servo_0</sub_topic>
<p_gain>10.0</p_gain>
</plugin>
<plugin filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>right_leg_joint</joint_name>
<sub_topic>servo_0</sub_topic>
<p_gain>10.0</p_gain>
</plugin>
and these parameters to the airframe file
# Landing gear
param set-default RWTO_TKOFF 1
param set-default FW_W_EN 1
param set-default SIM_GZ_SV_FUNC1 400
All’s good!
1 Like
Benja
June 19, 2023, 6:09pm
5
Great!!!
Just for your reference, you can control two or more joints with just one plugin, like this:
<plugin filename="gz-sim-joint-position-controller-system"
name="gz::sim::systems::JointPositionController">
<joint_name>left_leg_joint</joint_name>
<joint_name>right_leg_joint</joint_name>
<sub_topic>servo_0</sub_topic>
<p_gain>10.0</p_gain>
</plugin>
2 Likes
@Benja
Hi, Benja,
I saw it was done based on the Gazebo Graden, and I was wondering whether I can immigrate it into Gazebo Classic in Ubuntu 20.04.
Do you have any suggestions? I do need a servo motor plugin for my simulation.
Many thanks!
Allen