Merge pull request #32 from ci-group/diffcpgs

Revolve improvements

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.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)
+{
+}