wassname/kair_algorithms_draft
1
0
Fork 0
mirror of https://github.com/wassname/kair_algorithms_draft.git synced 2026-09-09 11:25:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add doctrings and change file directory

Browse Source
This commit is contained in:
kdh0429
2019-05-30 23:25:09 +09:00
parent bd1711305b
commit 8cbfe50a9e
8 changed files with 152 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/config/dynamixel/config.py
Executable
+31
View File
@@ -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
}
scripts/demo/open_manipulator/config.py
-36
View File
@@ -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,
}
scripts/demo/open_manipulator/open_manipulator_demo_collector.py
+13 -10
View File
@@ -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]
scripts/demo/open_manipulator/utils.py
-13
View File
@@ -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
scripts/demo/open_manipulator/pos_read_write.py → scripts/dynamixel_read_write.py
+12 -2
View File
@@ -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)
scripts/requirements-dev.txt
+1 -1
View File
@@ -1,4 +1,4 @@
pre-commit
#pre-commit
# formatting
isort
scripts/run_open_manipulator_demo.py
+74
View File
@@ -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()
scripts/utils.py
Executable
+21
View File
@@ -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