bevy_urdf 0.4.3

use bevy::ecs::system::ScheduleSystem;
use bevy::{
    color::palettes::css::WHITE, input::common_conditions::input_toggle_active, prelude::*,
};
use bevy_infinite_grid::{InfiniteGridBundle, InfiniteGridPlugin};
use bevy_inspector_egui::bevy_egui::EguiPlugin;
use bevy_inspector_egui::quick::WorldInspectorPlugin;
use bevy_panorbit_camera::*;
use bevy_rapier3d::prelude::*;
use bevy_stl::StlPlugin;
use bevy_urdf::control::ControlMotorPositions;
use bevy_urdf::control::MotorProps;
use bevy_urdf::plugin::RobotType;
use bevy_urdf::spawn::{LoadRobot, RapierOption, RobotLoaded, SpawnRobot};
use bevy_urdf::urdf_asset_loader::UrdfAsset;
use bevy_urdf::URDFPlugin;

#[derive(Resource)]
struct UrdfRobotHandle(Option<Handle<UrdfAsset>>);

fn main() {
    App::new()
        .add_plugins((
            DefaultPlugins,
            URDFPlugin {
                default_system_setup: false,
                ..default()
            },
            StlPlugin,
            PanOrbitCameraPlugin,
            RapierPhysicsPlugin::<NoUserData>::default(),
            EguiPlugin {
                enable_multipass_for_primary_context: true,
            },
            InfiniteGridPlugin,
            WorldInspectorPlugin::default().run_if(input_toggle_active(false, KeyCode::Escape)),
        ))
        .init_state::<AppState>()
        .insert_resource(ClearColor(Color::linear_rgb(1.0, 1.0, 1.0)))
        .insert_resource(UrdfRobotHandle(None))
        .add_systems(
            Update,
            (
                URDFPlugin::<NoUserData>::get_systems(PhysicsSet::SyncBackend)
                    .in_set(PhysicsSet::SyncBackend),
                URDFPlugin::<NoUserData>::get_systems(PhysicsSet::StepSimulation)
                    .in_set(PhysicsSet::StepSimulation),
                URDFPlugin::<NoUserData>::get_systems(PhysicsSet::Writeback)
                    .in_set(PhysicsSet::Writeback),
            ),
        )
        .add_systems(Startup, setup)
        .add_systems(Update, start_simulation.run_if(in_state(AppState::Loading)))
        .add_systems(Update, (control_motors,))
        .run();
}

fn start_simulation(
    mut commands: Commands,
    mut er_robot_loaded: EventReader<RobotLoaded>,
    mut ew_spawn_robot: EventWriter<SpawnRobot>,
    mut state: ResMut<NextState<AppState>>,
) {
    for event in er_robot_loaded.read() {
        ew_spawn_robot.write(SpawnRobot {
            handle: event.handle.clone(),
            mesh_dir: event.mesh_dir.clone(),
            parent_entity: None,
            robot_type: RobotType::Manipulator,
            uav_descriptor: None,
            uuv_descriptor: None,
        });
        state.set(AppState::Simulation);
        commands.insert_resource(UrdfRobotHandle(Some(event.handle.clone())));
    }
}

#[derive(Debug, Clone, Copy, Default, Eq, PartialEq, Hash, States)]
enum AppState {
    #[default]
    Loading,
    Simulation,
}

#[allow(deprecated)]
fn setup(mut commands: Commands, mut ew_load_robot: EventWriter<LoadRobot>) {
    // Scene
    commands.insert_resource(AmbientLight {
        color: WHITE.into(),
        brightness: 300.0,
        ..default()
    });

    // ground
    commands.spawn((
        InfiniteGridBundle {
            transform: Transform::from_xyz(0.0, -1.0, 0.0),
            ..default()
        },
        RigidBody::Fixed,
        Collider::cuboid(900., 0.05, 900.),
    ));

    // camera
    commands.spawn((
        Camera3d { ..default() },
        Transform::from_xyz(-0.019, 0.32, -0.793).looking_at(Vec3::ZERO, Vec3::Y),
        PanOrbitCamera {
            focus: Vec3::new(0.0, 0.0, 0.0),
            ..Default::default()
        },
    ));

    // load robot
    ew_load_robot.send(LoadRobot {
        robot_type: RobotType::Manipulator,
        urdf_path: "manipulators/so-101/so101_new_calib.urdf".to_string(),
        mesh_dir: "assets/manipulators/so-101/".to_string(),
        rapier_options: RapierOption {
            interaction_groups: None,
            translation_shift: None,
            create_colliders_from_visual_shapes: false,
            create_colliders_from_collision_shapes: true,
            make_roots_fixed: true,
        },
        marker: None,
        uav_descriptor: None,
        uuv_descriptor: None,
    });
}

fn control_motors(
    robot_handle: Res<UrdfRobotHandle>,
    mut ew_control_motors: EventWriter<ControlMotorPositions>,
) {
    // lower="-0.174533" upper="1.74533
    // lower="-2.74385" upper="2.84121"
    // lower="-1.65806" upper="1.65806"
    // lower="-1.69" upper="1.69"
    // lower="-1.74533" upper="1.74533"
    // lower="-1.41986" upper="-1.11986"

    if let Some(handle) = robot_handle.0.clone() {
        let mut positions: Vec<f32> = Vec::new();
        let mut motor_props: Vec<MotorProps> = Vec::new();

        for i in 0..50 {
            let mut value = 1.0;
            // let i = i+1;
            if i == 0 {
                value = 0.0;
            }
            if i == 1 {
                value = (-2.74385 + 2.84121) / 2.0;
            }
            if i == 2 {
                value = (-1.65806 + 1.65806) / 2.0;
            }
            if i == 3 {
                value = (-1.69 + 1.69) / 2.0;
            }
            if i == 4 {
                value = (-1.74533 + 1.74533) / 2.0;
            }
            if i == 5 {
                value = 3.14; // (-1.41986 - 1.11986) / 2.0 + 1.0;
            }

            positions.push(value); // rng.random_range(0.0..5.0));
            motor_props.push(MotorProps {
                stiffness: 17.8,
                damping: 0.6,
            });
        }

        ew_control_motors.write(ControlMotorPositions {
            handle,
            positions,
            motor_props,
        });
    }
}