diff --git a/scripts/algorithms/common/abstract/agent.py b/scripts/algorithms/common/abstract/agent.py index dbecc9a..b7f0b68 100755 --- a/scripts/algorithms/common/abstract/agent.py +++ b/scripts/algorithms/common/abstract/agent.py @@ -80,7 +80,7 @@ class AbstractAgent(object): 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 @@ class AbstractAgent(object): score += reward step += 1 - print ( + print( "[INFO] episode %d\tstep: %d\ttotal score: %d" % (i_episode, step, score) ) diff --git a/scripts/algorithms/fd/sac_agent.py b/scripts/algorithms/fd/sac_agent.py index 4e3760c..52164a9 100644 --- a/scripts/algorithms/fd/sac_agent.py +++ b/scripts/algorithms/fd/sac_agent.py @@ -201,7 +201,7 @@ class Agent(SACAgent): 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 diff --git a/scripts/algorithms/fd/td3_agent.py b/scripts/algorithms/fd/td3_agent.py index 3c50e4c..6111d60 100644 --- a/scripts/algorithms/fd/td3_agent.py +++ b/scripts/algorithms/fd/td3_agent.py @@ -180,7 +180,7 @@ class Agent(TD3Agent): 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 diff --git a/scripts/algorithms/sac/agent.py b/scripts/algorithms/sac/agent.py index 060233e..51c9fce 100644 --- a/scripts/algorithms/sac/agent.py +++ b/scripts/algorithms/sac/agent.py @@ -225,7 +225,7 @@ class Agent(AbstractAgent): 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) @@ -242,7 +242,7 @@ class Agent(AbstractAgent): 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.""" @@ -267,7 +267,7 @@ class Agent(AbstractAgent): """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" diff --git a/scripts/algorithms/td3/agent.py b/scripts/algorithms/td3/agent.py index 7d53fec..84b7701 100644 --- a/scripts/algorithms/td3/agent.py +++ b/scripts/algorithms/td3/agent.py @@ -177,7 +177,7 @@ class Agent(AbstractAgent): 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 @@ class Agent(AbstractAgent): 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 @@ class Agent(AbstractAgent): """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]) diff --git a/scripts/config/demo/__init__.py b/scripts/config/demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/config/demo/open_manipulator/reacher_v0.py b/scripts/config/demo/open_manipulator/reacher_v0.py new file mode 100644 index 0000000..0a4eee8 --- /dev/null +++ b/scripts/config/demo/open_manipulator/reacher_v0.py @@ -0,0 +1,12 @@ +config = { + "USE_PLATFORM": False, + "DAMPING": 0.01, + "JOINT_VEL_LIMIT": 4, + "NUM_TARGET_DEMO": 10, + "HZ": 100, + "SAVE_PATH": "../DemoCollection.json", +} + + +def get(): + return config diff --git a/scripts/config/dynamixel/config.py b/scripts/config/dynamixel/config.py index 76da9b7..8f2d0d7 100755 --- a/scripts/config/dynamixel/config.py +++ b/scripts/config/dynamixel/config.py @@ -3,29 +3,29 @@ config = { "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 + "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 + "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 + "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 + "DEVICENAME": "/dev/ttyUSB0", # Connected USB port + "TORQUE_ENABLE": 1, # Torque on + "TORQUE_DISABLE": 0, # Torque off } diff --git a/scripts/demo/open_manipulator/open_manipulator_demo_collector.py b/scripts/demo/open_manipulator/open_manipulator_demo_collector.py index c38aa15..1d274d0 100755 --- a/scripts/demo/open_manipulator/open_manipulator_demo_collector.py +++ b/scripts/demo/open_manipulator/open_manipulator_demo_collector.py @@ -21,15 +21,15 @@ from urdf_parser_py.urdf import URDF class DemoCollector(object): """Demo collector class which controls openmanipulator based on jacobain method.""" - def __init__(self): + def __init__(self, cfg): rospy.loginfo("Start Demo Collector") # TODO: Receive True or False with parser to check real or simulation. - self.use_platform = rospy.get_param("~use_platform", False) + self.cfg = cfg + self.use_platform = rospy.get_param("~use_platform", self.cfg["USE_PLATFORM"]) self.print_start_message() self.robot_urdf = URDF.from_parameter_server() self.robot = KDLKinematics(self.robot_urdf, "world", "end_effector_link") - self.save_path = "../DemoCollection.json" self.init_shared_variables() self.init_observation() @@ -37,6 +37,8 @@ class DemoCollector(object): self.init_subscriber() self.init_publisher() + self.save_path = self.cfg["SAVE_PATH"] + def print_start_message(self): if self.use_platform is False: rospy.loginfo("Start Gazebo Demo Collector") @@ -72,8 +74,10 @@ class DemoCollector(object): - material: https://www.youtube.com/watch?v=hE_Duih_7JE&list=PLggLP4f-rq00efLcgMcG1m4k5CKlgRcGh """ self.mutex = threading.Lock() - self.damping = rospy.get_param("~damping", 0.01) - self.joint_vel_limit = rospy.get_param("~joint_vel_limit", 4) + self.damping = rospy.get_param("~damping", self.cfg["DAMPING"]) + self.joint_vel_limit = rospy.get_param( + "~joint_vel_limit", self.cfg["JOINT_VEL_LIMIT"] + ) self.q = np.zeros(4) # Joint angles self.q_desired = np.zeros(4) self.qdot = np.zeros(4) # Joint velocities @@ -82,10 +86,7 @@ class DemoCollector(object): self.T_goal = np.array(self.robot.forward(self.q)) self.T_cur = np.array(self.robot.forward(self.q)) - # TODO: extract num_tar_demo to config - self.num_tar_demo = 3 - self.num_cur_demo = 0 - + self.num_target_demo = self.cfg["NUM_TARGET_DEMO"] self.control_start_time = self.get_rostime() def init_observation(self): @@ -144,13 +145,13 @@ class DemoCollector(object): collection. """ # TODO: extract 100 to config - self.hz = 100 + self.hz = self.cfg["HZ"] self.r = rospy.Rate(self.hz) self.start_log() self.q_init = list(self.q) self.done_move_to_target = False - for i in range(self.num_tar_demo): + for i in range(self.num_target_demo): print("Episode: ", i) rospy.loginfo("Moving to Initial Position") # go to init pose @@ -235,6 +236,9 @@ class DemoCollector(object): def move_to_target(self, rollout_num): """Move robot to target. + [Resolved Rate Motion Control] + - material: https://www.youtube.com/embed/rkHs7K0ad14?rel=0&showinfo=0 + [Resolved Rate Motion Control] - material: https://www.youtube.com/embed/rkHs7K0ad14?rel=0&showinfo=0 @@ -249,7 +253,6 @@ class DemoCollector(object): 5) Get q_new by inverse term. 6) Scaling joint velocities. 7) Set joint states. - 8) Save file. """ t_now = rospy.get_rostime().secs + rospy.get_rostime().nsecs * 10 ** -9 # TODO: why mutex needed? @@ -396,17 +399,26 @@ class DemoCollector(object): x_dot_f: velocity when x_f """ if t < t_0: - x_t = x_0 # theta(0) + x_t = x_0 elif t > t_f: - x_t = x_f # theta(t_f) + x_t = x_f else: total_x = x_f - x_0 - elapsed_t = t - t_0 # t - total_t = t_f - t_0 # t_f + elapsed_t = t - t_0 + total_t = t_f - t_0 - x_t = (x_0 + x_dot_0 * elapsed_t - + (3 * total_x / total_t**2 - 2 * x_dot_0 / total_t - x_dot_f / total_t) * elapsed_t**2 - + (-2 * total_x / total_t**3 + (x_dot_0 + x_dot_f) / total_t**2) * elapsed_t**3) + x_t = ( + x_0 + + x_dot_0 * elapsed_t + + ( + 3 * total_x / total_t ** 2 + - 2 * x_dot_0 / total_t + - x_dot_f / total_t + ) + * elapsed_t ** 2 + + (-2 * total_x / total_t ** 3 + (x_dot_0 + x_dot_f) / total_t ** 2) + * elapsed_t ** 3 + ) return x_t diff --git a/scripts/dynamixel_read_write.py b/scripts/dxl_read_write.py similarity index 99% rename from scripts/dynamixel_read_write.py rename to scripts/dxl_read_write.py index d7eeef0..6571f0d 100755 --- a/scripts/dynamixel_read_write.py +++ b/scripts/dxl_read_write.py @@ -27,6 +27,7 @@ from utils import deg2rad, rad2deg, rpm2rad class DynamixelPositionControl(object): """Dynamixel read & write class.""" + def __init__(self, cfg): # Dynamixel Setting rospy.loginfo("Dynamixel Position Controller Created") diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py index e47c385..9c85140 100755 --- a/scripts/envs/open_manipulator/ros_interface.py +++ b/scripts/envs/open_manipulator/ros_interface.py @@ -10,7 +10,6 @@ import rospy # noqa import tf # noqa import tf.transformations as tr # noqa from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel # noqa -from geometry_msgs.msg import Pose from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState from pykdl_utils.kdl_kinematics import KDLKinematics from sensor_msgs.msg import JointState @@ -266,7 +265,7 @@ class OpenManipulatorRosBaseInterface(object): ) else: raise ValueError("Control mode %s is not known!" % control_mode) - print (lower_bounds, upper_bounds, self.cfg["ACTION_DIM"]) + print(lower_bounds, upper_bounds, self.cfg["ACTION_DIM"]) return gym.spaces.Box(low=lower_bounds, high=upper_bounds, dtype=np.float32) def get_observation_space(self): @@ -305,7 +304,7 @@ class OpenManipulatorRosBaseInterface(object): if dist < self.distance_threshold: self.success_count += 1 if self.success_count == self.cfg["SUCCESS_COUNT"]: - print ("Current episode succeeded") + print("Current episode succeeded") return True else: return False @@ -361,7 +360,7 @@ class OpenManipulatorRosBaseInterface(object): rospy.logwarn("OUT OF BOUNDARY : joint_1_limit exceeds") if self.termination_count == term_count: - print ("Current episode terminated") + print("Current episode terminated") self.termination_count = 0 return True else: @@ -396,7 +395,7 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface): """Set target block Gazebo model""" # random generated blocks for train if block_pose is None: - polar_rad, polar_theta, z, overhead_orientation = ( + polar_rad, polar_theta, z, _ = ( np.random.uniform(*self.cfg["POLAR_RADIAN_BOUNDARY"]), np.random.uniform(*self.cfg["POLAR_THETA_BOUNDARY"]), np.random.uniform(*self.cfg["Z_BOUNDARY"]), diff --git a/scripts/gazebo_test_open_manipulator.py b/scripts/gazebo_test_open_manipulator.py index 061caca..015a5b7 100755 --- a/scripts/gazebo_test_open_manipulator.py +++ b/scripts/gazebo_test_open_manipulator.py @@ -57,13 +57,13 @@ def test_rotate(): "/open_manipulator/goal_joint_space_path_from_present", SetJointPosition ) _ = task_space_srv("arm", _qpose, 2.0) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.loginfo("Path planning service call failed: {0}".format(e)) _qpose.position[0] += -1.0 _qpose.position[3] += -1.0 try: _ = task_space_srv("arm", _qpose, 2.0) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.loginfo("Path planning service call failed: {0}".format(e)) @@ -103,7 +103,7 @@ def test_achieve_goal(): "/open_manipulator/goal_task_space_path", SetKinematicsPose ) _ = task_space_srv("arm", "gripper", forward_pose, 2.0) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.loginfo("Path planning service call failed: {0}".format(e)) rospy.sleep(5.0) env.ros_interface.delete_target_block() @@ -134,7 +134,7 @@ def test_workspace_limit(): "/open_manipulator/goal_task_space_path", SetKinematicsPose ) _ = task_space_srv("arm", "gripper", forward_pose, 3.0) - except rospy.ServiceException, e: + except rospy.ServiceException as e: rospy.loginfo("Path planning service call failed: {0}".format(e)) rospy.sleep(3.0) env.ros_interface.check_for_termination() diff --git a/scripts/run_open_manipulator_demo.py b/scripts/run_open_manipulator_demo.py index 72eaab0..6acfaf4 100644 --- a/scripts/run_open_manipulator_demo.py +++ b/scripts/run_open_manipulator_demo.py @@ -6,68 +6,19 @@ """ import rospy -import importlib - -import gym - -import algorithms.common.helper_functions as common_utils - -# configurations -parser = argparse.ArgumentParser(description="Pytorch RL algorithms") -parser.add_argument( - "--seed", type=int, default=777, help="random seed for reproducibility" -) -parser.add_argument("--algo", type=str, default="sac", help="choose an algorithm") -parser.add_argument( - "--test", dest="test", action="store_true", help="test mode (no training)" -) -parser.add_argument( - "--load-from", type=str, help="load the saved model and optimizer at the beginning" -) -parser.add_argument( - "--off-render", dest="render", action="store_false", help="turn off rendering" -) -parser.add_argument( - "--render-after", - type=int, - default=0, - help="start rendering after the input number of episode", -) -parser.add_argument("--log", dest="log", action="store_true", help="turn on logging") -parser.add_argument("--save-period", type=int, default=200, help="save model period") -parser.add_argument("--episode-num", type=int, default=20000, help="total episode num") -parser.add_argument( - "--max-episode-steps", type=int, default=-1, help="max episode step" -) -parser.add_argument( - "--demo-path", type=str, default="data/reacher_demo.pkl", help="demonstration path" -) - -parser.set_defaults(test=False) -parser.set_defaults(load_from=None) -parser.set_defaults(render=True) -parser.set_defaults(log=False) -args = parser.parse_args() +from config.demo.open_manipulator.reacher_v0 import config as cfg +from demo.open_manipulator.open_manipulator_demo_collector import DemoCollector def main(): """Main.""" # env initialization - env = gym.make("Reacher-v1") - # set a random seed - common_utils.set_random_seed(args.seed, env) - - # agent initialization - module_path = "config.agent.reacher-v1." + args.algo - agent = importlib.import_module(module_path) - agent = agent.get(env, args) - - # run - if args.test: - agent.test() - else: - agent.train() + try: + collector = DemoCollector(cfg) + collector.run() + except rospy.ROSInterruptException: + pass if __name__ == "__main__": diff --git a/scripts/test_lstm.py b/scripts/test_lstm.py index 1688407..b69d640 100644 --- a/scripts/test_lstm.py +++ b/scripts/test_lstm.py @@ -114,7 +114,7 @@ if __name__ == "__main__": loss.backward() optimizer.step() - print ("[epoch: %d] loss: %f" % i, loss) + print("[epoch: %d] loss: %f" % i, loss) # eval eval_data = generate_wave_data(x_range, input_wave_nm, True, num_test_waves)