Pushreachtask

README.md

@@ -19,6 +19,11 @@ You also need to install the following packages:
 apt install python3-tk
 ```
 
+## Download
+```bash
+git clone --recursive https://github.com/HorizonRobotics/SocialRobot
+```
+
 ## To compile
 ```bash
 cd REPO_ROOT
@@ -27,7 +32,7 @@ mkdir build
 cd build
 cmake ..
 make -j
-cd REPO_ROOT
+cd ..  # REPO_ROOT
 pip3 install -e .
 ```
 if cmake .. complains about cannot find configuration file provided by "gazebo", run
@@ -165,6 +170,22 @@ It's not so easy, for some task you may need some trials to complete it. An exam
 ### python
 You need to make sure the python you use matches the python found by cmake. You can check this by looking at REPO_ROOT/build/CMakeCache.txt
 
+### Python config cmake errors
+```
+CMake Error at pybind11/tools/FindPythonLibsNew.cmake:95 (message):
+  Python config failure:
+
+Call Stack (most recent call first):
+  pybind11/tools/pybind11Tools.cmake:16 (find_package)
+  pybind11/CMakeLists.txt:33 (include)
+```
+
+Remove the makefiles and build again:
+```bash
+cd REPO_ROOT
+rm -rf build
+```
+
 ### display
 
 You need to make sure your `DISPLAY` environment variable points to a valid display, otherwise camera sensor cannot be created and there will be the following error:

pygazebo/pygazebo.cc

@@ -22,6 +22,7 @@
 #include <gazebo/sensors/SensorManager.hh>
 #include <gazebo/sensors/SensorsIface.hh>
 #include <gazebo/util/LogRecord.hh>
+#include <ignition/math/Rand.hh>
 #include <mutex>  // NOLINT
 #include <sstream>
 
@@ -469,6 +470,7 @@ class World {
 
 void Initialize(const std::vector<std::string>& args,
                 int port = 0,
+                int seed = -1,
                 bool quiet = false) {
   if (port != 0) {
     std::string uri = "localhost:" + std::to_string(port);
@@ -477,6 +479,9 @@ void Initialize(const std::vector<std::string>& args,
   if (!gazebo_initialized) {
     gazebo::common::Console::SetQuiet(quiet);
     gazebo::setupServer(args);
+    if (seed >= 0) {
+      ignition::math::Rand::Seed((unsigned int)seed);
+    }
     // gazebo::runWorld uses World::RunLoop(). RunLoop() starts LogWorker()
     // every time. LogWorker will always store something in the buffer when
     // it is started. But we don't have a running LogRecord to collect all
@@ -568,6 +573,7 @@ PYBIND11_MODULE(pygazebo, m) {
         "Initialize",
         py::arg("args") = std::vector<std::string>(),
         py::arg("port") = 0,
+        py::arg("seed") = -1,
         py::arg("quiet") = false);
 
   m.def("close", &Close, "Shutdown everything of gazebo");

python/social_bot/envs/gazebo_base.py

@@ -43,6 +43,8 @@ def __init__(self,
                  world_config=None,
                  sim_time_precision=0.001,
                  port=None,
+                 seed=-1,
+                 use_port_in_gz_seed=True,
                  quiet=False):
         """
         Args:
@@ -52,6 +54,10 @@ def __init__(self,
                 see `_modify_world_xml` for details
              sim_time_precision (float): the time precision of the simulator
              port (int): Gazebo port
+             seed (int): random seed for gazebo, -1 for using the default: PID
+             use_port_in_gz_seed (bool): if True, use port and seed in gazebo seed.
+                Set to False when doing repeatability debugging.  Set to True when
+                running several envs in parallel during regular training.
              quiet (bool) Set quiet output
         """
         os.environ["GAZEBO_MODEL_DATABASE_URI"] = ""
@@ -61,12 +67,17 @@ def __init__(self,
         # This avoids different parallel simulations having the same randomness.
         # When calling from alf, alf.environments.utils.create_environment calls
         # env.seed() afterwards, which is the real seed being used.  Not this one.
-        random.seed(port)
+        _seed = port
+        if seed >= 0:
+            _seed = seed
+            if use_port_in_gz_seed:
+                _seed += port
+        random.seed(_seed)
         self._rendering_process = None
         self._rendering_camera = None
         # the default camera pose for rendering rgb_array, could be override
         self._rendering_cam_pose = "10 -10 6 0 0.4 2.4"
-        gazebo.initialize(port=port, quiet=quiet)
+        gazebo.initialize(port=port, seed=_seed, quiet=quiet)
 
         if world_file:
             world_file_abs_path = os.path.join(social_bot.get_world_dir(),

python/social_bot/envs/play_ground.py

@@ -36,7 +36,7 @@
 from social_bot.envs.gazebo_base import GazeboEnvBase
 from social_bot.teacher import TaskGroup
 from social_bot.teacher import TeacherAction
-from social_bot.tasks import GoalTask, ICubAuxiliaryTask, KickingBallTask, Reaching3D
+from social_bot.tasks import GoalTask, PushReachTask, ICubAuxiliaryTask, KickingBallTask, Reaching3D
 
 
 @gin.configurable
@@ -173,7 +173,7 @@ def __init__(self,
         self._teacher = teacher.Teacher(task_groups_exclusive=False)
         self._has_goal_task = False
         for task in tasks:
-            if task == GoalTask:
+            if task in (GoalTask, PushReachTask):
                 self._has_goal_task = True
             task_group = TaskGroup()
             task_group.add_task(task(env=self, max_steps=max_steps))

python/social_bot/models/pioneer2dx_noplugin_ghost/model-ghost.sdf

@@ -0,0 +1,102 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="pioneer2dx_noplugin_ghost">
+    <static>true</static>
+    <link name="chassis">
+      <pose>0 0 0.16 0 0 0</pose>
+      <inertial>
+        <mass>5.67</mass>
+        <inertia>
+          <ixx>0.07</ixx>
+          <iyy>0.08</iyy>
+          <izz>0.10</izz>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <pose>0 0 0.04 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>model://pioneer2dx/meshes/chassis.dae</uri>
+          </mesh>
+        </geometry>
+        <transparency>0.7</transparency>
+      </visual>
+      <visual name="castor_visual">
+        <transparency>0.7</transparency>
+        <pose>-0.200 0 -0.12 0 0 0</pose>
+        <geometry>
+          <sphere>
+            <radius>0.04</radius>
+          </sphere>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/FlatBlack</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+    <link name="right_wheel">
+      <pose>0.1 -.17 0.11 0 1.5707 1.5707</pose>
+      <inertial>
+        <mass>1.5</mass>
+        <inertia>
+          <ixx>0.0051</ixx>
+          <iyy>0.0051</iyy>
+          <izz>0.0090</izz>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <transparency>0.7</transparency>
+        <geometry>
+          <cylinder>
+            <radius>0.11</radius>
+            <length>0.05</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/FlatBlack</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+    <link name="left_wheel">
+      <pose>0.1 .17 0.11 0 1.5707 1.5707</pose>
+      <inertial>
+        <mass>1.5</mass>
+        <inertia>
+          <ixx>0.0051</ixx>
+          <iyy>0.0051</iyy>
+          <izz>0.0090</izz>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyz>0</iyz>
+        </inertia>
+      </inertial>
+      <visual name="visual">
+        <transparency>0.7</transparency>
+        <geometry>
+          <cylinder>
+            <radius>0.11</radius>
+            <length>0.05</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/FlatBlack</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

python/social_bot/models/pioneer2dx_noplugin_ghost/model.config

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+
+<model>
+  <!-- This is essentially same as the original Pioneer 2DX model without the model plugin -->
+  <name>Pioneer 2DX Ghost</name>
+  <version>1.0</version>
+  <sdf version="1.5">model-ghost.sdf</sdf>
+
+  <author>
+    <name>Nate Koenig</name>
+    <email>nate@osrfoundation.org</email>
+  </author>
+
+  <description>
+    A model of the pioneer 2 dx robot. This is same as https://bitbucket.org/osrf/gazebo_models/src/default/pioneer2dx/
+    with the plugin removed.
+  </description>
+
+  <depend>
+  </depend>
+</model>

python/social_bot/models/target_ball/model.sdf

@@ -8,7 +8,7 @@
       <visual name="visual">
         <geometry>
           <sphere>
-            <radius>0.03</radius>
+            <radius>0.1</radius>
           </sphere>
         </geometry>
         <material>

python/social_bot/models/wood_cube_30cm_without_offset/model.config

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>Wood cube 30cm</name>
+  <version>1.0</version>
+  <sdf version='1.5'>model.sdf</sdf>
+
+  <author>
+    <name>Nate Koenig</name>
+    <email>nate@osrfoundation.org</email>
+  </author>
+
+  <description>
+    A 30cm^3 wooden cube modified from the 10x10x10 cm wooden cube.
+    The 5cm height offset of the internal link has been removed from the original version.
+    Also reduced mass by 0.3, and friction to 1/2.
+  </description>
+</model>

python/social_bot/models/wood_cube_30cm_without_offset/model.sdf

@@ -0,0 +1,63 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="wood_cube_30cm_without_offset">
+    <link name="link">
+      <inertial>
+        <mass>0.2167</mass>
+        <inertia>
+          <ixx>0.0008611666666666669</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0008611666666666669</iyy>
+          <iyz>0</iyz>
+          <izz>0.0008611666666666669</izz>
+        </inertia>
+      </inertial>
+
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.3 0.3 0.3</size>
+          </box>
+        </geometry>
+        <surface>
+          <contact>
+            <!-- Red Pine coefficients for longitudinal axis of the wood
+                 according to:
+                 http://www.fpl.fs.fed.us/documnts/fplgtr/fplgtr113/ch04.pdf -->
+            <poissons_ratio>0.347</poissons_ratio>
+            <elastic_modulus>8.8e+09</elastic_modulus>
+            <ode>
+              <kp>100000</kp>
+              <kd>100</kd>
+              <max_vel>100.0</max_vel>
+              <min_depth>0.001</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <torsional>
+              <coefficient>0.5</coefficient>
+              <use_patch_radius>0</use_patch_radius>
+              <surface_radius>0.01</surface_radius>
+            </torsional>
+          </friction>
+        </surface>
+      </collision>
+
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.3 0.3 0.3</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Wood</name>
+          </script>
+        </material>
+      </visual>
+
+    </link>
+  </model>
+</sdf>