Merge pull request #32 from ci-group/diffcpgs

Revolve improvements

Author: portaloffreedom

cpprevolve/CMakeLists.txt

@@ -62,6 +62,9 @@ include_directories(${Boost_INCLUDE_DIRS})
 find_package(GSL REQUIRED)
 include_directories(${GSL_INCLUDE_DIRS})
 
+find_package(yaml-cpp REQUIRED)
+include_directories(${YAML_CPP_INCLUDE_DIR})
+
 # Find Gazebo
 # LOCAL_GAZEBO_DIR can be set to a path with a gazebo-config.cmake
 if (LOCAL_GAZEBO_DIR)
@@ -154,9 +157,10 @@ set_source_files_properties(
 # Plugin C++ files
 file(GLOB_RECURSE
      REVOLVE_GZ_SRC
-     revolve/gazebo/brain/*.cpp
+     revolve/gazebo/brains/*.cpp
      revolve/gazebo/motors/*.cpp
      revolve/gazebo/sensors/*.cpp
+     revolve/gazebo/util/*.cpp
      revolve/gazebo/plugin/BodyAnalyzer.cpp
      revolve/gazebo/plugin/RobotController.cpp
      revolve/gazebo/plugin/WorldController.cpp
@@ -189,6 +193,7 @@ target_link_libraries(
     ${GAZEBO_LIBRARIES}
     ${Boost_LIBRARIES}
     ${GSL_LIBRARIES}
+    ${YAML_CPP_LIBRARIES}
 )
 
 # Create World plugin

cpprevolve/revolve/gazebo/Types.h

@@ -20,11 +20,9 @@
 #ifndef REVOLVE_GZ_MODEL_TYPES_H_
 #define REVOLVE_GZ_MODEL_TYPES_H_
 
+#include <memory>
 #include <vector>
 
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-
 namespace revolve
 {
   namespace gazebo
@@ -41,17 +39,17 @@ namespace revolve
 
     class Evaluator;
 
-    typedef boost::shared_ptr< Brain > BrainPtr;
+    typedef std::shared_ptr< Brain > BrainPtr;
 
-    typedef boost::shared_ptr< Motor > MotorPtr;
+    typedef std::shared_ptr< Motor > MotorPtr;
 
-    typedef boost::shared_ptr< VirtualSensor > SensorPtr;
+    typedef std::shared_ptr< VirtualSensor > SensorPtr;
 
-    typedef boost::shared_ptr< MotorFactory > MotorFactoryPtr;
+    typedef std::shared_ptr< MotorFactory > MotorFactoryPtr;
 
-    typedef boost::shared_ptr< SensorFactory > SensorFactoryPtr;
+    typedef std::shared_ptr< SensorFactory > SensorFactoryPtr;
 
-    typedef boost::shared_ptr< Evaluator > EvaluatorPtr;
+    typedef std::shared_ptr< Evaluator > EvaluatorPtr;
 
     typedef std::vector< double > Spline;
 

cpprevolve/revolve/gazebo/brain/Brain.h cpprevolve/revolve/gazebo/brains/Brain.hcpprevolve/revolve/gazebo/brain/Brain.h renamed to cpprevolve/revolve/gazebo/brains/Brain.h

@@ -27,6 +27,7 @@
 #include <boost/shared_ptr.hpp>
 
 #include <gazebo/common/common.hh>
+#include <gazebo/gazebo.hh>
 
 #include <revolve/gazebo/Types.h>
 
@@ -52,6 +53,9 @@ namespace revolve
           const std::vector< SensorPtr > &_sensors,
           const double _time,
           const double _step) = 0;
+
+      /// \brief Transport node
+      protected: ::gazebo::transport::NodePtr node_;
     };
   } /* namespace gazebo */
 } /* namespace revolve */

cpprevolve/revolve/gazebo/brains/BrainFactory.cpp

@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam
+ *
+ * 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
+ *
+ *     http://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.
+ *
+ * Description: TODO: <Add brief description about file purpose>
+ * Author: Milan Jelisavcic
+ * Date: December 23, 2018
+ *
+ */
+
+#include <revolve/gazebo/brains/BrainFactory.h>
+#include <revolve/gazebo/brains/Brains.h>
+
+namespace gz = gazebo;
+
+using namespace revolve::gazebo;
+
+/////////////////////////////////////////////////
+BrainFactory::BrainFactory(::gazebo::physics::ModelPtr _model)
+    : model_(_model)
+{
+}
+
+/////////////////////////////////////////////////
+BrainFactory::~BrainFactory() = default;
+
+///////////////////////////////////////////////////
+//BrainPtr BrainFactory::Brain(
+//    sdf::ElementPtr _brainSdf,
+//    const std::vector< MotorPtr > &_motors,
+//    const std::vector< SensorPtr > &_sensors)
+//{
+//  BrainPtr brain = nullptr;
+////  if ("ann" == _type)
+////  {
+////    brain.reset(new NeuralNetwork(model_, _brainSdf, _motors, _sensors));
+////  }
+////  else if ("rlpower" == _type)
+////  {
+////    brain.reset(new RLPower(model_, _brainSdf, _motors, _sensors));
+////  }
+////  else if ("diff_cpg" == type)
+////  {
+////    brain.reset(new );
+////  }
+//
+//  return brain;
+//}
+//
+///////////////////////////////////////////////////
+//BrainPtr BrainFactory::Create(
+//    sdf::ElementPtr _brainSdf,
+//    const std::vector< MotorPtr > &_motors,
+//    const std::vector< SensorPtr > &_sensors)
+//{
+////  auto typeParam = _motorSdf->GetAttribute("type");
+////  auto partIdParam = _motorSdf->GetAttribute("part_id");
+////  auto idParam = _motorSdf->GetAttribute("id");
+////
+////  if (not typeParam or not partIdParam or not idParam)
+////  {
+////    std::cerr << "Motor is missing required attributes (`id`, `type` or "
+////                 "`part_id`)." << std::endl;
+////    throw std::runtime_error("Motor error");
+////  }
+////
+////  auto partId = partIdParam->GetAsString();
+////  auto type = typeParam->GetAsString();
+////  auto id = idParam->GetAsString();
+//
+//  BrainPtr brain = nullptr;
+////      this->Brain(_brainSdf, _motors, _sensors);
+////  assert(not brain || "[BrainFactory::Create] Brain type is unknown.");
+//
+//  return brain;
+//}

cpprevolve/revolve/gazebo/brains/BrainFactory.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam
+ *
+ * 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
+ *
+ *     http://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.
+ *
+ * Description: TODO: <Add brief description about file purpose>
+ * Author: Milan Jelisavcic
+ * Date: December 23, 2018
+ *
+ */
+
+#ifndef REVOLVE_GAZEBO_BRAINS_BRAINFACTORY_H_
+#define REVOLVE_GAZEBO_BRAINS_BRAINFACTORY_H_
+
+#include <string>
+
+#include <gazebo/common/common.hh>
+
+#include <revolve/gazebo/Types.h>
+
+namespace revolve
+{
+  namespace gazebo
+  {
+    class BrainFactory
+    {
+      /// \brief Constructor
+      /// \param[in] _model Model identifier
+      public: explicit BrainFactory(::gazebo::physics::ModelPtr _model);
+
+      /// \brief Destructor
+      public: virtual ~BrainFactory();
+
+      /// \brief Returns a brain pointer reference from a brain element.
+      /// This is the convenience wrapper over `create` that has required
+      /// attributes already checked, usually you should override this when
+      /// adding new brain types.
+      public: virtual BrainPtr Brain(
+          sdf::ElementPtr _brainSdf,
+          const std::vector< MotorPtr > &_motors,
+          const std::vector< SensorPtr > &_sensors);
+
+      /// \brief Creates a brain for the given model for the given SDF element.
+      public: virtual BrainPtr Create(
+          sdf::ElementPtr _brainSdf,
+          const std::vector< MotorPtr > &_motors,
+          const std::vector< SensorPtr > &_sensors);
+
+      /// \brief Internal reference to the robot model
+      protected: ::gazebo::physics::ModelPtr model_;
+    };
+  }
+}
+
+#endif // REVOLVE_GAZEBO_BRAINS_BRAINFACTORY_H_

cpprevolve/revolve/gazebo/brains/Brains.h

@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam
+ *
+ * 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
+ *
+ *     http://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.
+ *
+ * Description: TODO: <Add brief description about file purpose>
+ * Author: Milan Jelisavcic
+ * Date: Mar 16, 2015
+ *
+ */
+
+#ifndef REVOLVE_GAZEBO_BRAINS_BRAINS_H_
+#define REVOLVE_GAZEBO_BRAINS_BRAINS_H_
+
+#include <revolve/gazebo/brains/DifferentialCPG.h>
+#include <revolve/gazebo/brains/NeuralNetwork.h>
+#include <revolve/gazebo/brains/RLPower.h>
+
+#endif // REVOLVE_GAZEBO_BRAINS_BRAINS_H_

cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp

@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam
+ *
+ * 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
+ *
+ *     http://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.
+ *
+ * Description: TODO: <Add brief description about file purpose>
+ * Author: Milan Jelisavcic
+ * Date: December 29, 2018
+ *
+ */
+
+#include "DifferentialCPG.h"
+#include "../motors/Motor.h"
+#include "../sensors/Sensor.h"
+#include "../util/YamlBodyParser.h"
+
+namespace gz = gazebo;
+
+using namespace revolve::gazebo;
+
+/////////////////////////////////////////////////
+DifferentialCPG::DifferentialCPG(
+    const ::gazebo::physics::ModelPtr &_model,
+    const sdf::ElementPtr &_node,
+    const std::vector< revolve::gazebo::MotorPtr > &_motors,
+    const std::vector< revolve::gazebo::SensorPtr > &_sensors)
+    : flipState_(false)
+{
+  // Create transport node
+  this->node_.reset(new gz::transport::Node());
+  this->node_->Init();
+
+  auto name = _model->GetName();
+  // Listen to network modification requests
+//  alterSub_ = node_->Subscribe(
+//      "~/" + name + "/modify_diff_cpg", &DifferentialCPG::Modify,
+//      this);
+
+  auto numMotors = _motors.size();
+  auto numSensors = _sensors.size();
+
+  auto genome = _node->GetAttribute("genome")->GetAsString();
+  auto parser = new YamlBodyParser(genome);
+
+
+}
+
+/////////////////////////////////////////////////
+DifferentialCPG::~DifferentialCPG() = default;
+
+/////////////////////////////////////////////////
+void DifferentialCPG::Update(
+    const std::vector< revolve::gazebo::MotorPtr > &_motors,
+    const std::vector< revolve::gazebo::SensorPtr > &_sensors,
+    const double _time,
+    const double _step)
+{
+}