import time
from pyrep import VRep
from math import *
import matplotlib.pyplot as plt
import numpy as np
# contextlib
# simpy
# multiprocessing cpu
with VRep.connect("127.0.0.1", 19997) as vrep:
# vrep.simulation.stop()
# time.sleep(2)
vrep.simulation.start()
j_vel = vrep.joint.with_velocity_control("joint_force")
j_pos = vrep.joint.with_position_control("joint_position")
j_pas = vrep.joint.passive("joint_passive")
j_sph = vrep.joint.spherical("sp_joint")
j_spr = vrep.joint.spring("joint_spring")
s = vrep.sensor.proximity("sensor")
v = vrep.sensor.vision("vision")
j_vel.set_target_velocity(2)
j_spr.set_target_position(2)
plt.figure("Raw image")
for i in range(5):
b = pi / 9
j_pos.set_target_position(b * i + 0.2)
import os
os.environ["VREP"] = "/home/anjd/github/V-REP_PRO_EDU_V3_5_0_Linux"
os.environ[
"VREP_LIBRARY"] = "/home/anjd/github/V-REP_PRO_EDU_V3_5_0_Linux/programming/remoteApiBindings/lib/lib/Linux/64Bit/"
from pyrep import VRep
from hardware.robot import Robot
from moo_system import MooSystem
from naive_system import NaiveSystem
from simple_system import SimpleSystem
if __name__ == '__main__':
# run
with VRep.connect("127.0.0.1", 19997) as api:
r = Robot(api)
# cs = NaiveSystem(r)
# cs = SimpleSystem(r)
cs = MooSystem(r)
cs.run()
# ------------------------------------
# ----------------------------------
# Initialize the neural network
# ----------------------------------
nn_param = [200, 200, 200] # 128, 128
params = {"batchSize": 100, "buffer": 50000, "nn": nn_param}
MainNetwork = NeuralNetwork(NUM_INPUT, nn_param)
TargetNetwork = MainNetwork
# ----------------------------------
# Remote API connection
# ----------------------------------
api = VRep.connect("127.0.0.1", 19997)
r = Robot(api)
# ----------------------------------
#
# ----------------------------------
x_rob_init, y_rob_init = -1.7000, -1.9250 # True
#x_rob_init, y_rob_init = -1.685, -1.884 # Alternative
x_goal_init = [1.625, -0.425, 1.125, -1.925, -1.925, -0.275]
y_goal_init = [1.7, 0.85, -1.75, -1.225, 1.7, -0.75]
dist_to_goal_back_init1 = math.sqrt((x_rob_init - x_goal_init[0])**2 +
(y_rob_init - y_goal_init[0])**2)
dist_to_goal_back_init2 = math.sqrt((x_rob_init - x_goal_init[1])**2 +
(y_rob_init - y_goal_init[1])**2)
dist_to_goal_back_init3 = math.sqrt((x_rob_init - x_goal_init[2])**2 +