Integrate webots

CMakeLists.txt

@@ -28,6 +28,7 @@ ENDIF(COMMAND cmake_policy)
 OPTION(ENABLE_TESTS_FLAG OFF)
 OPTION(ENABLE_EXAMPLES_FLAG OFF)
 OPTION(ENABLE_DOC_FLAG OFF)
+OPTION(ENABLE_WEBOTS OFF)
 
 SET(CMAKE_BUILD_TYPE "Debug")
 
@@ -69,6 +70,22 @@ ENDIF()
 
 LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
+IF(ENABLE_WEBOTS)
+	# we have webots include the directories
+	#INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/external/webots/src)
+	#LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/external/webots/lib)
+
+    SET(WEBOTS_LIB_DIRS ${PROJECT_SOURCE_DIR}/external/webots/lib/controller)    
+    SET(WEBOTS_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/external/webots/include/controller/cpp)
+
+	INCLUDE_DIRECTORIES(${WEBOTS_INCLUDE_DIRS})
+	LINK_DIRECTORIES(${WEBOTS_LIB_DIRS})
+	#MESSAGE( STATUS  "${WEBOTS_INCLUDE_DIRS}")
+
+	SET(RLENVSCPP_WEBOTS ON)
+
+ENDIF()
+
 configure_file(config.h.in ${PROJECT_SOURCE_DIR}/src/rlenvs/rlenvscpp_config.h @ONLY)
 configure_file(version.h.in ${PROJECT_SOURCE_DIR}/src/rlenvs/rlenvscpp_version.h @ONLY)
 
@@ -105,6 +122,9 @@ FILE(GLOB SRCS src/rlenvs/*.cpp
                #src/rlenvs/envs/gym_pybullet_drones/*.cpp
                src/rlenvs/envs/grid_world/*.cpp
 			   src/rlenvs/envs/connect2/*.cpp
+			   src/rlenvs/envs/webots_envs/*.cpp
+			   src/rlenvs/rigid_bodies/*.cpp
+			   src/rlenvs/rigid_bodies/webots_robots/*.cpp
 			   src/rlenvs/dynamics/*.cpp
 			   src/rlenvs/utils/*.cpp
 			   src/rlenvs/utils/io/*.cpp

config.h.in

@@ -7,5 +7,7 @@
 /*Use PyTorch */
 #cmakedefine USE_PYTORCH
 
+#cmakedefine RLENVSCPP_WEBOTS
+
 #endif
 

examples/CMakeLists.txt

@@ -2,6 +2,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
 
 INCLUDE_DIRECTORIES(${PROJECT_INCL_DIR}) 
 INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${WEBOTS_INCLUDE_DIRS})
 
 #SET(CMAKE_CXX_COMPILER /usr/bin/g++-11)
 #SET(CMAKE_C_COMPILER /usr/bin/gcc-11)
@@ -17,9 +18,11 @@ ENDIF()
 
 # use the Boost link directories
 LINK_DIRECTORIES(${Boost_LIBRARY_DIR})
+LINK_DIRECTORIES(${WEBOTS_LIB_DIRS})
 LINK_DIRECTORIES(${CMAKE_INSTALL_PREFIX})
 
 
+
 ADD_SUBDIRECTORY(example_1)
 ADD_SUBDIRECTORY(example_2)
 ADD_SUBDIRECTORY(example_3)
@@ -33,3 +36,4 @@ ADD_SUBDIRECTORY(example_10)
 ADD_SUBDIRECTORY(example_11)
 ADD_SUBDIRECTORY(example_12)
 ADD_SUBDIRECTORY(example_13)
+ADD_SUBDIRECTORY(webots/world_1)

examples/example_1/CMakeLists.txt

@@ -5,3 +5,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_10/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_11/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_12/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_13/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_2/CMakeLists.txt

@@ -5,3 +5,4 @@ SET(EXECUTABLE  example_2)
 
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_3/CMakeLists.txt

@@ -5,3 +5,4 @@ SET(EXECUTABLE  example_3)
 
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_4/CMakeLists.txt

@@ -5,4 +5,4 @@ SET(EXECUTABLE  example_4)
 
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_5/CMakeLists.txt

@@ -5,3 +5,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_6/CMakeLists.txt

@@ -5,4 +5,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_7/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_8/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/example_9/CMakeLists.txt

@@ -6,4 +6,4 @@ SET(SOURCE ${EXECUTABLE}.cpp)
 ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
 
 TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
-
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/webots/world_1/CMakeLists.txt

@@ -0,0 +1,8 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 3.20)
+
+SET(EXECUTABLE  world_1)
+SET(SOURCE ${EXECUTABLE}.cpp)
+
+ADD_EXECUTABLE(${EXECUTABLE} ${SOURCE})
+TARGET_LINK_LIBRARIES(${EXECUTABLE} rlenvscpplib)
+TARGET_LINK_LIBRARIES(${EXECUTABLE} CppController)

examples/webots/world_1/controllers/e_puck_controller/Makefile

@@ -0,0 +1,72 @@
+# Copyright 1996-2023 Cyberbotics Ltd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+### Generic Makefile.include for Webots controllers, physics plugins, robot
+### window libraries, remote control libraries and other libraries
+### to be used with GNU make
+###
+### Platforms: Windows, macOS, Linux
+### Languages: C, C++
+###
+### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
+###          Edmund Ronald, Sergei Poskriakov
+###
+###-----------------------------------------------------------------------------
+###
+### This file is meant to be included from the Makefile files located in the
+### Webots projects subdirectories. It is possible to set a number of variables
+### to customize the build process, i.e., add source files, compilation flags,
+### include paths, libraries, etc. These variables should be set in your local
+### Makefile just before including this Makefile.include. This Makefile.include
+### should never be modified.
+###
+### Here is a description of the variables you may set in your local Makefile:
+###
+### ---- C Sources ----
+### if your program uses several C source files:
+### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
+###
+### ---- C++ Sources ----
+### if your program uses several C++ source files:
+### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
+###
+### ---- Compilation options ----
+### if special compilation flags are necessary:
+CFLAGS = -std=c++20 -Wno-unused-result
+###
+### ---- Linked libraries ----
+### if your program needs additional libraries:
+INCLUDE = -I"/home/alex/qi3/rlenvs_from_cpp/src"
+LIBRARIES = -L"/home/alex/qi3/rlenvs_from_cpp/build" -lrlenvscpplib
+###
+### ---- Linking options ----
+### if special linking flags are needed:
+### LFLAGS = -s
+###
+### ---- Webots included libraries ----
+### if you want to use the Webots C API in your C++ controller program:
+### USE_C_API = true
+###
+### ---- Debug mode ----
+### if you want to display the gcc command line for compilation and link, as
+### well as the rm command details used for cleaning:
+VERBOSE = 1
+###
+###-----------------------------------------------------------------------------
+
+### Do not modify: this includes Webots global Makefile.include
+null :=
+space := $(null) $(null)
+WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
+include $(WEBOTS_HOME_PATH)/resources/Makefile.include

examples/webots/world_1/controllers/e_puck_controller/e_puck_controller.cpp

@@ -0,0 +1,121 @@
+// File:          e_puck_controller.cpp
+// Date:
+// Description:
+// Author:
+// Modifications:
+
+// You may need to add webots include files such as
+// <webots/DistanceSensor.hpp>, <webots/Motor.hpp>, etc.
+// and/or to add some other includes
+
+
+
+
+#include <webots/Robot.hpp>
+#include <iostream>
+#include <memory>
+#include <vector>
+#include <unordered_map>
+
+// we need this as all webots functionality is
+// under 
+#include "rlenvs/rlenvscpp_config.h"
+
+//#define RLENVSCPP_WEBOTS
+
+#ifdef RLENVSCPP_WEBOTS
+
+#include "rlenvs/rlenvs_types_v2.h"
+#include "rlenvs/envs/time_step.h"
+#include "rlenvs/envs/webots_envs/epuck_simple_grid_world.h"
+
+
+#include <iostream>
+
+// All the webots classes are defined in the "webots" namespace
+using namespace webots;
+using namespace rlenvscpp;
+
+namespace webots_example_1{
+
+
+
+using rlenvscpp::envs::webots_envs::EpuckSimpleGridWorld;
+
+
+}
+
+// This is the main program of your controller.
+// It creates an instance of your Robot instance, launches its
+// function(s) and destroys it at the end of the execution.
+// Note that only one instance of Robot should be created in
+// a controller program.
+// The arguments of the main function can be specified by the
+// "controllerArgs" field of the Robot node
+int main() {
+
+
+   using namespace webots_example_1;
+
+
+	// create the Robot instance.
+	//std::shared_ptr<Robot> robot = std::make_shared<Robot>();// Robot();
+
+	// create the environment
+	EpuckSimpleGridWorld env;
+
+	std::unordered_map<std::string, std::any> options;
+	options["right_motor_init_velocity"] = 0.25 / 2.0;
+	options["left_motor_init_velocity"] = 0.25 / 2.0;
+	env.make("v0", options); 
+
+           auto& robot = env.get_robot();
+
+           auto time_step = robot.get_basic_time_step();
+           std::cout<<"Basic time step used: "<<time_step<<std::endl;
+
+	for(uint_t s=0; s<10; ++s){
+
+	    robot.step(time_step);
+		auto odometry = robot.compute_odometry();
+	    std::cout<<"odometry"<<std::endl;
+
+	}
+
+  // get the time step of the current world.
+  //int timeStep = (int)robot->getBasicTimeStep();
+
+  //std::cout<<"Time step size: "<<timeStep<<std::endl;
+
+  // You should insert a getDevice-like function in order to get the
+  // instance of a device of the robot. Something like:
+  //  Motor *motor = robot->getMotor("motorname");
+  //  DistanceSensor *ds = robot->getDistanceSensor("dsname");
+  //  ds->enable(timeStep);
+
+  // Main loop:
+  // - perform simulation steps until Webots is stopping the controller
+//  while (robot->step(timeStep) != -1) {
+//    // Read the sensors:
+//    // Enter here functions to read sensor data, like:
+//    //  double val = ds->getValue();
+//
+//    // Process sensor data here.
+//
+//    // Enter here functions to send actuator commands, like:
+//    //  motor->setPosition(10.0);
+//  };
+//
+//  // Enter here exit cleanup code.
+//
+//  delete robot;
+  return 0;
+}
+#else
+#include <iostream>
+int main(){
+
+std::cout<<"This example requires Webots enabled. Rebuild the library with -DENABLE_WEBOTS=ON"<<std::endl;
+return 0;
+}
+#endif

examples/webots/world_1/world_1.cpp

@@ -0,0 +1,27 @@
+//#include "rlenvs/envs/webots_envs/epuck_simple_grid_world.h"
+#include "rlenvs/rlenvscpp_config.h"
+#include "rlenvs/envs/webots_envs/webots_base_env.h"
+#include "rlenvs/rlenvs_types_v2.h"
+#include "rlenvs/envs/time_step.h"
+#include "rlenvs/envs/env_types.h"
+
+using namespace rlenvscpp;
+using namespace rlenvscpp::envs;
+using namespace rlenvscpp::envs::webots_envs;
+
+typedef std::vector<real_t> state_type;
+typedef TimeStep<uint_t> time_step_type;
+typedef ScalarDiscreteEnv<1, 1> state_action_space_type;
+class MyClass: public WebotsEnvBase<time_step_type, state_action_space_type>
+{};
+
+int main() {
+
+	//using rlenvscpp::envs::webots_envs::EpuckSimpleGridWorld;
+	//EpuckSimpleGridWorld env;
+
+	return 0;
+
+}
+
+

examples/webots/world_1/worlds/.world.wbproj

@@ -0,0 +1,11 @@
+Webots Project File version R2024a
+perspectives: 000000ff00000000fd0000000200000001000003070000038cfc0200000001fb0000001400540065007800740045006400690074006f007201000000140000038c0000008800ffffff000000030000078000000064fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000004770000038c00000001000000020000000100000008fc00000000
+simulationViewPerspectives: 000000ff0000000100000002000001e4000002910100000002010000000101
+sceneTreePerspectives: 000000ff00000001000000030000001c000000c0000000fa0100000002010000000201
+minimizedPerspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f000000000000000000000001000003f3000003f2fc0200000001fb0000001400540065007800740045006400690074006f00720100000014000003f20000008900ffffff000000030000078000000153fc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000078000000000000000000000038b000003f200000004000000040000000100000008fc00000000
+maximizedDockId: -1
+centralWidgetVisible: 1
+orthographicViewHeight: 1
+textFiles: 0 "controllers/e_puck_controller/Makefile"
+consoles: Console:All:All
+renderingDevicePerspectives: e-puck:camera;1;1;0;0