WIP: Task Morphology Research.

cpprevolve/revolve/gazebo/CMakeLists.txt

@@ -78,7 +78,7 @@ if (LOCAL_GAZEBO_DIR)
             NO_DEFAULT_PATH)
     message(WARNING "Using local Gazebo @ ${gazebo_DIR}")
 else()
-    find_package(gazebo 9 REQUIRED)
+    find_package(gazebo 10 REQUIRED)
 endif()
 include_directories(${GAZEBO_INCLUDE_DIRS})
 link_directories(${GAZEBO_LIBRARY_DIRS})

cpprevolve/revolve/gazebo/msgs/robot_states.proto

@@ -2,12 +2,21 @@ syntax = "proto2";
 package revolve.msgs;
 import "time.proto";
 import "pose.proto";
+import "vector3d.proto";
+
+message Orientation {
+  optional gazebo.msgs.Vector3d vec_forward = 1;
+  optional gazebo.msgs.Vector3d vec_left = 2;
+  optional gazebo.msgs.Vector3d vec_back = 3;
+  optional gazebo.msgs.Vector3d vec_right = 4;
+}
 
 message RobotState {
   required uint32 id = 1;
   required string name = 2;
   required gazebo.msgs.Pose pose = 3;
   optional bool dead = 4;
+  optional Orientation orientation_vecs = 5;
 }
 
 message RobotStates {

cpprevolve/revolve/gazebo/plugin/WorldController.cpp

@@ -21,8 +21,9 @@
 
 #include "WorldController.h"
 
-namespace gz = gazebo;
+//#define USE_MARKERS
 
+namespace gz = gazebo;
 using namespace revolve::gazebo;
 
 /////////////////////////////////////////////////
@@ -122,8 +123,91 @@ void WorldController::Reset()
     this->lastRobotStatesUpdateTime_ = 0; //this->world_->SimTime().Double();
 }
 
+
+enum Orientation {
+    FORWARD = 0,
+    LEFT = 1,
+    BACK = 2,
+    RIGHT = 3,
+};
+
+ignition::math::Pose3d generateMarkerPose(const Orientation index, const ignition::math::Pose3d &robotPose)
+{
+    static const std::array<ignition::math::Vector3d,4> markerOffset {
+            ignition::math::Vector3d { 0 , -1, 0}, // Forward
+            ignition::math::Vector3d { 1 ,  0, 0}, // Left
+            ignition::math::Vector3d { 0 ,  1, 0}, // Backwards
+            ignition::math::Vector3d {-1 ,  0, 0}, // Right
+    };
+
+    assert(index < 4);
+    return ignition::math::Pose3d(
+            robotPose.CoordPositionAdd(markerOffset[index]),
+            ignition::math::Quaterniond::Identity
+    );
+
+}
+
+
+void fillMessages(const Orientation orientation,
+        const ignition::math::Pose3d &worldPose,
+        ignition::msgs::Marker &markerMsg,
+        ::revolve::msgs::Orientation* orientationVecs,
+        bool materials = false)
+{
+    ignition::math::Pose3d markerPose = generateMarkerPose(orientation, worldPose);
+    ignition::math::Vector3d orientation_vec = markerPose.Pos() - worldPose.Pos();
+
+    switch (orientation) {
+        case FORWARD:
+            gz::msgs::Set(orientationVecs->mutable_vec_forward(), orientation_vec);
+            break;
+        case LEFT:
+            gz::msgs::Set(orientationVecs->mutable_vec_left(), orientation_vec);
+            break;
+        case BACK:
+            gz::msgs::Set(orientationVecs->mutable_vec_back(), orientation_vec);
+            break;
+        case RIGHT:
+            gz::msgs::Set(orientationVecs->mutable_vec_right(), orientation_vec);
+            break;
+        default:
+            assert(false);
+    }
+
+#ifdef USE_MARKERS
+    // absolute position
+    //ignition::msgs::Set(markerMsg.mutable_pose(), markerPose);
+    // relative vector
+    ignition::msgs::Set(markerMsg.mutable_pose(),
+                        ignition::math::Pose3d(orientation_vec, ignition::math::Quaterniond::Identity));
+
+    if (materials) {
+        ignition::msgs::Material *matMsg = markerMsg.mutable_material();
+        switch (orientation) {
+            case FORWARD:
+                matMsg->mutable_script()->set_name("Gazebo/BlueLaser");
+                break;
+            case LEFT:
+                matMsg->mutable_script()->set_name("Gazebo/Red");
+                break;
+            case BACK:
+                matMsg->mutable_script()->set_name("Gazebo/Yellow");
+                break;
+            case RIGHT:
+                matMsg->mutable_script()->set_name("Gazebo/Green");
+                break;
+            default:
+                assert(false);
+        }
+    }
+#endif
+}
+
+
 /////////////////////////////////////////////////
-void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) {
+void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info)
+{
     if (not this->robotStatesPubFreq_) {
         return;
     }
@@ -136,9 +220,17 @@ void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) {
         msgs::RobotStates msg;
         gz::msgs::Set(msg.mutable_time(), _info.simTime);
 
+        // MARKER MESSAGE
+        ignition::msgs::Marker markerMsg;
+#ifdef USE_MARKERS
+        ::ignition::transport::Node ignNode;
+        markerMsg.set_ns("revolve");
+        markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
+#endif
+
         {
             boost::recursive_mutex::scoped_lock lock_physics(*this->world_->Physics()->GetPhysicsUpdateMutex());
-            for (const auto &model : this->world_->Models()) {
+            for (const boost::shared_ptr<gz::physics::Model> &model : this->world_->Models()) {
                 if (model->IsStatic()) {
                     // Ignore static models such as the ground and obstacles
                     continue;
@@ -150,9 +242,61 @@ void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) {
                 stateMsg->set_id(model->GetId());
 
                 auto poseMsg = stateMsg->mutable_pose();
-                auto relativePose = model->RelativePose();
+                // relative pose and world pose are the same because the robot is relative to the world directly
+                //auto relativePose = model->RelativePose();
+                auto worldPose = model->WorldPose();
+                ::revolve::msgs::Orientation* orientationVecs = stateMsg->mutable_orientation_vecs();
+
+                // UPDATE/GENERATE MARKERS
+
+                std::map<gz::physics::Model*, std::array<uint64_t,4>>::iterator model_markers = markers_.find(model.get());
+                if (model_markers == markers_.end()) {
+                    std::array<uint64_t, 4> new_marker_ids {
+                            markers_ids_,
+                            markers_ids_+1,
+                            markers_ids_+2,
+                            markers_ids_+3
+                    };
+                    markers_ids_ += 4;
+#ifdef USE_MARKERS
+                    markerMsg.set_type(ignition::msgs::Marker::SPHERE);
+
+                    const double MARKER_SCALE = 0.05;
+                    ignition::msgs::Set(markerMsg.mutable_scale(),
+                            ignition::math::Vector3d(MARKER_SCALE,MARKER_SCALE,MARKER_SCALE));
+#endif
+
+                    int i = 0;
+                    for (uint64_t marker_id : new_marker_ids) {
+                        Orientation orientation = Orientation(i);
+                        fillMessages(orientation, worldPose, markerMsg, orientationVecs, true);
+#ifdef USE_MARKERS
+                        markerMsg.set_id(marker_id);
+                        assert(ignNode.Request("/marker", markerMsg));
+#endif
+                        i++;
+                    }
+
+                    bool success;
+                    std::tie(model_markers, success) = markers_.emplace(
+                            model.get(),
+                            new_marker_ids
+                    );
+                    assert(success);
+                } else {
+                    int i = 0;
+                    for (uint64_t marker_id : model_markers->second) {
+                        Orientation orientation = Orientation(i);
+                        fillMessages(orientation, worldPose, markerMsg, orientationVecs);
+#ifdef USE_MARKERS
+                        markerMsg.set_id(marker_id);
+                        assert(ignNode.Request("/marker", markerMsg));
+#endif
+                        i++;
+                    }
+                }
 
-                gz::msgs::Set(poseMsg, relativePose);
+                gz::msgs::Set(poseMsg, worldPose);
 
                 // Death sentence check
                 const std::string name = model->GetName();

cpprevolve/revolve/gazebo/plugin/WorldController.h

@@ -128,6 +128,9 @@ class WorldController: public ::gazebo::WorldPlugin
 //    boost::mutex world_insert_remove_mutex;
 
     ::gazebo::physics::Model_V models_to_remove;
+
+    std::map<::gazebo::physics::Model*, std::array<uint64_t,4> > markers_;
+    uint64_t markers_ids_ = 0;
 };
 
 }  // namespace gazebo

experiments/task-morphology/check_running_experiments.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+for i in exp*log; do echo -n "$i "; value=$(grep "Exported snapshot" $i|tail -n1|sed -E "s/\[(.*),.*Exported snapshot ([0-9]+).*/\2 -> \1/g"); echo $value; done

experiments/task-morphology/experimentation_script.sh

@@ -0,0 +1,32 @@
+#!/bin/bash
+set -e
+set -x
+
+
+#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_A --fitness displacement --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done
+
+#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_B --fitness rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done
+
+#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_C --fitness gait_with_rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done
+
+#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_D --fitness gait_and_rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done
+
+#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_E --fitness rotation_with_gait --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done
+
+
+runs=10
+runs_start=0
+start_port=13000
+exp_name=Experiment_E
+fitness=rotation_with_gait
+cores=4
+exp_name=Experiment_B
+fitness=rotation
+log_suffix=''
+manager=experiments/task-morphology/manager.py
+
+for i in $(seq $runs)
+do
+	run=$(($i+runs_start))
+	screen -d -m -S "${exp_name}_${run}" -L -Logfile "${exp_name}${log_suffix}_${run}.log" nice -n19 ./revolve.sh --manager $manager --experiment-name $exp_name --fitness $fitness --n-cores $cores --port-start $((${start_port} + ($run*10))) --run $run --recovery-enabled True
+done