From 8cbfe50a9eb320a03ad544e28d5b8fa1034c85fc Mon Sep 17 00:00:00 2001 From: kdh0429 Date: Thu, 30 May 2019 23:25:09 +0900 Subject: [PATCH] Add doctrings and change file directory --- scripts/config/dynamixel/config.py | 31 ++++++++ scripts/demo/open_manipulator/config.py | 36 --------- .../open_manipulator_demo_collector.py | 23 +++--- scripts/demo/open_manipulator/utils.py | 13 ---- ..._read_write.py => dynamixel_read_write.py} | 14 +++- scripts/requirements-dev.txt | 2 +- scripts/run_open_manipulator_demo.py | 74 +++++++++++++++++++ scripts/utils.py | 21 ++++++ 8 files changed, 152 insertions(+), 62 deletions(-) create mode 100755 scripts/config/dynamixel/config.py delete mode 100755 scripts/demo/open_manipulator/config.py delete mode 100755 scripts/demo/open_manipulator/utils.py rename scripts/{demo/open_manipulator/pos_read_write.py => dynamixel_read_write.py} (96%) create mode 100644 scripts/run_open_manipulator_demo.py create mode 100755 scripts/utils.py diff --git a/scripts/config/dynamixel/config.py b/scripts/config/dynamixel/config.py new file mode 100755 index 0000000..76da9b7 --- /dev/null +++ b/scripts/config/dynamixel/config.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 +} diff --git a/scripts/demo/open_manipulator/config.py b/scripts/demo/open_manipulator/config.py deleted file mode 100755 index 113e110..0000000 --- a/scripts/demo/open_manipulator/config.py +++ /dev/null @@ -1,36 +0,0 @@ -config = { - # pos_read_write - # TODO : Change Control Table to XM430-W350 spec ### - "DXL_RESOLUTION": 0.088, # In degree - "DXL_VELOCITY_RESOLUTION": 0.229, # In rpm # For XM430-W210 0.229 - "DXL_TO_CURRENT": 2.69, # 2.69 mA - # Control table address - "ADDR_TORQUE_ENABLE": 64, - "ADDR_PRESENT_POSITION": 132, - "ADDR_PRESENT_VELOCITY": 128, - "ADDR_PRESENT_CURRENT": 126, - "ADDR_OP_MODE": 11, - "ADDR_GOAL_POSITION": 116, - # Data Byte Length - "LEN_GOAL_POSITION": 4, - "LEN_PRESENT_POSITION": 4, - "LEN_PRESENT_VELOCITY": 4, - "LEN_PRESENT_CURRENT": 2, - "CW_LIMIT": 4095, - "CCW_LIMIT": 0, - "DXL_POS_OFFSET": 2048, - # Protocol version - "PROTOCOL_VERSION": 2.0, - # Default setting - "DXL1_ID": 11, - "DXL2_ID": 12, - "DXL3_ID": 13, - "DXL4_ID": 14, - "BAUDRATE": 1000000, - "DEVICENAME": "/dev/ttyUSB0", - "TORQUE_ENABLE": 1, - "TORQUE_DISABLE": 0, - # Collector - "DAMPING": 0.02, - "JOINT_VEL_LIMIT": 4, -} diff --git a/scripts/demo/open_manipulator/open_manipulator_demo_collector.py b/scripts/demo/open_manipulator/open_manipulator_demo_collector.py index 2985bcc..514cd21 100755 --- a/scripts/demo/open_manipulator/open_manipulator_demo_collector.py +++ b/scripts/demo/open_manipulator/open_manipulator_demo_collector.py @@ -1,14 +1,17 @@ #!/usr/bin/env python +"""Collect demos using jacobian based control. + +- Author: DH Kim +- Contact: kdh0429@snu.ac.kr +""" import json import random import threading import time from collections import OrderedDict from math import pi, pow - import numpy as np - # ROS Imports import rospy from pykdl_utils.kdl_kinematics import KDLKinematics @@ -16,14 +19,10 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Float64, Float64MultiArray from urdf_parser_py.urdf import URDF -#################### -# GLOBAL VARIABLES # -#################### -DAMPING = 0.01 # 0.00 -JOINT_VEL_LIMIT = 4 # 2rad/s - class DemoCollector(object): + """Demo collector class which controls openmanipulator based on jacobain method.""" + def __init__(self): rospy.loginfo("Start Demo Collector") @@ -33,8 +32,8 @@ class DemoCollector(object): # Shared variables self.mutex = threading.Lock() - self.damping = rospy.get_param("~damping", DAMPING) - self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT) + self.damping = rospy.get_param("~damping", 0.01) + self.joint_vel_limit = rospy.get_param("~joint_vel_limit", 4) self.q = np.zeros(4) # Joint angles self.q_desired = np.zeros(4) self.qdot = np.zeros(4) # Joint velocities @@ -117,8 +116,10 @@ class DemoCollector(object): if self.num_cur_demo > self.num_tar_demo: print("Demo Collection Finished!") self.is_finished = True + quit() def joint_states_cb(self, joint_states): + """ Save joint states published in ROS to class member.""" self.is_joint_states_cb = True i = 0 while i < 4: @@ -128,9 +129,11 @@ class DemoCollector(object): i += 1 def start_log(self): + """ Start logging in .txt format.""" self.f = open("../DemoEpisode" + str(self.num_cur_demo) + ".txt", "w") def set_target(self): + """ Randomly set target within joint limit and workspace limit.""" appropriate_target = False while appropriate_target is False: q_limit_L = [-pi * 0.5, -pi * 0.5, -pi * 0.3, -pi * 0.57] diff --git a/scripts/demo/open_manipulator/utils.py b/scripts/demo/open_manipulator/utils.py deleted file mode 100755 index e3eb319..0000000 --- a/scripts/demo/open_manipulator/utils.py +++ /dev/null @@ -1,13 +0,0 @@ -from math import pi - - -def deg2rad(deg): - return deg * pi / 180 - - -def rad2deg(rad): - return rad * 180 / pi - - -def rpm2rad(rpm): - return 2 * rpm * pi / 60 diff --git a/scripts/demo/open_manipulator/pos_read_write.py b/scripts/dynamixel_read_write.py similarity index 96% rename from scripts/demo/open_manipulator/pos_read_write.py rename to scripts/dynamixel_read_write.py index 64131ac..d7eeef0 100755 --- a/scripts/demo/open_manipulator/pos_read_write.py +++ b/scripts/dynamixel_read_write.py @@ -1,10 +1,15 @@ #!/usr/bin/env python +"""Read dynamixel state and publish through ROS. Also, control dynamixel position with subscribed joint command + +- Author: DH Kim +- Contact: kdh0429@snu.ac.kr +""" import numpy as np # ROS Imports import rospy -from config import config as cfg +from config.dynamixel import config as cfg from dynamixel_sdk import ( DXL_HIBYTE, DXL_HIWORD, @@ -21,6 +26,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") @@ -215,6 +221,7 @@ class DynamixelPositionControl(object): ) def joint_command_cb(self, joint_desired): + """ Transform subscribed joint command to dynamixel byte information.""" i = 0 while i < 4: self.q_desired[i] = joint_desired.data[i] @@ -236,6 +243,7 @@ class DynamixelPositionControl(object): i += 1 def read_dxl(self): + """ Read dynamixel position, velocity, current value and publish through ROS.""" self.groupBulkReadPosition.txRxPacket() self.dxl_present_position[0] = self.groupBulkReadPosition.getData( @@ -361,6 +369,7 @@ class DynamixelPositionControl(object): self.joint_states_pub.publish(self.joint_states) def write_dxl(self): + """ Write joint command to dynamixel.""" self.groupSyncWrite.addParam(self.cfg["DXL1_ID"], self.dxl_goal_position[0]) self.groupSyncWrite.addParam(self.cfg["DXL2_ID"], self.dxl_goal_position[1]) self.groupSyncWrite.addParam(self.cfg["DXL3_ID"], self.dxl_goal_position[2]) @@ -370,6 +379,7 @@ class DynamixelPositionControl(object): self.groupSyncWrite.clearParam() def error_check(self, dxl_comm_result, dxl_error): + """ Check dynamixel error.""" if dxl_comm_result != self.cfg["COMM_SUCCESS"]: print("%s" % self.packetHandler.getTxRxResult(dxl_comm_result)) elif dxl_error != 0: @@ -377,7 +387,7 @@ class DynamixelPositionControl(object): def main(): - rospy.init_node("DXL_pos_control") + rospy.init_node("dynamixel_read_write") try: DynamixelPositionControl(cfg) diff --git a/scripts/requirements-dev.txt b/scripts/requirements-dev.txt index b1be888..b526747 100644 --- a/scripts/requirements-dev.txt +++ b/scripts/requirements-dev.txt @@ -1,4 +1,4 @@ -pre-commit +#pre-commit # formatting isort diff --git a/scripts/run_open_manipulator_demo.py b/scripts/run_open_manipulator_demo.py new file mode 100644 index 0000000..72eaab0 --- /dev/null +++ b/scripts/run_open_manipulator_demo.py @@ -0,0 +1,74 @@ +# -*- coding: utf-8 -*- +"""Collect demo data with jacobian based control method. + +- Author: DH Kim +- Contact: kdh0429@snu.ac.kr +""" + +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() + + +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() + + +if __name__ == "__main__": + main() diff --git a/scripts/utils.py b/scripts/utils.py new file mode 100755 index 0000000..2712340 --- /dev/null +++ b/scripts/utils.py @@ -0,0 +1,21 @@ +"""Utils for dynamixel. + +- Author: DH Kim +- Contact: kdh0429@snu.ac.kr +""" +from math import pi + + +def deg2rad(deg): + """ Transform degree to radian.""" + return deg * pi / 180 + + +def rad2deg(rad): + """ Transform radian to degree.""" + return rad * 180 / pi + + +def rpm2rad(rpm): + """ Transform RPM to radian.""" + return 2 * rpm * pi / 60