Add the prototype robot plugin

src/plugins/robots/CMakeLists.txt

@@ -19,3 +19,7 @@ endif(ARGOS_BUILD_FOR_SIMULATOR OR ARGOS_BUILD_FOR STREQUAL "e-puck")
 if(ARGOS_BUILD_FOR_SIMULATOR OR ARGOS_BUILD_FOR STREQUAL "spiri")
   add_subdirectory(spiri)
 endif(ARGOS_BUILD_FOR_SIMULATOR OR ARGOS_BUILD_FOR STREQUAL "spiri")
+
+if(ARGOS_BUILD_FOR_SIMULATOR)
+  add_subdirectory(prototype)
+endif(ARGOS_BUILD_FOR_SIMULATOR)

src/plugins/robots/prototype/CMakeLists.txt

@@ -0,0 +1,90 @@
+#
+# Prototype headers
+#
+# argos3/plugins/robots/prototype/control_interface
+set(ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_CONTROLINTERFACE
+  control_interface/ci_prototype_joints_sensor.h
+  control_interface/ci_prototype_joints_actuator.h)
+# argos3/plugins/robots/prototype/simulator 
+if(ARGOS_BUILD_FOR_SIMULATOR)
+  set(ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR
+    simulator/prototype_joints_default_actuator.h
+    simulator/prototype_entity.h
+    simulator/prototype_joint_entity.h
+    simulator/prototype_joint_equipped_entity.h
+    simulator/prototype_link_entity.h
+    simulator/prototype_link_equipped_entity.h
+    simulator/dynamics3d_prototype_model.h
+    simulator/prototype_joints_default_sensor.h)
+endif(ARGOS_BUILD_FOR_SIMULATOR)
+
+#
+# Prototype sources
+#
+# argos3/plugins/robots/prototype/control_interface
+set(ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE
+  ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_CONTROLINTERFACE}
+  control_interface/ci_prototype_joints_actuator.cpp
+  control_interface/ci_prototype_joints_sensor.cpp)
+# argos3/plugins/robots/prototype/simulator 
+if(ARGOS_BUILD_FOR_SIMULATOR)
+  set(ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE
+    ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE}
+    ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR}
+    simulator/dynamics3d_prototype_model.cpp
+    simulator/prototype_entity.cpp
+    simulator/prototype_joint_entity.cpp
+    simulator/prototype_joint_equipped_entity.cpp
+    simulator/prototype_joints_default_actuator.cpp
+    simulator/prototype_joints_default_sensor.cpp
+    simulator/prototype_link_entity.cpp
+    simulator/prototype_link_equipped_entity.cpp)
+  # Compile the graphical visualization only if the necessary libraries have been found
+  include(ARGoSCheckQTOpenGL)
+  if(ARGOS_COMPILE_QTOPENGL)
+    set(ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR
+      ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR}
+      simulator/qtopengl_prototype.h)
+    set(ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE
+      ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE}
+      simulator/qtopengl_prototype.cpp)
+  endif(ARGOS_COMPILE_QTOPENGL)
+endif(ARGOS_BUILD_FOR_SIMULATOR)
+
+#
+# Create the prototype plugin
+#
+
+include_directories(
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/BroadphaseCollision
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/CollisionDispatch
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/CollisionShapes
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/Gimpact
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/NarrowPhaseCollision
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/ConstraintSolver
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Dynamics
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Vehicle
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Character
+   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/LinearMath)
+
+add_library(argos3plugin_${ARGOS_BUILD_FOR}_prototype SHARED ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE})
+target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_prototype
+  argos3plugin_${ARGOS_BUILD_FOR}_genericrobot
+  argos3plugin_${ARGOS_BUILD_FOR}_dynamics3d)
+if(ARGOS_COMPILE_QTOPENGL)
+  target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_prototype argos3plugin_${ARGOS_BUILD_FOR}_qtopengl)
+endif(ARGOS_COMPILE_QTOPENGL)
+
+install(FILES ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_CONTROLINTERFACE}
+        DESTINATION include/argos3/plugins/robots/prototype/control_interface)
+
+if(ARGOS_BUILD_FOR_SIMULATOR)
+  install(FILES ${ARGOS3_HEADERS_PLUGINS_ROBOTS_PROTOTYPE_SIMULATOR}
+          DESTINATION include/argos3/plugins/robots/prototype/simulator)
+endif(ARGOS_BUILD_FOR_SIMULATOR)
+
+install(TARGETS argos3plugin_${ARGOS_BUILD_FOR}_prototype
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib/argos3
+  ARCHIVE DESTINATION lib/argos3)

src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.cpp

@@ -0,0 +1,55 @@
+/**
+ * @file <argos3/plugins/robots/prototype/control_interface/ci_robot_joints_actuator.cpp>
+ *
+ * @author Michael Allwright - <[email protected]>
+ */
+
+#include "ci_prototype_joints_actuator.h"
+
+#ifdef ARGOS_WITH_LUA
+#include <argos3/core/wrappers/lua/lua_utility.h>
+#endif
+
+namespace argos {
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   int LuaSetTarget(lua_State* pt_lua_state) {
+      /* get a reference to the actuator */     
+      CCI_PrototypeJointsActuator::SActuator* pc_actuator = 
+         reinterpret_cast<CCI_PrototypeJointsActuator::SActuator*>(lua_touserdata(pt_lua_state, lua_upvalueindex(1)));
+      /* check number of arguments */
+      if(lua_gettop(pt_lua_state) != 1) {
+         std::string strErrMsg = "robot.joints." + pc_actuator->Id + ".set_target() requires 1 argument";
+         return luaL_error(pt_lua_state, strErrMsg.c_str());
+      }
+      luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
+      /* Perform action */
+      pc_actuator->Target = lua_tonumber(pt_lua_state, 1);
+      return 0;
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   void CCI_PrototypeJointsActuator::CreateLuaState(lua_State* pt_lua_state) {
+      CLuaUtility::StartTable(pt_lua_state, "joints");
+      for(SActuator* pc_actuator : m_vecActuators) {
+         CLuaUtility::StartTable(pt_lua_state, pc_actuator->Id);
+         /* push a pointer to each actuator onto the lua stack */
+         lua_pushstring(pt_lua_state, "set_target");
+         lua_pushlightuserdata(pt_lua_state, pc_actuator);
+         lua_pushcclosure(pt_lua_state, &LuaSetTarget, 1);
+         lua_settable(pt_lua_state, -3);
+         CLuaUtility::EndTable(pt_lua_state);
+      }
+      CLuaUtility::EndTable(pt_lua_state);
+   }
+#endif
+
+
+}

src/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.h

@@ -0,0 +1,46 @@
+/**
+ * @file <argos3/plugins/robots/prototype/control_interface/ci_prototype_joints_actuator.h>
+ *
+ * @author Michael Allwright - <[email protected]>
+ */
+
+#ifndef CCI_PROTOTYPE_JOINTS_ACTUATOR_H
+#define CCI_PROTOTYPE_JOINTS_ACTUATOR_H
+
+namespace argos {
+   class CCI_PrototypeJointsActuator;
+}
+
+#include <argos3/core/control_interface/ci_actuator.h>
+#include <argos3/core/utility/math/angles.h>
+
+namespace argos {
+
+   class CCI_PrototypeJointsActuator : virtual public CCI_Actuator {
+
+   public:
+      struct SActuator {
+         using TVector = std::vector<SActuator*>;
+         SActuator(const std::string& str_id, Real f_target) :
+            Id(str_id),
+            Target(f_target) {}
+         std::string Id;
+         Real Target;
+      };
+
+   public:
+      /**
+       * Destructor.
+       */
+      virtual ~CCI_PrototypeJointsActuator() {}
+
+#ifdef ARGOS_WITH_LUA
+      virtual void CreateLuaState(lua_State*);
+#endif
+
+   protected: 
+      SActuator::TVector m_vecActuators;
+   };
+}
+
+#endif

src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.cpp

@@ -0,0 +1,51 @@
+/**
+ * @file <argos3/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.cpp>
+ *
+ * @author Michael Allwright - <[email protected]>
+ */
+
+#include "ci_prototype_joints_sensor.h"
+
+#ifdef ARGOS_WITH_LUA
+#include <argos3/core/wrappers/lua/lua_utility.h>
+#endif
+
+namespace argos {
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   void CCI_PrototypeJointsSensor::CreateLuaState(lua_State* pt_lua_state) {
+      /* CLuaUtility::OpenRobotStateTable is required since we don't know 
+         whether the joint actuator has been added to the robot nor do we
+         know which joints it may have added to the robot's table in Lua */
+      CLuaUtility::OpenRobotStateTable(pt_lua_state, "joints");
+      for(SSensor* pc_sensor : m_vecSensors) {
+         CLuaUtility::OpenRobotStateTable(pt_lua_state, pc_sensor->Id);     
+         CLuaUtility::AddToTable(pt_lua_state, "encoder", pc_sensor->Value);
+         CLuaUtility::CloseRobotStateTable(pt_lua_state);
+      }
+      CLuaUtility::CloseRobotStateTable(pt_lua_state);
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   void CCI_PrototypeJointsSensor::ReadingsToLuaState(lua_State* pt_lua_state) {
+      lua_getfield(pt_lua_state, -1, "joints");
+      for(SSensor* pc_sensor : m_vecSensors) {
+         lua_getfield(pt_lua_state, -1, (pc_sensor->Id).c_str());
+         CLuaUtility::AddToTable(pt_lua_state, "encoder", pc_sensor->Value);
+         lua_pop(pt_lua_state, 1);
+      }
+      lua_pop(pt_lua_state, 1);
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+}

src/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.h

@@ -0,0 +1,50 @@
+/**
+ * @file <argos3/plugins/robots/prototype/control_interface/ci_prototype_joints_sensor.h>
+ *
+ * @author Michael Allwright - <[email protected]>
+ */
+
+#ifndef CCI_PROTOTYPE_JOINTS_SENSOR_H
+#define CCI_PROTOTYPE_JOINTS_SENSOR_H
+
+namespace argos {
+   class CCI_PrototypeJointsSensor;
+}
+
+#include <argos3/core/control_interface/ci_sensor.h>
+#include <argos3/core/utility/math/quaternion.h>
+#include <argos3/core/utility/math/vector3.h>
+
+namespace argos {
+
+   class CCI_PrototypeJointsSensor : virtual public CCI_Sensor {
+
+   public:
+
+      struct SSensor {
+         using TVector = std::vector<SSensor*>;
+         SSensor(const std::string& str_id) :
+            Id(str_id) {}
+         std::string Id;
+         Real Value;
+      };
+
+   public:
+
+      /**
+       * Destructor.
+       */
+      virtual ~CCI_PrototypeJointsSensor() {}
+
+#ifdef ARGOS_WITH_LUA
+      virtual void CreateLuaState(lua_State* pt_lua_state);
+
+      void ReadingsToLuaState(lua_State* pt_lua_state);
+#endif
+
+   protected: 
+      SSensor::TVector m_vecSensors;
+   };
+}
+
+#endif