<?xml version="1.0" ?>
<!--
  Humanoid Lab World - Chapter 2 Beginner Demo

  A simple laboratory environment for learning Gazebo simulation.
  Includes:
  - Ground plane with friction
  - Basic lighting
  - Laboratory walls
  - Simple humanoid placeholder model

  Usage:
    gazebo --verbose humanoid_lab.world

  Or with ROS 2:
    ros2 launch gazebo_ros gazebo.launch.py world:=humanoid_lab.world
-->
<sdf version="1.6">
  <world name="humanoid_lab">

    <!-- Physics Configuration -->
    <physics type="ode">
      <real_time_update_rate>1000</real_time_update_rate>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <ode>
        <solver>
          <type>quick</type>
          <iters>50</iters>
          <sor>1.3</sor>
        </solver>
        <constraints>
          <cfm>0.0</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>100</contact_max_correcting_vel>
          <contact_surface_layer>0.001</contact_surface_layer>
        </constraints>
      </ode>
    </physics>

    <!-- Scene Configuration -->
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>true</shadows>
    </scene>

    <!-- Sun Light -->
    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <!-- Ceiling Light -->
    <light type="point" name="ceiling_light">
      <pose>0 0 4 0 0 0</pose>
      <diffuse>0.5 0.5 0.5 1</diffuse>
      <specular>0.1 0.1 0.1 1</specular>
      <attenuation>
        <range>20</range>
        <constant>0.5</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
    </light>

    <!-- Ground Plane -->
    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>20 20</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.0</mu>
                <mu2>1.0</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>20 20</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.1 0.1 0.1 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <!-- Lab Floor (Checkered Pattern Effect) -->
    <model name="lab_floor">
      <static>true</static>
      <pose>0 0 0.001 0 0 0</pose>
      <link name="link">
        <visual name="visual">
          <geometry>
            <box>
              <size>10 10 0.001</size>
            </box>
          </geometry>
          <material>
            <ambient>0.9 0.9 0.95 1</ambient>
            <diffuse>0.9 0.9 0.95 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- Lab Walls -->
    <!-- North Wall -->
    <model name="wall_north">
      <static>true</static>
      <pose>0 5 1.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>10 0.2 3</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>10 0.2 3</size>
            </box>
          </geometry>
          <material>
            <ambient>0.9 0.9 0.9 1</ambient>
            <diffuse>0.9 0.9 0.9 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- South Wall -->
    <model name="wall_south">
      <static>true</static>
      <pose>0 -5 1.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>10 0.2 3</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>10 0.2 3</size>
            </box>
          </geometry>
          <material>
            <ambient>0.9 0.9 0.9 1</ambient>
            <diffuse>0.9 0.9 0.9 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- East Wall -->
    <model name="wall_east">
      <static>true</static>
      <pose>5 0 1.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>0.2 10 3</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.2 10 3</size>
            </box>
          </geometry>
          <material>
            <ambient>0.85 0.85 0.9 1</ambient>
            <diffuse>0.85 0.85 0.9 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- West Wall -->
    <model name="wall_west">
      <static>true</static>
      <pose>-5 0 1.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>0.2 10 3</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.2 10 3</size>
            </box>
          </geometry>
          <material>
            <ambient>0.85 0.85 0.9 1</ambient>
            <diffuse>0.85 0.85 0.9 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- Simple Humanoid Placeholder -->
    <!-- This represents a basic humanoid shape for demonstration -->
    <!-- Replace with actual URDF humanoid model in practice -->
    <model name="humanoid_placeholder">
      <pose>0 0 0 0 0 0</pose>

      <!-- Torso -->
      <link name="torso">
        <pose>0 0 0.9 0 0 0</pose>
        <inertial>
          <mass>10</mass>
          <inertia>
            <ixx>0.1</ixx>
            <iyy>0.1</iyy>
            <izz>0.05</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.3 0.2 0.4</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.3 0.2 0.4</size>
            </box>
          </geometry>
          <material>
            <ambient>0.2 0.4 0.8 1</ambient>
            <diffuse>0.2 0.4 0.8 1</diffuse>
          </material>
        </visual>
      </link>

      <!-- Head -->
      <link name="head">
        <pose>0 0 1.25 0 0 0</pose>
        <inertial>
          <mass>2</mass>
          <inertia>
            <ixx>0.01</ixx>
            <iyy>0.01</iyy>
            <izz>0.01</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <sphere>
              <radius>0.12</radius>
            </sphere>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <sphere>
              <radius>0.12</radius>
            </sphere>
          </geometry>
          <material>
            <ambient>0.9 0.75 0.6 1</ambient>
            <diffuse>0.9 0.75 0.6 1</diffuse>
          </material>
        </visual>
      </link>

      <!-- Left Leg -->
      <link name="left_leg">
        <pose>0.08 0 0.4 0 0 0</pose>
        <inertial>
          <mass>5</mass>
          <inertia>
            <ixx>0.05</ixx>
            <iyy>0.05</iyy>
            <izz>0.01</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.05</radius>
              <length>0.6</length>
            </cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.05</radius>
              <length>0.6</length>
            </cylinder>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.3 0.3 0.3 1</diffuse>
          </material>
        </visual>
      </link>

      <!-- Right Leg -->
      <link name="right_leg">
        <pose>-0.08 0 0.4 0 0 0</pose>
        <inertial>
          <mass>5</mass>
          <inertia>
            <ixx>0.05</ixx>
            <iyy>0.05</iyy>
            <izz>0.01</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <cylinder>
              <radius>0.05</radius>
              <length>0.6</length>
            </cylinder>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <cylinder>
              <radius>0.05</radius>
              <length>0.6</length>
            </cylinder>
          </geometry>
          <material>
            <ambient>0.3 0.3 0.3 1</ambient>
            <diffuse>0.3 0.3 0.3 1</diffuse>
          </material>
        </visual>
      </link>

      <!-- Left Foot -->
      <link name="left_foot">
        <pose>0.08 0.05 0.05 0 0 0</pose>
        <inertial>
          <mass>1</mass>
          <inertia>
            <ixx>0.001</ixx>
            <iyy>0.001</iyy>
            <izz>0.001</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.08 0.15 0.03</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.0</mu>
                <mu2>1.0</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.08 0.15 0.03</size>
            </box>
          </geometry>
          <material>
            <ambient>0.2 0.2 0.2 1</ambient>
            <diffuse>0.2 0.2 0.2 1</diffuse>
          </material>
        </visual>
      </link>

      <!-- Right Foot -->
      <link name="right_foot">
        <pose>-0.08 0.05 0.05 0 0 0</pose>
        <inertial>
          <mass>1</mass>
          <inertia>
            <ixx>0.001</ixx>
            <iyy>0.001</iyy>
            <izz>0.001</izz>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyz>0</iyz>
          </inertia>
        </inertial>
        <collision name="collision">
          <geometry>
            <box>
              <size>0.08 0.15 0.03</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1.0</mu>
                <mu2>1.0</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>0.08 0.15 0.03</size>
            </box>
          </geometry>
          <material>
            <ambient>0.2 0.2 0.2 1</ambient>
            <diffuse>0.2 0.2 0.2 1</diffuse>
          </material>
        </visual>
      </link>

      <!-- Fixed Joints (for static placeholder) -->
      <joint name="head_joint" type="fixed">
        <parent>torso</parent>
        <child>head</child>
      </joint>

      <joint name="left_hip" type="fixed">
        <parent>torso</parent>
        <child>left_leg</child>
      </joint>

      <joint name="right_hip" type="fixed">
        <parent>torso</parent>
        <child>right_leg</child>
      </joint>

      <joint name="left_ankle" type="fixed">
        <parent>left_leg</parent>
        <child>left_foot</child>
      </joint>

      <joint name="right_ankle" type="fixed">
        <parent>right_leg</parent>
        <child>right_foot</child>
      </joint>
    </model>

    <!-- Lab Equipment: Table -->
    <model name="lab_table">
      <static>true</static>
      <pose>3 0 0.4 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>1.5 0.8 0.8</size>
            </box>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <box>
              <size>1.5 0.8 0.8</size>
            </box>
          </geometry>
          <material>
            <ambient>0.6 0.4 0.2 1</ambient>
            <diffuse>0.6 0.4 0.2 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- Lab Equipment: Monitor on Table -->
    <model name="monitor">
      <static>true</static>
      <pose>3 0 1.0 0 0 0</pose>
      <link name="link">
        <visual name="screen">
          <geometry>
            <box>
              <size>0.6 0.05 0.4</size>
            </box>
          </geometry>
          <material>
            <ambient>0.1 0.1 0.1 1</ambient>
            <diffuse>0.1 0.1 0.1 1</diffuse>
          </material>
        </visual>
      </link>
    </model>

    <!-- Gazebo ROS Plugin for clock -->
    <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
      <ros>
        <namespace>/gazebo</namespace>
      </ros>
      <update_rate>50</update_rate>
    </plugin>

  </world>
</sdf>
