diff --git a/launch/open_manipulator_demo.launch b/launch/open_manipulator_demo.launch new file mode 100644 index 0000000..093559a --- /dev/null +++ b/launch/open_manipulator_demo.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 8267f97..529b121 100644 --- a/scripts/algorithms/sac/agent.py +++ b/scripts/algorithms/sac/agent.py @@ -264,7 +264,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) @@ -281,7 +281,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.""" @@ -306,7 +306,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 723636d..e4497ef 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/__init__.py b/scripts/config/demo/open_manipulator/__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 100755 index 0000000..4130d60 --- /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": "../demo_collection.json", +} + + +def get(): + return config diff --git a/scripts/config/dynamixel/config.py b/scripts/config/dynamixel/config.py new file mode 100755 index 0000000..8f2d0d7 --- /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/__init__.py b/scripts/demo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/demo/open_manipulator/__init__.py b/scripts/demo/open_manipulator/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/scripts/demo/open_manipulator/open_manipulator_demo_collector.py b/scripts/demo/open_manipulator/open_manipulator_demo_collector.py new file mode 100755 index 0000000..ede3818 --- /dev/null +++ b/scripts/demo/open_manipulator/open_manipulator_demo_collector.py @@ -0,0 +1,418 @@ +#!/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 defaultdict +from math import pi, pow + +import numpy as np + +import rospy +from pykdl_utils.kdl_kinematics import KDLKinematics +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64, Float64MultiArray +from urdf_parser_py.urdf import URDF + + +class DemoCollector(object): + """Demo collector class which controls openmanipulator based on jacobain method.""" + + def __init__(self, cfg): + rospy.init_node("demo_collector") + rospy.loginfo("Start Demo Collector") + # TODO: Receive True or False with parser to check real or simulation. + 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.init_shared_variables() + self.init_observation() + + 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") + else: + rospy.loginfo("Start Real Demo Collector") + + def init_shared_variables(self): + """Initialize shared variables. + + q, qdot, effort: 4 size array. + Receive from joint state callback. + + q: [joint1_position, joint2_position, joint3_position, joint4_position] + qdot: [joint1_velocity, joint2_velocity, joint3_velocity, joint4_velocity] + effot: [joint1_torque, joint2_torque, joint3_torque, joint4_torque] + ex) [q1, q2, q3, q4] + + T: 4x4 transformation matrix + Get matrix by solve forward kinematics. + [R p + 0 1] + - R: Rotation matrix + - p: position + ex) + [s -c 0 x + c s 0 y + 0 0 1 z + 0 0 0 1] + + [Homogeneous Transformation Matrices] + - material: https://www.youtube.com/watch?v=vlb3P7arbkU + [Forward Kinematics] + - material: https://www.youtube.com/watch?v=hE_Duih_7JE&list=PLggLP4f-rq00efLcgMcG1m4k5CKlgRcGh + """ + self.mutex = threading.Lock() + 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 + self.effort = np.zeros(4) # Joint torque + self.T_target = np.array(self.robot.forward(self.q)) + self.T_goal = np.array(self.robot.forward(self.q)) + self.T_cur = np.array(self.robot.forward(self.q)) + + self.num_target_demo = self.cfg["NUM_TARGET_DEMO"] + self.control_start_time = self.get_rostime() + + def init_observation(self): + """Initialize observation""" + self._gripper_pos = np.zeros(3) + self._gripper_orientation = np.zeros(4) + + def init_subscriber(self): + """Initialize joint states subscriber.""" + if self.use_platform is False: + self.joint_states_sub = rospy.Subscriber( + "/open_manipulator/joint_states", JointState, self.joint_states_cb + ) + else: + self.joint_states_sub = rospy.Subscriber( + "/open_manipulator/joint_states_real", JointState, self.joint_states_cb + ) + + def init_publisher(self): + """Initialize joint command publisher.""" + self.j1_pos_command_pub = rospy.Publisher( + "/open_manipulator/joint1_position/command", Float64, queue_size=3 + ) + self.j2_pos_command_pub = rospy.Publisher( + "/open_manipulator/joint2_position/command", Float64, queue_size=3 + ) + self.j3_pos_command_pub = rospy.Publisher( + "/open_manipulator/joint3_position/command", Float64, queue_size=3 + ) + self.j4_pos_command_pub = rospy.Publisher( + "/open_manipulator/joint4_position/command", Float64, queue_size=3 + ) + self.joint_pos_command_to_dxl_pub = rospy.Publisher( + "/open_manipulator/joint_position/command", Float64MultiArray, queue_size=3 + ) + + def publish_pos_commands(self, q_desired): + self.j1_pos_command_pub.publish(q_desired[0]) + self.j2_pos_command_pub.publish(q_desired[1]) + self.j3_pos_command_pub.publish(q_desired[2]) + self.j4_pos_command_pub.publish(q_desired[3]) + self.joint_pos_command_to_dxl_pub.publish(data=q_desired) + + def joint_states_cb(self, joint_states): + """ Save joint states published in ROS to class member.""" + for i in range(4): + self.q[i] = joint_states.position[i + 2] + self.qdot[i] = joint_states.velocity[i + 2] + self.effort[i] = joint_states.effort[i + 2] + + def get_rostime(self): + return rospy.get_rostime().secs + rospy.get_rostime().nsecs * 10 ** -9 + + def run(self): + """Run demo collection. + + 1) If init is false, do initial setting. + 2) If target is not set, and robot is initial pose, set new target. + 3) If target is set, move to target. + 4) If robot pose is not initial pose, move to initial pose. + 5) If target is set and robot pose is not initial pose, take ros sleep. + 6) If number of target demo is bigger than number of current demo, finish demo + collection. + """ + self.hz = self.cfg["HZ"] + self.r = rospy.Rate(self.hz) + self.start_log() + self.q_init = list(self.q) + + for i in range(self.num_target_demo): + print("Episode: ", i) + rospy.loginfo("Moving to Initial Position") + # go to init pose + self.done_init = False + self.control_start_time = self.get_rostime() + while not self.done_init: + self.move_to_init() + self.r.sleep() + + self.set_target() + self.T_init = np.array(self.robot.forward(self.q)) + + # go to target + self.done_move_to_target = False + self.control_start_time = self.get_rostime() + while not self.done_move_to_target: + self.move_to_target(i) # run 3 + self.r.sleep() + + print("Demo Collection Finished!") + self.save_demo_collection(self.save_path) + quit() + + def save_demo_collection(self, save_path): + with open(save_path, "w") as f: + json.dump(self.data, f) + print("Demo file saved successfully") + + def start_log(self): + """ Start logging in dict(dict(list)) type.""" + self.data = defaultdict(lambda: defaultdict(list)) + + def set_target(self): + """ Randomly set target within joint limit and workspace limit. + + q_limit_L: Low limit of joint. + q_limit_H: High limit of joint. + q_rand: Randomly set joint angle. + T target: Transformation matrix of randomly set target. + Get matrix by robot forward kinematics of q_rand. + target: Position of randomly set target. + Get target array from transformation matrix. + min_op_distance: Minimum workspace distance limit. + max_op_distance: Maximum workspace distance limit. + + 1) Initialize and set new target if target is not appropriate. + 2) Get random joint angle within joint limits. + 3) Get transformation matrix and position array by robot forward kinmetics. + 4) Calculate target position within workspace distance limits. + 5) Check if it is appropriate target. Finish loop if it is true, othercase + continue loop. + """ + appropriate_target = False + while appropriate_target is False: + q_limit_L = [-pi * 0.5, -pi * 0.5, -pi * 0.3, -pi * 0.57] + q_limit_H = [pi * 0.5, pi * 0.5, pi * 0.44, pi * 0.65] + rand_scale = np.zeros(4) + q_rand = np.zeros(4) + for i in range(4): + rand_scale[i] = random.random() + q_rand[i] = rand_scale[i] * (q_limit_H[i] - q_limit_L[i]) + q_limit_L[i] + + self.T_target = np.array(self.robot.forward(q_rand)) + target = np.empty_like(self.T_target[:3, 3]) + target[:] = self.T_target[:3, 3] + + min_op_distance = 0.15 + max_op_distance = 0.4 + + if np.linalg.norm(np.abs(target)) > max_op_distance: + target = target * max_op_distance / np.linalg.norm(np.abs(target)) + + if np.linalg.norm(np.abs(target)) < min_op_distance: + target = target * min_op_distance / np.linalg.norm(np.abs(target)) + + if target[0] > 0.0: + if target[2] > 0.04: + self.T_target[:3, 3] = target + appropriate_target = True + print("Target :", self.T_target[:3, 3]) + return + + 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 + + q_now: Current joint angles. + T_cur: Current transformation matrix. + + 1) Get current time + 2) Get gripper position and orientation by calculate forward kinematics + of current joint angles. + 3) Path planning by cubic function. Set goal matrix. + 4) Get jacobian . + 5) Get q_new by inverse term. + 6) Scaling joint velocities. + 7) Set joint states. + """ + t_now = self.get_rostime() + with self.mutex: + q_now = self.q + + self.T_cur = np.array(self.robot.forward(q_now)) + self._gripper_pos = self.T_cur[:3, 3] + self._gripper_orientation[3] = ( + 1 + self.T_cur[0, 0] + self.T_cur[1, 1] + self.T_cur[2, 2] + ) ** 0.5 + self._gripper_orientation[0] = (self.T_cur[2, 1] - self.T_cur[1, 2]) / ( + 4 * self._gripper_orientation[3] + ) + self._gripper_orientation[1] = (self.T_cur[0, 2] - self.T_cur[2, 0]) / ( + 4 * self._gripper_orientation[3] + ) + self._gripper_orientation[2] = (self.T_cur[1, 0] - self.T_cur[0, 1]) / ( + 4 * self._gripper_orientation[3] + ) + + # implement multi-array cubic calculation + for i in range(3): + self.T_goal[i, 3] = self.cubic( + t_now, + self.control_start_time, + self.control_start_time + 2.0, + self.T_init[i, 3], + self.T_target[i, 3], + 0.0, + 0.0, + ) + + e = self.T_goal[:3, 3] - self.T_cur[:3, 3] + + Jb = np.array(self.robot.jacobian(q_now)) + Jv = Jb[:3, :] + + invterm = np.linalg.inv(np.dot(Jv, Jv.T) + pow(self.damping, 2) * np.eye(3)) + kp = 2.0 + qdot_new = np.dot(np.dot(Jv.T, invterm), kp * e) + + # Scaling joint velocity + def _limit_q_dot(qdot_new): + minus_v = abs(np.amin(qdot_new)) + plus_v = abs(np.amax(qdot_new)) + if minus_v > plus_v: + scale = minus_v + else: + scale = plus_v + if scale > self.joint_vel_limit: + qdot_new = qdot_new / scale * self.joint_vel_limit + return qdot_new + + self.qdot = _limit_q_dot(qdot_new) + + def _get_observation(): + return np.concatenate( + [ + self._gripper_pos, + self._gripper_orientation, + self.q, + self.qdot, + self.effort, + ] + ) + + state = _get_observation() + + dt = 1.0 / self.hz + self.q_desired = self.q_desired + qdot_new * dt + self.q_desired = self.joint_limit_check(self.q_desired) + self.publish_pos_commands(self.q_desired) + next_state = _get_observation() + + def _is_done_move_to_target(): + if np.mean(np.abs(self.T_target[:3, 3] - self.T_cur[:3, 3])) < 0.001: + self.q_init = list(self.q) + print("Target arrived!") + return True + else: + return False + + self.done_move_to_target = _is_done_move_to_target() + + # append data + self.data[rollout_num]["state"].append(state.tolist()) + self.data[rollout_num]["action"].append(self.q_desired.tolist()) + self.data[rollout_num]["next_state"].append(next_state.tolist()) + self.data[rollout_num]["curr_xyz"].append(self.T_cur[:3, 3].tolist()) + self.data[rollout_num]["target_xyz"].append(self.T_target[:3, 3].tolist()) + self.data[rollout_num]["done"].append(self.done_move_to_target) + + def move_to_init(self): + """Move robot to initial pose.""" + t_now = self.get_rostime() + for i in range(4): + self.q_desired[i] = self.cubic( + t_now, + self.control_start_time, + self.control_start_time + 3.0, + self.q_init[i], + 0.0, + 0.0, + 0.0, + ) + self.q_desired = self.joint_limit_check(self.q_desired) + self.publish_pos_commands(self.q_desired) + + def _is_done_init(): + if np.mean(np.abs(np.zeros(4) - self.q)) < 0.05: + time.sleep(2.0) + print("Initial Pose Arrived!") + return True + else: + return False + + self.done_init = _is_done_init() + + def joint_limit_check(self, q_target): + q_limit_L = [-pi * 0.9, -pi * 0.57, -pi * 0.3, -pi * 0.57] + q_limit_H = [pi * 0.9, pi * 0.5, pi * 0.44, pi * 0.65] + for i in range(4): + if q_target[i] < q_limit_L[i]: + q_target[i] = q_limit_L[i] + elif q_target[i] > q_limit_H[i]: + q_target[i] = q_limit_H[i] + return q_target + + def cubic(self, t, t_0, t_f, x_0, x_f, x_dot_0, x_dot_f): + """ + [Cubic polynomials] + - material: http://ocw.snu.ac.kr/sites/default/files/NOTE/Chap07_Trajectory%20generation.pdf + + t: time + t_0: init time + t_f: final time + x_0: init position + x_f: final position + x_dot_0: velocity when x_0 + x_dot_f: velocity when x_f + """ + if t < t_0: + x_t = x_0 + elif t > t_f: + x_t = x_f + else: + total_x = x_f - x_0 + 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 + ) + return x_t diff --git a/scripts/dxl_read_write.py b/scripts/dxl_read_write.py new file mode 100755 index 0000000..6571f0d --- /dev/null +++ b/scripts/dxl_read_write.py @@ -0,0 +1,402 @@ +#!/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.dynamixel import config as cfg +from dynamixel_sdk import ( + DXL_HIBYTE, + DXL_HIWORD, + DXL_LOBYTE, + DXL_LOWORD, + GroupBulkRead, + GroupSyncWrite, + PacketHandler, + PortHandler, +) +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64MultiArray +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") + self.cfg = cfg + self.portHandler = PortHandler(self.cfg["DEVICENAME"]) + self.packetHandler = PacketHandler(self.cfg["PROTOCOL_VERSION"]) + self.groupSyncWrite = GroupSyncWrite( + self.portHandler, + self.packetHandler, + self.cfg["ADDR_GOAL_POSITION"], + self.cfg["LEN_GOAL_POSITION"], + ) + self.groupBulkReadPosition = GroupBulkRead(self.portHandler, self.packetHandler) + self.groupBulkReadVelocity = GroupBulkRead(self.portHandler, self.packetHandler) + self.groupBulkReadCurrent = GroupBulkRead(self.portHandler, self.packetHandler) + # Port Open + if self.portHandler.openPort(): + print("Succeeded to open the port") + else: + print("Failed to open the port") + quit() + + # Set port baudrate + if self.portHandler.setBaudRate(self.cfg["BAUDRATE"]): + print("Succeeded to change the baudrate") + else: + print("Failed to change the baudrate") + quit() + + self.packetHandler.write1ByteTxRx( + self.portHandler, self.cfg["DXL1_ID"], self.cfg["ADDR_OP_MODE"], 3 + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, self.cfg["DXL2_ID"], self.cfg["ADDR_OP_MODE"], 3 + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, self.cfg["DXL3_ID"], self.cfg["ADDR_OP_MODE"], 3 + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, self.cfg["DXL4_ID"], self.cfg["ADDR_OP_MODE"], 3 + ) + + self.groupBulkReadPosition.addParam( + self.cfg["DXL1_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.groupBulkReadPosition.addParam( + self.cfg["DXL2_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.groupBulkReadPosition.addParam( + self.cfg["DXL3_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.groupBulkReadPosition.addParam( + self.cfg["DXL4_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.groupBulkReadVelocity.addParam( + self.cfg["DXL1_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + self.groupBulkReadVelocity.addParam( + self.cfg["DXL2_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + self.groupBulkReadVelocity.addParam( + self.cfg["DXL3_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + self.groupBulkReadVelocity.addParam( + self.cfg["DXL4_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + + self.groupBulkReadCurrent.addParam( + self.cfg["DXL1_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + self.groupBulkReadCurrent.addParam( + self.cfg["DXL2_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + self.groupBulkReadCurrent.addParam( + self.cfg["DXL3_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + self.groupBulkReadCurrent.addParam( + self.cfg["DXL4_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + + # Enable Dynamixel Torque + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL1_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_ENABLE"], + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL2_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_ENABLE"], + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL3_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_ENABLE"], + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL4_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_ENABLE"], + ) + + # ROS Publisher + self.joint_states_pub = rospy.Publisher( + "/open_manipulator/joint_states_real", JointState, queue_size=3 + ) + # ROS Subcriber + self.joint_pos_command_sub = rospy.Subscriber( + "/open_manipulator/joint_position/command", + Float64MultiArray, + self.joint_command_cb, + ) + + self.joint_states = JointState() + self.dxl_present_position = np.zeros(4) + self.dxl_present_velocity = np.zeros(4) + self.dxl_present_current = np.zeros(4) + self.q_desired = np.zeros(4) + self.dxl_goal_position = [ + [0, 0, 0, 0], + [0, 0, 0, 0], + [0, 0, 0, 0], + [0, 0, 0, 0], + ] + self.read_dxl() + for i in range(4): + self.dxl_goal_position[i] = [ + DXL_LOBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))), + DXL_HIBYTE(DXL_LOWORD(int(self.dxl_present_position[i]))), + DXL_LOBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))), + DXL_HIBYTE(DXL_HIWORD(int(self.dxl_present_position[i]))), + ] + + self.r = rospy.Rate(100) + try: + while not rospy.is_shutdown(): + self.read_dxl() + self.write_dxl() + self.r.sleep() + except KeyboardInterrupt: + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL1_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_DISABLE"], + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL2_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_DISABLE"], + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL3_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_DISABLE"], + ) + self.packetHandler.write1ByteTxRx( + self.portHandler, + self.cfg["DXL4_ID"], + self.cfg["ADDR_TORQUE_ENABLE"], + self.cfg["TORQUE_DISABLE"], + ) + + 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] + dxl_command = int( + rad2deg(self.q_desired[i]) / self.cfg["DXL_RESOLUTION"] + + self.cfg["DXL_POS_OFFSET"] + ) + if dxl_command > self.cfg["CW_LIMIT"]: + dxl_command = self.cfg["CW_LIMIT"] + elif dxl_command < self.cfg["CCW_LIMIT"]: + dxl_command = self.cfg["CCW_LIMIT"] + + self.dxl_goal_position[i] = [ + DXL_LOBYTE(DXL_LOWORD(dxl_command)), + DXL_HIBYTE(DXL_LOWORD(dxl_command)), + DXL_LOBYTE(DXL_HIWORD(dxl_command)), + DXL_HIBYTE(DXL_HIWORD(dxl_command)), + ] + 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( + self.cfg["DXL1_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.dxl_present_position[1] = self.groupBulkReadPosition.getData( + self.cfg["DXL2_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.dxl_present_position[2] = self.groupBulkReadPosition.getData( + self.cfg["DXL3_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + self.dxl_present_position[3] = self.groupBulkReadPosition.getData( + self.cfg["DXL4_ID"], + self.cfg["ADDR_PRESENT_POSITION"], + self.cfg["LEN_PRESENT_POSITION"], + ) + + self.groupBulkReadVelocity.txRxPacket() + self.dxl_present_velocity[0] = self.groupBulkReadVelocity.getData( + self.cfg["DXL1_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + self.dxl_present_velocity[1] = self.groupBulkReadVelocity.getData( + self.cfg["DXL2_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + self.dxl_present_velocity[2] = self.groupBulkReadVelocity.getData( + self.cfg["DXL3_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + self.dxl_present_velocity[3] = self.groupBulkReadVelocity.getData( + self.cfg["DXL4_ID"], + self.cfg["ADDR_PRESENT_VELOCITY"], + self.cfg["LEN_PRESENT_VELOCITY"], + ) + + self.groupBulkReadCurrent.txRxPacket() + self.dxl_present_current[0] = self.groupBulkReadVelocity.getData( + self.cfg["DXL1_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + self.dxl_present_current[1] = self.groupBulkReadVelocity.getData( + self.cfg["DXL2_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + self.dxl_present_current[2] = self.groupBulkReadVelocity.getData( + self.cfg["DXL3_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + self.dxl_present_current[3] = self.groupBulkReadVelocity.getData( + self.cfg["DXL4_ID"], + self.cfg["ADDR_PRESENT_CURRENT"], + self.cfg["LEN_PRESENT_CURRENT"], + ) + + for i in range(4): + if self.dxl_present_velocity[i] > 2 ** ( + 8 * self.cfg["ADDR_PRESENT_VELOCITY"] / 2 + ): + self.dxl_present_velocity[i] = self.dxl_present_velocity[i] - 2 ** ( + 8 * self.cfg["ADDR_PRESENT_VELOCITY"] + ) + if self.dxl_present_current[i] > 2 ** ( + 8 * self.cfg["LEN_PRESENT_CURRENT"] / 2 + ): + self.dxl_present_current[i] = self.dxl_present_current[i] - 2 ** ( + 8 * self.cfg["LEN_PRESENT_CURRENT"] + ) + + q_current = [ + 0.0, + 0.0, + deg2rad( + (self.dxl_present_position[0] - self.cfg["DXL_POS_OFFSET"]) + * self.cfg["DXL_RESOLUTION"] + ), + deg2rad( + (self.dxl_present_position[1] - self.cfg["DXL_POS_OFFSET"]) + * self.cfg["DXL_RESOLUTION"] + ), + deg2rad( + (self.dxl_present_position[2] - self.cfg["DXL_POS_OFFSET"]) + * self.cfg["DXL_RESOLUTION"] + ), + deg2rad( + (self.dxl_present_position[3] - self.cfg["DXL_POS_OFFSET"]) + * self.cfg["DXL_RESOLUTION"] + ), + ] + qdot_current = [ + 0.0, + 0.0, + rpm2rad(self.dxl_present_velocity[0] * self.cfg["DXL_VELOCITY_RESOLUTION"]), + rpm2rad(self.dxl_present_velocity[1] * self.cfg["DXL_VELOCITY_RESOLUTION"]), + rpm2rad(self.dxl_present_velocity[2] * self.cfg["DXL_VELOCITY_RESOLUTION"]), + rpm2rad(self.dxl_present_velocity[3] * self.cfg["DXL_VELOCITY_RESOLUTION"]), + ] + motor_current = [ + 0.0, + 0.0, + self.dxl_present_current[0] * self.cfg["DXL_TO_CURRENT"], + self.dxl_present_current[1] * self.cfg["DXL_TO_CURRENT"], + self.dxl_present_current[2] * self.cfg["DXL_TO_CURRENT"], + self.dxl_present_current[3] * self.cfg["DXL_TO_CURRENT"], + ] + + self.joint_states.position = q_current + self.joint_states.velocity = qdot_current + self.joint_states.effort = motor_current + + 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]) + self.groupSyncWrite.addParam(self.cfg["DXL4_ID"], self.dxl_goal_position[3]) + + self.groupSyncWrite.txPacket() + 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: + print("%s" % self.packetHandler.getRxPacketError(dxl_error)) + + +def main(): + rospy.init_node("dynamixel_read_write") + + try: + DynamixelPositionControl(cfg) + except rospy.ROSInterruptException: + pass + + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/scripts/envs/open_manipulator/ros_interface.py b/scripts/envs/open_manipulator/ros_interface.py index ddea996..f706242 100755 --- a/scripts/envs/open_manipulator/ros_interface.py +++ b/scripts/envs/open_manipulator/ros_interface.py @@ -12,7 +12,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 5fea141..2ae8980 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 new file mode 100755 index 0000000..7f45e4c --- /dev/null +++ b/scripts/run_open_manipulator_demo.py @@ -0,0 +1,27 @@ +#! /usr/bin/env python + +# -*- coding: utf-8 -*- +"""Collect demo data with jacobian based control method. + +- Author: DH Kim +- Contact: kdh0429@snu.ac.kr +""" + +import rospy +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 + + try: + collector = DemoCollector(cfg) + collector.run() + except rospy.ROSInterruptException: + pass + + +if __name__ == "__main__": + 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) 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