Creation of Custom FetchObstaclePickAndPlace environment faces issue with adding obstacle data in the observation of the environment

[ChristosPeridis]

Hello dear members of the Farama Team!

I have made a fork of the Gymnasium-Robotics API. I am working on creating a custom environment, based on the 'FetchPickAndPlace-v2' environment. This new environment will introduce an obstacle in the simulation. I have created the appropriate .xml file that introduces the obstacle, which is named 'objstacle0'. For this environment I wanted to add to the original observation of the 'FetchPickAnPlace' environment the following:

1. The position of the obstacle in the 3D space.
2. The relative position of the robot gripper and the obstacle.
3. The relative position of the object and the obstacle.

However, after creating the environment and performing some steps in it, the environment seems to return 0 values for the new three values added to the observation. An example can be seen in the picture bellow:

To further debug this behaviour I imported mujoco and I made the mujoco_utils from hidden to public attribute of the environment, so I can access the low level methods that extract the data from the MuJoCo simulator. I was surprised to see that the obstacle and the object where returning the same positions:

I retrieved this data by using the 'get_site_xpos()' method of the 'mujoco_utils' package. However, that is definitely not the case because from the observation we can see that the achieved goal has different value. Furthermore you can see from the image of the simulation that the object is in different position to the one of the obstacle.

Here is the overwritten version of the 'generate_mujoco_observations()' method that I am using:

def generate_mujoco_observations(self):
# positions
grip_pos = self._utils.get_site_xpos(self.model, self.data, "robot0:grip")

    dt = self.n_substeps * self.model.opt.timestep
    grip_velp = (
        self._utils.get_site_xvelp(self.model, self.data, "robot0:grip") * dt
    )

    robot_qpos, robot_qvel = self._utils.robot_get_obs(
        self.model, self.data, self._model_names.joint_names
    )
    if self.has_object:
        object_pos = self._utils.get_site_xpos(self.model, self.data, "object0")
        # rotations
        object_rot = rotations.mat2euler(
            self._utils.get_site_xmat(self.model, self.data, "object0")
        )
        # velocities
        object_velp = (
            self._utils.get_site_xvelp(self.model, self.data, "object0") * dt
        )
        object_velr = (
            self._utils.get_site_xvelr(self.model, self.data, "object0") * dt
        )
        # gripper state
        object_rel_pos = object_pos - grip_pos
        object_velp -= grip_velp
    else:
        object_pos = (
            object_rot
        ) = object_velp = object_velr = object_rel_pos = np.zeros(0)
    gripper_state = robot_qpos[-2:]

    gripper_vel = (
        robot_qvel[-2:] * dt
    )  # change to a scalar if the gripper is made symmetric
    
    # Extract the positions of the gripper and the object
    #grip_pos = self._utils.get_site_xpos(self.model, self.data, "robot0:grip")
    #object_pos = self._utils.get_site_xpos(self.model, self.data, "object0")
    
    # Calculate the obstacle's position (assuming its site name is "obstacle")
    obstacle_pos = self._utils.get_site_xpos(self.model, self.data, "obstacle0")
    
    # Calculate the relative positions
    gripper_to_obstacle = obstacle_pos - grip_pos
    object_to_obstacle = obstacle_pos - object_pos
    
    # Extend the original observations with the relative positions
    #extended_observations = observations + (gripper_to_obstacle,) + (object_to_obstacle,)        
    return (
        grip_pos,
        object_pos,
        object_rel_pos,
        gripper_state,
        object_rot,
        object_velp,
        object_velr,
        grip_velp,
        gripper_vel,
        obstacle_pos,
        gripper_to_obstacle,
        object_to_obstacle
    )

And here the code of the obstacle_pick_and_place.xml:

<include file="shared.xml" />

<worldbody>
	<geom name="floor0" pos="0.8 0.75 0" size="0.85 0.7 1" type="plane" condim="3" material="floor_mat" />
	<body name="floor0" pos="0.8 0.75 0">
		<site name="target0" pos="0 0 0.5" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere" />
	</body>

	<include file="robot.xml" />
	
	<body pos="1.3 0.75 0.2" name="table0">
		<geom size="0.25 0.35 0.2" type="box" mass="2000" material="table_mat" />
	</body>

	<body pos="1.3 0.75 0.48" name="obstacle0">
		<geom  size="0.025 0.35 0.075" type="box" rgba="0.6 0.3 0 1" />
	</body>
	
	<body name="object0" pos="0.025 0.025 0.025">
		<joint name="object0:joint" type="free" damping="0.01" />
		<geom size="0.025 0.025 0.025" type="box" condim="3" name="object0" material="block_mat" mass="2" />
		<site name="object0" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere" />
	</body>

	<light directional="true" ambient="0.2 0.2 0.2" diffuse="0.8 0.8 0.8" specular="0.3 0.3 0.3" castshadow="false" pos="0 0 4" dir="0 0 -1" name="light0" />
</worldbody>

<actuator>
	<position ctrllimited="true" ctrlrange="0 0.2" joint="robot0:l_gripper_finger_joint" kp="30000" name="robot0:l_gripper_finger_joint" user="1" />
	<position ctrllimited="true" ctrlrange="0 0.2" joint="robot0:r_gripper_finger_joint" kp="30000" name="robot0:r_gripper_finger_joint" user="1" />
</actuator>

Could you please help me debug and fix this behaviour ?

Thank you very much in advance for all the valuable help and support !!!

Kind regards,

Christos Peridis