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

Refactoring demo collector (#56)

Browse Source
This commit is contained in:
Donghyeon Kim
2019-08-17 14:29:58 +09:00
committed by Whi Kwon
parent 055d97661e
commit 18d6824191
19 changed files with 972 additions and 20 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
@@ -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"
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/__init__.py
View File
scripts/config/demo/open_manipulator/reacher_v0.py
Executable
+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": "../demo_collection.json",
}
def get():
return config
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/__init__.py
View File
scripts/demo/open_manipulator/__init__.py
View File
scripts/demo/open_manipulator/open_manipulator_demo_collector.py
Executable
+418
View File
@@ -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
scripts/dxl_read_write.py
Executable
+402
View File
@@ -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()
scripts/envs/open_manipulator/ros_interface.py
+4 -5
View File
@@ -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"]),
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
Executable
+27
View File
@@ -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()
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)
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