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

Modified demo config and run demo script (#83)

Browse Source 
* Add removed q_dot scaling factor

* Refactored cubic function

* Separate config and add run file.

* Resolve flake8 issue

* Resolve flake8 issue

* Remove whitespace before ( issue
This commit is contained in:
Whi Kwon
2019-07-12 01:26:34 +09:00
committed by GitHub
parent 2f8dbf5249
commit 0fac485205
14 changed files with 90 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/algorithms/common/abstract/agent.py
+2 -2
View File
@@ -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)
)
scripts/algorithms/fd/sac_agent.py
+1 -1
View File
@@ -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
scripts/algorithms/fd/td3_agent.py
+1 -1
View File
@@ -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
scripts/algorithms/sac/agent.py
+3 -3
View File
@@ -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"
scripts/algorithms/td3/agent.py
+3 -3
View File
@@ -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])
scripts/config/demo/__init__.py
View File
scripts/config/demo/open_manipulator/reacher_v0.py
+12
View File
@@ -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
scripts/config/dynamixel/config.py
+20 -20
View File
@@ -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
}
scripts/demo/open_manipulator/open_manipulator_demo_collector.py
+31 -19
View File
@@ -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
scripts/dynamixel_read_write.py → scripts/dxl_read_write.py
+1
View File
@@ -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")
scripts/envs/open_manipulator/ros_interface.py
+4 -5
View File
@@ -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"]),
scripts/gazebo_test_open_manipulator.py
+4 -4
View File
@@ -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()
scripts/run_open_manipulator_demo.py
+7 -56
View File
@@ -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__":
scripts/test_lstm.py
+1 -1
View File
@@ -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)