class DobotRealEnv(gym.Env, utils.EzPickle):
def __init__(self):
super().__init__()
# Find the port on which dobot is connected
available_ports = glob('/dev/tty*USB*')
if len(available_ports) == 0:
print('no port found for Dobot Magician')
exit(1)
def_port = available_ports[0]
self.dobot = DobotController(port=def_port)
self.observation_space = None
self.action_space = MultiDiscrete([3, 3, 3])
def compute_reward(self):
return 0
def step(self, action):
real_action = action - 1
self.dobot.moveangleinc(*real_action, r=0, q=1)
reward = self.compute_reward(image, centroid)
poses = self.dobot.get_dobot_joint()
done = False
info = None
return poses, reward, done, info
def main(port):
dobot = DobotController(port=def_port)
print_help()
# ipdb.set_trace()
while True:
try:
inp = input("Dobot>").split()
cmd = inp[0]
if cmd == 'q':
break
elif cmd == 'h':
print_help()
elif cmd == 'g':
e = int(inp[1])
t = float(inp[2])
dobot.grip(e, t)
elif cmd == 'm': ## add r later
x, y, z = [int(x) for x in inp[1:]]
dobot.moveangleinc(x, y, z)
elif cmd == 'j':
com, isJoint, q = [int(x) for x in inp[1:]]
print("com received")
dobot.jog(cmd=com, isJoint=isJoint, q=q)
else:
dobot.disconnect()
exit()
except Exception as e:
print("Invalid Command or Value. Exiting.")
print(e)
exit()
def __init__(self, camera_port_left=1):
super().__init__()
self.camera_obj = Vision(camera_port_left=camera_port_left) ## initialize camera
available_ports = glob('/dev/tty*USB*')
if len(available_ports) == 0:
print('no port found for Dobot Magician')
exit(1)
def_port = available_ports[0]
self.dobot = DobotController(port=def_port)
self.observation_space = None
self.action_space = MultiDiscrete([3, 3, 3])
def __init__(self):
super().__init__()
# Find the port on which dobot is connected
available_ports = glob('/dev/tty*USB*')
if len(available_ports) == 0:
print('no port found for Dobot Magician')
exit(1)
def_port = available_ports[0]
self.dobot = DobotController(port=def_port)
self.observation_space = None
self.action_space = MultiDiscrete([3, 3, 3])
def __init__(self):
super().__init__()
available_ports = glob('/dev/tty*USB*')
if len(available_ports) == 0:
print('no port found for Dobot Magician')
exit(1)
def_port = available_ports[0]
self.dobot = DobotController(port=def_port)
# poses =[pose.x, pose.y, pose.z, pose.rHead, pose.joint1Angle,
# pose.joint2Angle, pose.joint3Angle, pose.joint4Angle(gripper)]
# ignore rhead and joint4angle
self.poses_low = np.array([125, -165, -10, -50, -5, -5])
self.poses_high = np.array([330, 190, 160, 55, 70, 75])
self.observation_space = Box(low=self.poses_low, high=self.poses_high)
# -1 0 1 actions - Incremental change in angles
self.action_space = MultiDiscrete([3, 3, 3])
self.timestep = 0
self.max_timesteps = 400
class DobotCamRealEnv(gym.Env, utils.EzPickle):
def __init__(self, camera_port_left=1):
super().__init__()
self.camera_obj = Vision(camera_port_left=camera_port_left) ## initialize camera
available_ports = glob('/dev/tty*USB*')
if len(available_ports) == 0:
print('no port found for Dobot Magician')
exit(1)
def_port = available_ports[0]
self.dobot = DobotController(port=def_port)
self.observation_space = None
self.action_space = MultiDiscrete([3, 3, 3])
## initialize dobot
def compute_reward(self, image, centroid):
return 0
def step(self, action):
real_action = action - 1
self.dobot.moveangleinc(*real_action, r=0, q=1)
image, centroid, poses = self.get_observation()
self.image = image
reward = self.compute_reward(image, centroid)
done = False
if not centroid:
done = True
info = None
observation = {"image": self.image, "pose": poses}
return observation, reward, done, info
def reset(self):
# return to DEFINED_HOME
self.dobot.movexyz(*self.dobot.DEFINED_HOME, q=1)
image, centroid, poses = self.get_observation()
self.image = image
observation = {"image": self.image, "pose": poses}
return observation
def get_observation(self):
im, centroid = self.get_image(centroid=True)
poses = self.dobot.get_dobot_joint()
return im, centroid, poses
def get_image(self, centroid=False):
if centroid:
im, center = self.camera_obj.get_obs_cam(centroid=centroid)
return [im, center]
else:
ima = self.camera_obj.get_obs_cam(centroid=centroid)
return ima
def render(self):
# self.camera_obj.show_image(render_time=10)
cv2.imshow('leftcam', self.image)
def close(self):
self.dobot.disconnect()
self.camera_obj.cap2.release()
from dobot_gym.utils.dobot_controller import DobotController
dobot = DobotController()
print("cmd")
# dobot.continuousCMD(50,0,0)
print(dobot.get_dobot_joint())
dobot.moveangleinc(0, 50, 0)
print(dobot.get_dobot_joint())
dobot.disconnect()
class DobotStraightEnv(gym.Env, gym.utils.EzPickle):
def __init__(self):
super().__init__()
available_ports = glob('/dev/tty*USB*')
if len(available_ports) == 0:
print('no port found for Dobot Magician')
exit(1)
def_port = available_ports[0]
self.dobot = DobotController(port=def_port)
# poses =[pose.x, pose.y, pose.z, pose.rHead, pose.joint1Angle,
# pose.joint2Angle, pose.joint3Angle, pose.joint4Angle(gripper)]
# ignore rhead and joint4angle
self.poses_low = np.array([125, -165, -10, -50, -5, -5])
self.poses_high = np.array([330, 190, 160, 55, 70, 75])
self.observation_space = Box(low=self.poses_low, high=self.poses_high)
# -1 0 1 actions - Incremental change in angles
self.action_space = MultiDiscrete([3, 3, 3])
self.timestep = 0
self.max_timesteps = 400
def compute_reward(self, poses):
x, y, z = poses[:3]
reward = -10 * y - 10 * z + x
return reward
def step(self, action):
real_action = action - 1
self.dobot.moveangleinc(*real_action, r=0, q=1)
poses = self.dobot.get_dobot_joint()
# [pose.x, pose.y, pose.z, pose.rHead, pose.joint1Angle, pose.joint2Angle, pose.joint3Angle, pose.joint4Angle]
reward = self.compute_reward(poses)
# TODO done condition using dobot limits
done = False
info = None
if self.timestep > self.max_timesteps or not self.check_pose_limit(
poses):
done = True
self.timestep += 1
return poses, reward, done, info
def check_pose_limit(self, poses):
print("Poses ", poses, poses[4] < self.poses_low[3])
if poses[0] > self.poses_high[0] or poses[0] < self.poses_low[0] or poses[1] < self.poses_low[1] or poses[1] > \
self.poses_high[1] or poses[2] > self.poses_high[2] or poses[2] < self.poses_low[2] or poses[4] > \
self.poses_high[3] or poses[4] < self.poses_low[3] or poses[5] > self.poses_high[4] or poses[5] < \
self.poses_low[4] or poses[6] > self.poses_high[5] or poses[6] < self.poses_low[5]:
# limit break
return False
else:
return True
def reset(self):
self.timestep = 0
self.dobot.movexyz(*self.dobot.DEFINED_HOME, q=1)
poses = self.dobot.get_dobot_joint()
return poses