import gym import sim as vrep import time import math import random import imageio import datetime import numpy as np from collections import deque import tensorflow as tf from tensorflow.keras.models import Model from tensorflow.keras.layers import Input, Dense, Concatenate from tensorflow.keras.optimizers import Adam import tensorflow_probability as tfp from matplotlib import pyplot as plt class Damo_Robot(): def __init__(self): self.joint_num = 7 self.joint_name = 'Franka_joint' self.force_sensor = 'Force_sensor' self.revolute_joint = 'Revolute_joint' self.target = 'target' self.cylinder = 'Shape' self.observation_space=1 self.action_space=2 # self.action_space.high=1 # self.action_space.low = -1 # self.path='path' # self.dummy='Dummy_Target' self.dummy = 'Path' # self.dummy = 'Franka_link8_resp' self.synchronous = False self.client_ip = '127.0.0.1' self.client_id = -1 self.joint_ranges = np.zeros((self.joint_num, 2), dtype=np.float32) self.joint_handle = np.zeros((self.joint_num,), np.int) self.joint_pos = np.zeros((self.joint_num,)) self.default_pose = np.array([0, 0, 0, -90, 0, 90, 0]) self.pi = math.pi / 180 def connection(self): # 关闭潜在的连接 vrep.simxFinish(-1) # 每隔0.2秒检测一次，直到连接上V-rep while True: self.client_id = vrep.simxStart(self.client_ip, 19999, True, True, 5000, 5) if self.client_id > -1: vrep.simxSetFloatSignal(self.client_id, "ConnectFlag", 19999, vrep.simx_opmode_oneshot) break else: time.sleep(0.2) print('Failed connecting to remote API server') print('Connection success!') def read_handle(self): for i in range(self.joint_num): _, self.joint_handle[i] = vrep.simxGetObjectHandle(self.client_id, self.joint_name + str(i + 1), vrep.simx_opmode_blocking) _, self.revolute_joint_handle = vrep.simxGetObjectHandle(self.client_id, self.revolute_joint, vrep.simx_opmode_blocking) _, self.force_sensor_handle = vrep.simxGetObjectHandle(self.client_id, self.force_sensor, vrep.simx_opmode_blocking) _, self.TargetHandle = vrep.simxGetObjectHandle(self.client_id, self.target, vrep.simx_opmode_blocking) _, self.DummyHandle = vrep.simxGetObjectHandle(self.client_id, self.dummy, vrep.simx_opmode_blocking) _, self.CylinderHandle = vrep.simxGetObjectHandle(self.client_id, self.cylinder, vrep.simx_opmode_blocking) print("handle available") def get_state(self): # targetPos1 = [30 * self.pi, 90 * self.pi, 0, 0, 0, 90 * self.pi, 0] # for i in range(7): # vrep.simxSetJointTargetPosition(self.client_id,self.joint_handle[i],targetPos1[i], # vrep.simx_opmode_oneshot) # # _, state, forcevector1, torquevector1 = vrep.simxReadForceSensor(self.client_id, self.force_sensor_handle, # vrep.simx_opmode_buffer) _, state, forcevector2, torquevector2 = vrep.simxReadForceSensor(self.client_id, self.force_sensor_handle, vrep.simx_opmode_blocking) # s=np.ravel(forcevector2) # for i in range(3): # s[i] = round(s[i],2 ) s = np.hstack([np.ravel(forcevector2), np.ravel(torquevector2)]) # print(s) return s[2] def reset(self): if vrep.simxGetConnectionId(self.client_id) == -1: self.connection() # targetPos1 = [30 * self.pi, 90 * self.pi, 0, 0, 0, 90 * self.pi, 0] # for i in range(7): # vrep.simxSetJointTargetPosition(self.client_id, self.joint_handle[i], targetPos1[i], # # vrep.simx_opmode_oneshot) # np.random.random_sample() # # hight=0.008 * np.random.random((1)) + 0.27299 # # vrep.simxSetObjectPosition(self.client_id,self.DummyHandle,-1,[-0.13312,-1.9997,hight] # ,vrep.simx_opmode_oneshot) # [-0.14531, -1.7991] # vrep.simxSetObjectPosition(self.client_id,self.DummyHandle,-1,[-0.13312,-1.9997,0.28099],vrep.simx_opmode_oneshot) # 圆头曲面ppo # vrep.simxSetObjectPosition(self.client_id, self.DummyHandle, -1, [0, 0, 0.24200], # vrep.simx_opmode_oneshot) # 圆头平面DDPG 随机 # np.random.random_sample() # # hight = 0.008 * np.random.random((1)) + 0.205 vrep.simxSetObjectPosition(self.client_id, self.DummyHandle, -1, [0, 0, 0.21] , vrep.simx_opmode_oneshot) # vrep.simxSetObjectOrientation(self.client_id, self.DummyHandle, self.DummyHandle, [0,0,0] # , vrep.simx_opmode_oneshot) _, state, forcevector2, torquevector2 = vrep.simxReadForceSensor(self.client_id, self.force_sensor_handle, vrep.simx_opmode_blocking) # s=np.ravel(forcevector2) s = np.hstack([np.ravel(forcevector2), np.ravel(torquevector2)]) return s[2] def conduct_action(self, a): self.action = a / 100.0 # a/10000 is circle black path if vrep.simxGetConnectionId(self.client_id) == -1: self.connection() # vrep.simxSetObjectOrientation(self.client_id, self.DummyHandle, self.DummyHandle, self.action[0:3] # , vrep.simx_opmode_oneshot) vrep.simxSetObjectPosition(self.client_id, self.DummyHandle, self.DummyHandle, [0, 0, self.action] , vrep.simx_opmode_oneshot) def get_reward(self, x): xt = np.array([1.0]) # xt = np.array([0.0, 0.0, 3.0]) r = -np.linalg.norm(np.array(x) - xt) # r = -abs(x[2] - xt[2]) # print("reward:", r) return r tfd = tfp.distributions tf.keras.backend.set_floatx('float64') # paper https://arxiv.org/pdf/1812.05905.pdf # code references https://github.com/StepNeverStop/RLs, https://github.com/rail-berkeley/softlearning def actor(state_shape, action_shape, units=(256, 128, 128)): state = Input(shape=state_shape) x = Dense(units[0], name="L0", activation="relu")(state) for index in range(1, len(units)): x = Dense(units[index], name="L{}".format(index), activation="relu")(x) actions_mean = Dense(action_shape, name="Out_mean")(x) actions_std = Dense(action_shape, name="Out_std")(x) model = Model(inputs=state, outputs=[actions_mean, actions_std]) return model def critic(state_shape, action_shape, units=(256, 128, 128)): inputs = [Input(shape=state_shape), Input(shape=action_shape)] concat = Concatenate(axis=-1)(inputs) x = Dense(units[0], name="Hidden0", activation="relu")(concat) for index in range(1, len(units)): x = Dense(units[index], name="Hidden{}".format(index), activation="relu")(x) output = Dense(1, name="Out_QVal")(x) model = Model(inputs=inputs, outputs=output) return model def update_target_weights(model, target_model, tau=0.005): weights = model.get_weights() target_weights = target_model.get_weights() for i in range(len(target_weights)): # set tau% of target model to be new weights target_weights[i] = weights[i] * tau + target_weights[i] * (1 - tau) target_model.set_weights(target_weights) class SAC: def __init__( self, env, lr_actor=1e-3, lr_critic=1e-4, actor_units=(256, 128), critic_units=(256, 128), auto_alpha=True, alpha=0.2, tau=0.125, #0.005 gamma=0.90, batch_size=64, #128 memory_cap=100000 ): self.env = env self.state_shape = env.observation_space # shape of observations self.action_shape = env.action_space # number of actions self.action_bound = 1 self.action_shift = 0 self.memory = deque(maxlen=int(memory_cap)) # Define and initialize actor network self.actor = actor(self.state_shape, self.action_shape, actor_units) self.actor_optimizer = Adam(learning_rate=lr_actor) self.log_std_min = -20 self.log_std_max = 2 print(self.actor.summary()) # Define and initialize critic networks self.critic_1 = critic(self.state_shape, self.action_shape, critic_units) self.critic_target_1 = critic(self.state_shape, self.action_shape, critic_units) self.critic_optimizer_1 = Adam(learning_rate=lr_critic) update_target_weights(self.critic_1, self.critic_target_1, tau=1.) self.critic_2 = critic(self.state_shape, self.action_shape, critic_units) self.critic_target_2 = critic(self.state_shape, self.action_shape, critic_units) self.critic_optimizer_2 = Adam(learning_rate=lr_critic) update_target_weights(self.critic_2, self.critic_target_2, tau=1.) print(self.critic_1.summary()) # Define and initialize temperature alpha and target entropy self.auto_alpha = auto_alpha if auto_alpha: self.target_entropy = -np.prod(self.action_shape) self.log_alpha = tf.Variable(0., dtype=tf.float64) self.alpha = tf.Variable(0., dtype=tf.float64) self.alpha.assign(tf.exp(self.log_alpha)) self.alpha_optimizer = Adam(learning_rate=lr_actor) else: self.alpha = tf.Variable(alpha, dtype=tf.float64) # Set hyperparameters self.gamma = gamma # discount factor self.tau = tau # target model update self.batch_size = batch_size # Tensorboard self.summaries = {} def process_actions(self, mean, log_std, test=False, eps=1e-6): std = tf.math.exp(log_std) raw_actions = mean if not test: raw_actions += tf.random.normal(shape=mean.shape, dtype=tf.float64) * std log_prob_u = tfd.Normal(loc=mean, scale=std).log_prob(raw_actions) actions = tf.math.tanh(raw_actions) log_prob = tf.reduce_sum(log_prob_u - tf.math.log(1 - actions ** 2 + eps)) actions = actions * self.action_bound + self.action_shift return actions, log_prob def act(self, state, test=False, use_random=False): state = np.expand_dims(state, axis=0).astype(np.float64) if use_random: a = tf.random.uniform(shape=(1, self.action_shape), minval=-1, maxval=1, dtype=tf.float64) else: means, log_stds = self.actor.predict(state) log_stds = tf.clip_by_value(log_stds, self.log_std_min, self.log_std_max) a, log_prob = self.process_actions(means, log_stds, test=test) q1 = self.critic_1.predict([state, a])[0][0] q2 = self.critic_2.predict([state, a])[0][0] self.summaries['q_min'] = tf.math.minimum(q1, q2) self.summaries['q_mean'] = np.mean([q1, q2]) return a def save_model(self, a_fn, c_fn): self.actor.save(a_fn) self.critic_1.save(c_fn) def load_actor(self, a_fn): self.actor.load_weights(a_fn) print(self.actor.summary()) def load_critic(self, c_fn): self.critic_1.load_weights(c_fn) self.critic_target_1.load_weights(c_fn) self.critic_2.load_weights(c_fn) self.critic_target_2.load_weights(c_fn) print(self.critic_1.summary()) def remember(self, state, action, reward, next_state, done): state = np.expand_dims(state, axis=0) next_state = np.expand_dims(next_state, axis=0) self.memory.append([state, action, reward, next_state, done]) def replay(self): if len(self.memory) < self.batch_size: return samples = random.sample(self.memory, self.batch_size) s = np.array(samples).T states, actions, rewards, next_states, dones = [np.vstack(s[i, :]).astype(np.float) for i in range(5)] with tf.GradientTape(persistent=True) as tape: # next state action log probs means, log_stds = self.actor(next_states) log_stds = tf.clip_by_value(log_stds, self.log_std_min, self.log_std_max) next_actions, log_probs = self.process_actions(means, log_stds) # critics loss current_q_1 = self.critic_1([states, actions]) current_q_2 = self.critic_2([states, actions]) next_q_1 = self.critic_target_1([next_states, next_actions]) next_q_2 = self.critic_target_2([next_states, next_actions]) next_q_min = tf.math.minimum(next_q_1, next_q_2) state_values = next_q_min - self.alpha * log_probs target_qs = tf.stop_gradient(rewards + state_values * self.gamma * (1. - dones)) critic_loss_1 = tf.reduce_mean(0.5 * tf.math.square(current_q_1 - target_qs)) critic_loss_2 = tf.reduce_mean(0.5 * tf.math.square(current_q_2 - target_qs)) # current state action log probs means, log_stds = self.actor(states) log_stds = tf.clip_by_value(log_stds, self.log_std_min, self.log_std_max) actions, log_probs = self.process_actions(means, log_stds) # actor loss current_q_1 = self.critic_1([states, actions]) current_q_2 = self.critic_2([states, actions]) current_q_min = tf.math.minimum(current_q_1, current_q_2) actor_loss = tf.reduce_mean(self.alpha * log_probs - current_q_min) # temperature loss if self.auto_alpha: alpha_loss = -tf.reduce_mean( (self.log_alpha * tf.stop_gradient(log_probs + self.target_entropy))) critic_grad = tape.gradient(critic_loss_1, self.critic_1.trainable_variables) # compute actor gradient self.critic_optimizer_1.apply_gradients(zip(critic_grad, self.critic_1.trainable_variables)) critic_grad = tape.gradient(critic_loss_2, self.critic_2.trainable_variables) # compute actor gradient self.critic_optimizer_2.apply_gradients(zip(critic_grad, self.critic_2.trainable_variables)) actor_grad = tape.gradient(actor_loss, self.actor.trainable_variables) # compute actor gradient self.actor_optimizer.apply_gradients(zip(actor_grad, self.actor.trainable_variables)) # tensorboard info self.summaries['q1_loss'] = critic_loss_1 self.summaries['q2_loss'] = critic_loss_2 self.summaries['actor_loss'] = actor_loss if self.auto_alpha: # optimize temperature alpha_grad = tape.gradient(alpha_loss, [self.log_alpha]) self.alpha_optimizer.apply_gradients(zip(alpha_grad, [self.log_alpha])) self.alpha.assign(tf.exp(self.log_alpha)) # tensorboard info self.summaries['alpha_loss'] = alpha_loss def train(self, max_epochs=100, random_epochs=25, max_steps=200, save_freq=10): current_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S") train_log_dir = 'logs/' + current_time summary_writer = tf.summary.create_file_writer(train_log_dir) Total_reward=[] done, use_random, episode, steps, epoch, episode_reward = False, True, 0, 0, 0, 0 cur_state = self.env.reset() while epoch < max_epochs: if steps > max_steps: done = True if done: episode += 1 print("episode {}: {} total reward, {} alpha, {} steps, {} epochs, {} para1 ,{} para2".format( episode, episode_reward, self.alpha.numpy(), steps, epoch, clip1, clip2)) with summary_writer.as_default(): tf.summary.scalar('Main/episode_reward', episode_reward, step=episode) tf.summary.scalar('Main/episode_steps', steps, step=episode) Total_reward.append(episode_reward) summary_writer.flush() done, cur_state, steps, episode_reward = False, self.env.reset(), 0, 0 if episode % save_freq == 0: self.save_model("hard/sac_actor_episode{}.h5".format(episode), "hard/sac_critic_episode{}.h5".format(episode)) if epoch > random_epochs and len(self.memory) > self.batch_size: use_random = False action = self.act(cur_state, use_random=use_random) # determine action action=np.array(action) # next_state, reward, done, _ = self.env.step(action[0]) # act on env clip1=np.clip(action[0][0],0,0.00002) clip2=np.clip(action[0][1],0,0.04) Fnk=1 kp=0.000002 kd=0.01 c1=np.random.choice([-1,1],size=1,p=[.5,.5]) c2 = np.random.choice([-1, 1], size=1, p=[.5, .5]) eft=cur_state-Fnk a=kp*eft+kd*eft+clip1*c1+clip2*c2 # a = kp * eft + kd * eft + action[0][0] * c1 + action[0][1] * c2 self.env.conduct_action(a) next_state=self.env.get_state() reward=self.env.get_reward(cur_state) # self.env.render(mode='rgb_array') print(cur_state, a,reward) self.remember(cur_state, action, reward, next_state, done) # add to memory self.replay() # train models through memory replay update_target_weights(self.critic_1, self.critic_target_1, tau=self.tau) # iterates target model update_target_weights(self.critic_2, self.critic_target_2, tau=self.tau) cur_state = next_state episode_reward += reward steps += 1 epoch += 1 # Tensorboard update with summary_writer.as_default(): if len(self.memory) > self.batch_size: tf.summary.scalar('Loss/actor_loss', self.summaries['actor_loss'], step=epoch) tf.summary.scalar('Loss/q1_loss', self.summaries['q1_loss'], step=epoch) tf.summary.scalar('Loss/q2_loss', self.summaries['q2_loss'], step=epoch) if self.auto_alpha: tf.summary.scalar('Loss/alpha_loss', self.summaries['alpha_loss'], step=epoch) tf.summary.scalar('Stats/alpha', self.alpha, step=epoch) if self.auto_alpha: tf.summary.scalar('Stats/log_alpha', self.log_alpha, step=epoch) tf.summary.scalar('Stats/q_min', self.summaries['q_min'], step=epoch) tf.summary.scalar('Stats/q_mean', self.summaries['q_mean'], step=epoch) tf.summary.scalar('Main/step_reward', reward, step=epoch) summary_writer.flush() self.save_model("hard/sac_actor_final_episode{}.h5".format(episode), "hard/sac_critic_final_episode{}.h5".format(episode)) steps = np.linspace(0., 100, 100) plt.plot(steps, Total_reward) plt.ylabel('Total reward') plt.xlabel('episode') # plt.savefig('G:\\training_picture\\Force.jpg') plt.legend() plt.grid() plt.show() def test(self, render=True, fps=30, filename='test_render.mp4'): cur_state, done, rewards = self.env.reset(), False, 0 video = imageio.get_writer(filename, fps=fps) while not done: action = self.act(cur_state, test=True) next_state, reward, done, _ = self.env.step(action[0]) cur_state = next_state rewards += reward if render: video.append_data(self.env.render(mode='rgb_array')) video.close() return rewards if __name__ == "__main__": gym_env = Damo_Robot() gym_env.connection() gym_env.read_handle() sac = SAC(gym_env) # sac.load_actor("save_model/sac_actor_final_episode99.h5") # sac.load_critic("save_model/sac_critic_final_episode99.h5") sac.train( max_epochs=20200, random_epochs=1000, max_steps=200, save_freq=2000) # reward = sac.test() # print(reward)