def find_expected_svf(self, discount, total_states, trajectories, reward):
# Creates object of robot markov model
robot_mdp = RobotMarkovModel()
# Returns the state and action values of the state space being created
state_values_from_trajectory, _ = robot_mdp.return_trajectories_data()
# Finds the terminal state value from the expert trajectory
# 0 indicates that the first trajectory data is being used
# total_states indicates that the last value of that trajectory data should be used as terminal state
terminal_state_val_from_trajectory = state_values_from_trajectory[0][
total_states - 1]
# Pass the gridsize required
# To create the state space model, we need to run the below commands
env_obj = RobotStateUtils(self.grid_size, discount,
terminal_state_val_from_trajectory)
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
# print "State space created is ", states
# V = {}
# for state in env_obj.states:
# V[state] = 0
# # Reinitialize policy
# policy = {}
# for state in env_obj.states:
# policy[state] = [key for key in env_obj.action_space]
# values_of_states, policy = env_obj.value_iteration(V, policy, 0.01)
# policy = np.random.randint(27, size=1331)
# print "policy is ", policy
# Finds the sum of features of the expert trajectory and list of all the features of the expert trajectory
expected_svf = env_obj.compute_state_visitation_frequency(trajectories)
# Returns the policy, state features of the trajectories considered in the state space and expected svf
return expected_svf
def max_ent_irl(self, trajectories, discount, n_trajectories, epochs,
learning_rate):
# Finds the total number of states and dimensions of the list of features array
# 0 indicates that the first trajectory data is being used
total_states = len(trajectories[0])
# total_states indicates that the last value of that trajectory data should be used as terminal state
terminal_state_val_from_trajectory = trajectories[0][total_states - 1]
print "Terminal state value is ", terminal_state_val_from_trajectory
# Creates the environment object created for the robot workspace
env_obj = RobotStateUtils(self.grid_size, discount,
terminal_state_val_from_trajectory)
# Creates the states and actions for environment
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
print "states is ", states
print "action are ", action
filename = "/home/vvarier/dvrk_automated_suturing/max_ent_trial4_parallel_compute_grid_size/trial4_grid11_parallel/weights_grid11.txt"
# Creates the feature matrix for the given robot environment
# Currently a identity feature matrix is being used
feat_map = np.eye(self.grid_size**3)
# feat_map = env_obj.get_feature_matrix()
# Initialize with random weights based on the dimensionality of the states
weights = np.random.uniform(size=(feat_map.shape[1], ))
# Find feature expectations, sum of features of trajectory/number of trajectories
feature_expectations = self.find_feature_expectations(
feat_map, trajectories)
# print "feature array is ", feature_array_all_trajectories[0][0:total_states]
# Gradient descent on alpha
for i in range(epochs):
if i % 20 == 0:
print "Epoch running is ", i
print "weights is ", weights
file_open = open(filename, 'a')
savetxt(file_open,
weights,
delimiter=',',
fmt="%10.5f",
newline=", ")
file_open.write("\n \n \n \n")
file_open.close()
reward = np.dot(feat_map, weights)
optimal_policy, expected_svf = self.find_expected_svf(
weights, discount, total_states, trajectories, reward)
# Computes the gradient
grad = feature_expectations - feat_map.T.dot(expected_svf)
# Change the learning rate based on the iteration running
if i < 50:
learning_rate = 0.1
else:
learning_rate = 0.1
# Gradient descent for finding new value of weights
weights += learning_rate * np.transpose(grad)
# print "weights is ", weight
# Compute the reward of the trajectory based on the weights value calculated
trajectory_reward = np.dot(feat_map, weights)
return trajectory_reward, weights
def find_expected_svf(self, weights, discount):
# Robot Object called
# Pass the gridsize required
env_obj = RobotStateUtils(11, weights)
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
# print "State space created is ", states
P_a = env_obj.get_transition_mat_deterministic()
# print "P_a is ", P_a
# print "shape of P_a ", P_a.shape
# Calculates the reward and feature for the trajectories being created in the state space
rewards = []
state_features = []
for i in range(len(states)):
r, f = env_obj.reward_func(states[i][0], states[i][1], states[i][2], weights)
rewards.append(r)
state_features.append(f)
# print "rewards is ", rewards
value, policy = env_obj.value_iteration(P_a, rewards, gamma=discount)
# policy = np.random.randint(27, size=1331)
print "policy is ", policy
robot_mdp = RobotMarkovModel()
# Finds the sum of features of the expert trajectory and list of all the features of the expert trajectory
expert_trajectory_states, _ = robot_mdp.return_trajectories_data()
expected_svf = env_obj.compute_state_visition_frequency(P_a, expert_trajectory_states, policy)
# Formats the features array in a way that it can be multiplied with the svf values
state_features = np.array([state_features]).transpose().reshape((len(state_features[0]), len(state_features)))
print "svf is ", expected_svf
print "svf shape is ", expected_svf.shape
# Returns the policy, state features of the trajectories considered in the state space and expected svf
return policy, state_features, expected_svf
def max_ent_irl(self, trajectories, discount, n_trajectories, epochs,
learning_rate):
# Finds the total number of states and dimensions of the list of features array
# 0 indicates that the first trajectory data is being used
total_states = len(trajectories[0])
# total_states indicates that the last value of that trajectory data should be used as terminal state
terminal_state_val_from_trajectory = trajectories[0][total_states - 1]
print "Terminal state value is ", terminal_state_val_from_trajectory
# Creates the environment object created for the robot workspace
env_obj = RobotStateUtils(self.grid_size, discount,
terminal_state_val_from_trajectory)
# Creates the states and actions for environment
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
print "states is ", states
print "action are ", action
# Creates the feature matrix for the given robot environment
# Currently a identity feature matrix is being used
feat_map = np.eye(self.grid_size**3)
# feat_map = env_obj.get_feature_matrix()
# Initialize with random weights based on the dimensionality of the states
weights = np.random.uniform(size=(feat_map.shape[1], ))
# Find feature expectations, sum of features of trajectory/number of trajectories
feature_expectations = self.find_feature_expectations(
feat_map, trajectories)
# print "feature array is ", feature_array_all_trajectories[0][0:total_states]
# Gradient descent on alpha
for i in range(epochs):
print "Epoch running is ", i
# Multiplies the features with randomized alpha value, size of output Ex: dot(449*449, 449x1)
reward = np.dot(feat_map, weights)
# Function which returns the policy being followed and the state visitation frequency
optimal_policy, expected_svf = self.find_expected_svf(
env_obj, states, action, reward, discount, trajectories,
learning_rate)
# Computes the gradient
grad = feature_expectations - feat_map.T.dot(expected_svf)
# Change the learning rate based on the iteration running
if i < 100:
learning_rate = 0.1
else:
learning_rate = 0.01
# Gradient descent for finding new value of weights
weights += learning_rate * np.transpose(grad)
print "weights is ", weights
# Compute the reward of the trajectory based on the weights value calculated
trajectory_reward = np.dot(feat_map, weights)
return trajectory_reward, weights
def find_expected_svf(self, weights, discount, total_states, trajectories,
reward):
# Creates object of robot markov model
robot_mdp = RobotMarkovModel()
# Returns the state and action values of the state space being created
state_values_from_trajectory, _ = robot_mdp.return_trajectories_data()
# Finds the terminal state value from the expert trajectory
# 0 indicates that the first trajectory data is being used
# total_states indicates that the last value of that trajectory data should be used as terminal state
terminal_state_val_from_trajectory = state_values_from_trajectory[0][
total_states - 1]
# Pass the gridsize required
# To create the state space model, we need to run the below commands
env_obj = RobotStateUtils(self.grid_size, discount,
terminal_state_val_from_trajectory)
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
# print "State space created is ", states
P_a = env_obj.get_transition_mat_deterministic()
# print "P_a is ", P_a
# Calculates the reward and feature for the trajectories being created in the state space
policy = env_obj.value_iteration(reward)
# policy = np.random.randint(27, size=1331)
# print "policy is ", policy
# Finds the sum of features of the expert trajectory and list of all the features of the expert trajectory
expected_svf = env_obj.compute_state_visitation_frequency(
trajectories, policy)
# Returns the policy, state features of the trajectories considered in the state space and expected svf
return policy, expected_svf
def find_expected_svf(self, grid_size, weights, discount, total_states):
# Creates object of robot markov model
robot_mdp = RobotMarkovModel()
# Returns the state and action values of the state space being created
state_values_from_trajectory, _ = robot_mdp.return_trajectories_data()
# Finds the terminal state value from the expert trajectory
# 0 indicates that the first trajectory data is being used
# total_states indicates that the last value of that trajectory data should be used as terminal state
terminal_state_val_from_trajectory = state_values_from_trajectory[0][
total_states - 1]
# Pass the gridsize required
# To create the state space model, we need to run the below commands
env_obj = RobotStateUtils(grid_size, weights,
terminal_state_val_from_trajectory)
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
# print "State space created is ", states
P_a = env_obj.get_transition_mat_deterministic()
# print "P_a is ", P_a
# Calculates the reward and feature for the trajectories being created in the state space
rewards = []
state_features = []
for i in range(len(states)):
r, f = env_obj.reward_func(states[i][0], states[i][1],
states[i][2], weights)
rewards.append(r)
state_features.append(f)
# print "rewards is ", rewards
value, policy = env_obj.value_iteration(P_a, rewards, gamma=discount)
# policy = np.random.randint(27, size=1331)
print "policy is ", policy
# Finds the sum of features of the expert trajectory and list of all the features of the expert trajectory
expert_trajectory_states, _ = robot_mdp.return_trajectories_data()
expected_svf = env_obj.compute_state_visitation_frequency(
P_a, expert_trajectory_states, policy)
# Formats the features array in a way that it can be multiplied with the svf values
state_features = np.array([state_features]).transpose().reshape(
(len(state_features[0]), len(state_features)))
print "svf is ", expected_svf
print "svf shape is ", expected_svf.shape
# Returns the policy, state features of the trajectories considered in the state space and expected svf
return policy, state_features, expected_svf
import numpy as np
from robot_state_utils import RobotStateUtils
from numpy import savetxt
from robot_markov_model import RobotMarkovModel
if __name__ == '__main__':
# Location to store the computed policy
file_name = "/home/vignesh/Desktop/individual_trials/version4/data1/policy_grid11.txt"
# term_state = np.random.randint(0, grid_size ** 3)]
goal = np.array([0.005, 0.055, -0.125])
# Create objects of classes
env_obj = RobotStateUtils(11, 0.01, goal)
mdp_obj = RobotMarkovModel()
# Store the expert trajectories
trajectories = mdp_obj.generate_trajectories()
index_vals = np.zeros(len(trajectories[0]))
for i in range(len(trajectories[0])):
index_vals[i] = env_obj.get_state_val_index(trajectories[0][i])
# print index_vals
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
rewards = np.zeros(len(states))
index = env_obj.get_state_val_index(goal)
for _, ele in enumerate(index_vals):
if ele == index:
rewards[int(ele)] = 10
else:
rewards[int(ele)] = 1
policy = env_obj.value_iteration(rewards)
file_open = open(file_name, 'a')
# policy = np.load(folder_dir + 'second_best_policy.npy')
# Third best policy learnt by Q learning
# policy = np.load(folder_dir + 'third_best_policy.npy')
# Miscellaneous good policies learnt by Q learning
# policy = np.load(folder_dir + 'misc_policy1.npy')
# policy = np.load(folder_dir + 'misc_policy2.npy')
# Define the starting 3D coordinate position of agent
starting_point = np.array([-0.025, 0.05, -0.135])
# Define the goal position (3D coordinate) of agent
goal_point = np.array([0.005, 0.055, -0.125])
# Initialize an array of visited states by agent
visited_states = starting_point
# Create the Object of class which creates the state space and action space
# Pass the required gridsize, discount, terminal_state_val_from_trajectory):
env_obj = RobotStateUtils(11, 0.1, goal_point)
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
# Get the starting and goal position index values in the grid world
start_index = env_obj.get_state_val_index(starting_point)
goal_index = env_obj.get_state_val_index(goal_point)
done = False
current_state_idx = start_index
print "policy is ", policy[current_state_idx]
# Repeat until the agent reaches the goal state
limit_val = 0
while not done:
next_state = []
# Find the action to take based on the policy learnt
action_idx = policy[current_state_idx]
print "action to be taken ", action[action_idx]
trajectories_z0 = [[0, 0, 0, 0] for i in range(1331)]
default_points = np.array(
[-0.03, -0.025, -0.02, -0.015, -0.01, -0.005, 0, 0.005, 0.010])
i = 0
# To create 3D points which can have different size
for z in default_points:
for y in default_points:
for x in default_points:
trajectories_z0[i] = [x, y, z, 0.]
i += 1
# Define the goal position (3D coordinate) of agent
goal = np.array([0.005, 0.055, -0.125])
# Create the Object of class which creates the state space and action space
env_obj = RobotStateUtils(11, 0.1, goal)
states = env_obj.create_state_space_model_func()
action = env_obj.create_action_set_func()
policy = np.zeros(len(states))
rewards = np.zeros(len(states))
index = env_obj.get_state_val_index(goal)
mdp_obj = RobotMarkovModel()
trajectories = mdp_obj.generate_trajectories()
visited_states_index_vals = np.zeros(len(trajectories[0]))
# Find the index values of the visited states
for i in range(len(trajectories[0])):
visited_states_index_vals[i] = env_obj.get_state_val_index(
trajectories[0][i])
for _, ele in enumerate(visited_states_index_vals):
if ele == index:
rewards[int(ele)] = 10