Refactoring demo collector (#56)

Author: kdh0429

launch/open_manipulator_demo.launch

@@ -0,0 +1,42 @@
+<launch>
+  <!-- These are the arguments you can pass this launch file, for example paused:=true -->
+  <arg name="use_robot_name" default="open_manipulator"
+       doc="Must match the robotNamespace tag in the gazebo description file"/>
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+  <arg name="use_platform" default="false"/>
+
+  <rosparam file="$(find open_manipulator_gazebo)/config/gazebo_controller.yaml" command="load" />
+
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find open_manipulator_gazebo)/worlds/empty.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+  </include>
+
+  <!-- Load the URDF into the ROS Parameter Server -->
+  <param name="robot_description"
+   command="$(find xacro)/xacro --inorder '$(find open_manipulator_description)/urdf/open_manipulator.urdf.xacro'"/>
+
+  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
+     args="-urdf -model open_manipulator -z 0.0 -param robot_description"/>
+
+  <!-- ros_control robotis manipulator launch file -->
+  <include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch">
+    <arg name="use_robot_name" value="$(arg use_robot_name)"/>
+  </include>
+
+  <!-- Run demo collector node -->
+  <node name="open_manipulator_demo_collector" pkg="kair_algorithms" type="run_open_manipulator_demo.py" respawn="false" output="screen">
+      <param name="use_platform" value="$(arg use_platform)" />
+  </node>
+
+</launch>

scripts/algorithms/common/abstract/agent.py

@@ -80,7 +80,7 @@ def save_params(self, params, n_episode):
         path = os.path.join("./save/" + save_name + "_ep_" + str(n_episode) + ".pt")
         torch.save(params, path)
 
-        print ("[INFO] Saved the model and optimizer to", path)
+        print("[INFO] Saved the model and optimizer to", path)
 
     @abstractmethod
     def write_log(self, *args):
@@ -109,7 +109,7 @@ def test(self):
                 score += reward
                 step += 1
 
-            print (
+            print(
                 "[INFO] episode %d\tstep: %d\ttotal score: %d"
                 % (i_episode, step, score)
             )

scripts/algorithms/fd/sac_agent.py

@@ -201,7 +201,7 @@ def update_model(self, experiences):
     def pretrain(self):
         """Pretraining steps."""
         pretrain_loss = list()
-        print ("[INFO] Pre-Train %d steps." % self.hyper_params["PRETRAIN_STEP"])
+        print("[INFO] Pre-Train %d steps." % self.hyper_params["PRETRAIN_STEP"])
         for i_step in range(1, self.hyper_params["PRETRAIN_STEP"] + 1):
             loss = self.update_model()
             pretrain_loss.append(loss)  # for logging

scripts/algorithms/fd/td3_agent.py

@@ -180,7 +180,7 @@ def update_model(self, experiences):
     def pretrain(self):
         """Pretraining steps."""
         pretrain_loss = list()
-        print ("[INFO] Pre-Train %d steps." % self.hyper_params["PRETRAIN_STEP"])
+        print("[INFO] Pre-Train %d steps." % self.hyper_params["PRETRAIN_STEP"])
         for i_step in range(1, self.hyper_params["PRETRAIN_STEP"] + 1):
             loss = self.update_model()
             pretrain_loss.append(loss)  # for logging

scripts/algorithms/sac/agent.py

@@ -264,7 +264,7 @@ def update_model(self, experiences):
     def load_params(self, path):
         """Load model and optimizer parameters."""
         if not os.path.exists(path):
-            print ("[ERROR] the input path does not exist. ->", path)
+            print("[ERROR] the input path does not exist. ->", path)
             return
 
         params = torch.load(path)
@@ -281,7 +281,7 @@ def load_params(self, path):
         if self.hyper_params["AUTO_ENTROPY_TUNING"]:
             self.alpha_optimizer.load_state_dict(params["alpha_optim"])
 
-        print ("[INFO] loaded the model and optimizer from", path)
+        print("[INFO] loaded the model and optimizer from", path)
 
     def save_params(self, n_episode):
         """Save model and optimizer parameters."""
@@ -306,7 +306,7 @@ def write_log(self, i, loss, score=0.0, delayed_update=1):
         """Write log about loss and score"""
         total_loss = loss.sum()
 
-        print (
+        print(
             "[INFO] episode %d, episode_step %d, total step %d, total score: %d\n"
             "total loss: %.3f actor_loss: %.3f qf_1_loss: %.3f qf_2_loss: %.3f "
             "vf_loss: %.3f alpha_loss: %.3f\n"

scripts/algorithms/td3/agent.py

@@ -177,7 +177,7 @@ def update_model(self, experiences):
     def load_params(self, path):
         """Load model and optimizer parameters."""
         if not os.path.exists(path):
-            print ("[ERROR] the input path does not exist. ->", path)
+            print("[ERROR] the input path does not exist. ->", path)
             return
 
         params = torch.load(path)
@@ -189,7 +189,7 @@ def load_params(self, path):
         self.critic2_target.load_state_dict(params["critic2_target_state_dict"])
         self.actor_optim.load_state_dict(params["actor_optim_state_dict"])
         self.critic_optim.load_state_dict(params["critic_optim_state_dict"])
-        print ("[INFO] loaded the model and optimizer from", path)
+        print("[INFO] loaded the model and optimizer from", path)
 
     def save_params(self, n_episode):
         """Save model and optimizer parameters."""
@@ -210,7 +210,7 @@ def write_log(self, i, loss, score):
         """Write log about loss and score"""
         total_loss = loss.sum()
 
-        print (
+        print(
             "[INFO] total_steps: %d episode: %d total score: %d, total loss: %f\n"
             "actor_loss: %.3f critic1_loss: %.3f critic2_loss: %.3f\n"
             % (self.total_steps, i, score, total_loss, loss[0], loss[1], loss[2])

scripts/config/demo/__init__.py

@@ -0,0 +1,12 @@
+config = {
+    "USE_PLATFORM": False,
+    "DAMPING": 0.01,
+    "JOINT_VEL_LIMIT": 4,
+    "NUM_TARGET_DEMO": 10,
+    "HZ": 100,
+    "SAVE_PATH": "../demo_collection.json",
+}
+
+
+def get():
+    return config

scripts/config/demo/open_manipulator/__init__.py

@@ -0,0 +1,31 @@
+config = {
+    "DXL_RESOLUTION": 0.088,  # Position resolution of XM430-W210 in degree
+    "DXL_VELOCITY_RESOLUTION": 0.229,  # Velocity resolition of XM430-W210 in rpm
+    "DXL_TO_CURRENT": 2.69,  # From dynamixel return value to current
+    # Control table address
+    "ADDR_TORQUE_ENABLE": 64,  # To set torque on/off
+    "ADDR_PRESENT_POSITION": 132,  # To read position
+    "ADDR_PRESENT_VELOCITY": 128,  # To read velocity
+    "ADDR_PRESENT_CURRENT": 126,  # To read current
+    "ADDR_OP_MODE": 11,  # To set operation mode (position/velocity/multi_turn mode)
+    "ADDR_GOAL_POSITION": 116,  # To write position
+    # Data Byte Length
+    "LEN_GOAL_POSITION": 4,  # In byte
+    "LEN_PRESENT_POSITION": 4,  # In byte
+    "LEN_PRESENT_VELOCITY": 4,  # In byte
+    "LEN_PRESENT_CURRENT": 2,  # In byte
+    "CW_LIMIT": 4095,  # Clockwise limit
+    "CCW_LIMIT": 0,  # Counter clock wise limit
+    "DXL_POS_OFFSET": 2048,  # Initial position
+    # Protocol version
+    "PROTOCOL_VERSION": 2.0,
+    # Default setting
+    "DXL1_ID": 11,  # Joint 1 ID
+    "DXL2_ID": 12,  # Joint 2 ID
+    "DXL3_ID": 13,  # Joint 3 ID
+    "DXL4_ID": 14,  # Joint 4 ID
+    "BAUDRATE": 1000000,
+    "DEVICENAME": "/dev/ttyUSB0",  # Connected USB port
+    "TORQUE_ENABLE": 1,  # Torque on
+    "TORQUE_DISABLE": 0,  # Torque off
+}