Sample code for setting Fetch environments to the goal

[avandekleut]

In my research, I need an image of the goal. The current code for the environments generates goals randomly. To automatically get the agent to the goal, you can use the following example code:

class Settable(robotics.FetchPushEnv):
    def __init__(self, reward_type='sparse'):
        super().__init__(reward_type)

    def _set_to_goal(self, goal):
        """
        Goals are always xyz coordinates, either of gripper end effector or of object
        """
        if self.has_object:
            object_qpos = self.sim.data.get_joint_qpos('object0:joint')
            assert object_qpos.shape == (7,)
            object_qpos[:3] = goal
            self.sim.data.set_joint_qpos('object0:joint', object_qpos)
            
        self.sim.data.set_mocap_pos('robot0:mocap', goal)
        
        self.sim.forward()
        
        for _ in range(100):
            self.sim.step()

In general it only needs to inherit from FetchEnv, but for simplicity I inherit from a subclass here. This could also easily be implemented instead as a wrapper.

[jkterry1]

PR #2762 is about to be merged, introducing V4 MuJoCo environments using new bindings and a dramatically newer version of the engine. If this issue still persists with the V4 ones, please create a new issue for it.