def __init__(self, n_dmps=4, n_bfs=7, timesteps=25, max_params=None):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.timesteps = timesteps
self.max_params = max_params
self.bounds = [(-wmax, wmax) for wmax in self.max_params]
self.used = np.array([False]*self.n_dmps + [True]*self.n_bfs*self.n_dmps + [True]*self.n_dmps)
self.default = np.array([0.] * (self.n_bfs+2) * self.n_dmps)
self.motor = copy(self.default)
self.dmp = DMPs_discrete(dmps=self.n_dmps, bfs=self.n_bfs, dt=1./self.timesteps)
def __init__(self, opt):
self.opt = opt
self.start_pos = [0.4, -0.15, 0.34]
self.end_pos = [0.4, -0.15, 0.64]
self.n_bfs = 200
# self.frame_num = self.opt.cut_frame_num+1+8
self.frame_num = self.opt.dmp_num + 1
self.w_init = np.random.uniform(size=(3, self.n_bfs)) * 100.0
self.dmp = DMPs_discrete(dt=1. / self.frame_num,
n_dmps=3,
n_bfs=self.n_bfs,
w=self.w_init,
y0=self.start_pos,
goal=self.end_pos)
class DMP:
def __init__(self, opt):
self.opt = opt
self.start_pos = [0.4, -0.15, 0.34]
self.end_pos = [0.4, -0.15, 0.64]
self.n_bfs = 200
# self.frame_num = self.opt.cut_frame_num+1+8
self.frame_num = self.opt.dmp_num + 1
self.w_init = np.random.uniform(size=(3, self.n_bfs)) * 100.0
self.dmp = DMPs_discrete(dt=1. / self.frame_num,
n_dmps=3,
n_bfs=self.n_bfs,
w=self.w_init,
y0=self.start_pos,
goal=self.end_pos)
def set_start(self, start_pos):
self.start_pos = start_pos
for i in range(3):
self.dmp.y0[i] = self.start_pos[i]
def set_goal(self, end_pos):
self.end_pos = end_pos
for i in range(3):
self.dmp.goal[i] = self.end_pos[i]
def get_traj(self):
y_track, dy_track, ddy_track = self.dmp.rollout()
actions = []
for i in range(y_track.shape[0]):
if i == 0:
continue
actions.append([x - y for x, y in zip(y_track[i], y_track[i - 1])])
return np.array(actions)[:self.opt.cut_frame_num]
def imitate(self, trajectories):
for traj in trajectories:
traj = traj[:125, :]
self.set_start(traj[0])
self.set_goal(traj[-1])
self.dmp.imitate_path(
y_des=np.array([traj[:, 0], traj[:, 1], traj[:, 2]]))
traj_path = os.path.join(self.opt.project_root, 'scripts', 'Dmp',
'traj.npy')
np.save(traj_path, traj)
def dmp_generate_w_mean(test_demo,mean_start,mean_goal):
dim = test_demo.shape[1]
dmp = DMPs_discrete(n_dmps=dim, n_bfs=300, ay=np.ones(dim)*25.0,dt=0.001 )
dmp.imitate_path(y_des=test_demo.T, plot=False)
dmp.goal=mean_goal
dmp.y0=mean_start
y_track, dy_track, ddy_track = dmp.rollout()
return np.array(y_track)
def generate_new_demo(mean_demo,new_start_list, new_goal_list):
dim = mean_demo.shape[1]
y_track_list =[]
for idx in range(len(new_start_list)):
dmp = DMPs_discrete(n_dmps=dim, n_bfs=300, ay=np.ones(dim)*25.0,dt=0.005 )
dmp.imitate_path(y_des=mean_demo.T, plot=False)
dmp.goal=new_goal_list[idx]
dmp.y0=new_start_list[idx]
y_track, dy_track, ddy_track = dmp.rollout()
y_track_list.append(y_track)
return np.array(y_track_list)
class MyDMP(object):
def __init__(self, n_dmps=4, n_bfs=6, timesteps=25, use_init=False, max_params=None):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.timesteps = timesteps
self.max_params = max_params
self.bounds = [(-wmax, wmax) for wmax in self.max_params]
if use_init:
self.used = np.array([True]*self.n_dmps + [True]*self.n_bfs*self.n_dmps + [True]*self.n_dmps)
else:
self.used = np.array([False]*self.n_dmps + [True]*self.n_bfs*self.n_dmps + [True]*self.n_dmps)
self.default = np.array([0.] * (self.n_bfs+2) * self.n_dmps)
self.motor = copy(self.default)
self.dmp = DMPs_discrete(dmps=self.n_dmps, bfs=self.n_bfs, dt=1./self.timesteps)
def trajectory(self, m):
"""
Compute the motor trajectories from dmp parameters.
"""
self.motor[self.used] = np.array(m)
self.dmp.y0 = self.motor[:self.dmp.dmps]
self.dmp.goal = self.motor[-self.dmp.dmps:]
self.dmp.w = self.motor[self.dmp.dmps:-self.dmp.dmps].reshape(self.dmp.dmps, self.dmp.bfs)
return self.dmp.rollout(timesteps=self.timesteps)
def imitate(self, traj, maxfun=2500):
"""
Imitate a given trajectory with parameter optimization (less than 1 second).
"""
self.dmp.imitate_path(np.transpose(traj))
x0 = np.array(list(self.dmp.w.flatten()) + list(self.dmp.goal))
f = lambda w:np.linalg.norm(traj - self.trajectory(w))
return fmin_l_bfgs_b(f, x0, maxfun=maxfun, bounds=self.bounds, approx_grad=True)[0]
plt.rc('ytick', labelsize=BIGGER_SIZE) # fontsize of the tick labels
plt.rc('legend', fontsize=BIGGER_SIZE) # legend fontsize
plt.rc('figure', titlesize=BIGGER_SIZE) # fontsize of the figure title
# test imitation of path run
# plt.figure(1 ,facecolor='w', edgecolor='w' )
n_bfs = [10, 30, 50, 100, 10000]
# a straight line to target
path1 = np.cos(np.arange(0, 1, .01) * 2)
# a strange path to target
path2 = np.zeros(path1.shape)
path2[int(len(path2) / 2.):] = .5
for ii, bfs in enumerate(n_bfs):
dmp = DMPs_discrete(n_dmps=2, n_bfs=bfs)
dmp.imitate_path(y_des=np.array([path1, path2]))
# change the scale of the movement
dmp.goal[0] = 0
dmp.goal[1] = 1
y_track, dy_track, ddy_track = dmp.rollout()
plt.figure(1, facecolor='w', edgecolor='w')
plt.subplot(211)
plt.plot(y_track[:, 0], lw=2)
plt.subplot(212)
plt.plot(y_track[:, 1], lw=2)
a = plt.plot(path1, 'r--', lw=2)
def evaluation_and_stat(weights, conf):
'''
Assume time is fixed : t = 5s
sampling_rate = how many samples per second is sent to simulator
redundant_simulation_step = how many additional simulation steps are necessary for reaching stable system state.
'''
spillage_distance_threshold = 0.11
redundant_simulation_step = round(1.25/conf['dt']) # 1.25s
linear_mode = False
worst_cost_function_value = 180 * 1.2 + 2 * float(len(conf['particle_handles'])) * (80/20)# as collision is one time such cost
collision = False
add_noise = True
if linear_mode == True:
total_time = 1.
sampling_rate = 1./conf['dt']
def sample_points_linear(sampling_rate, current_time_step, weights):
"""
weights = [a1,...a7]
"""
current_joint_angles = np.zeros(len(weights))
for i in range(len(weights)):
if i == 0:
current_joint_angles[i] = weights[i] * current_time_step / sampling_rate + ms.pi/2
elif i == 1:
current_joint_angles[i] = weights[i] * current_time_step / sampling_rate - ms.pi/2
else:
current_joint_angles[i] = weights[i] * current_time_step / sampling_rate
return current_joint_angles
for t in range(round(total_time * sampling_rate)):
theta = sample_points_linear(sampling_rate, t, weights)
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], theta[i], vrep.simx_opmode_streaming) #Original: vrep.simx_opmode_blocking
vrep.simxSynchronousTrigger(conf['clientID'])
if returnCode != 0:
print('Can not set joint target positions!')
constraint_satisfied = True
else:# -----------------------------------------------------------------DMP mode-------------------------------------------------------------
joint_angle_traj, joint_vel_traj, joint_acc_traj, angle_to_vertical_axis, step_spillage = [] , [] , [] , [] , []
# end_effector_position, end_effector_euler_angles = [] , []
n_dmps = 1
rollout_timesteps = conf['dmp_trajectory_timesteps']# Originally, 5 seconds, now even different among different joint angles.
for i in range(len(conf['joint_handles'])):
##Allowing alpha and beta learning
if conf['enable_beta'] == True:
alpha_rescaled, beta_rescaled = alpha_beta_rescaling(weights[i][-2], weights[i][-1], conf)
else:
alpha_rescaled, beta_rescaled = alpha_beta_rescaling(weights[i][-1], weights[i][-2], conf)
# Goal position allowed
dmp = DMPs_discrete(dt=conf['dt_dmp'], n_dmps=n_dmps, n_bfs=conf['n_bfs'], w=np.expand_dims(weights[i][:-2], axis=0), goal=weights[i][-2]+conf['initial_joint_angles'][i]*180 /ms.pi, ay=alpha_rescaled*np.ones(n_dmps), by=beta_rescaled*np.ones(n_dmps), y0=conf['initial_joint_angles'][i]*180 /ms.pi) # weight as parameters, goal as known
# Goal position fixed
# dmp = DMPs_discrete(dt=conf['dt_dmp'], n_dmps=n_dmps, n_bfs=conf['n_bfs'], w=np.exp(np.expand_dims(weights[i][:-1], axis=0)), goal=conf['initial_joint_angles'][i]*180 /ms.pi, ay=alpha_rescaled*np.ones(n_dmps), by=beta_rescaled*np.ones(n_dmps), y0=conf['initial_joint_angles'][i]*180 /ms.pi) # weight as parameters, goal as known
# # rescale the runtime
# if conf['enable_beta'] == False:
# dmp.cs.run_time = runtime_rescaling(weights[i][-1], conf)
y_track, dy_track, ddy_track = dmp.rollout(timesteps = rollout_timesteps, tau = 1.0) # Makes sure that the trajectory lengths are the same. #dmp.rollout(timesteps = total_time_for_dyms)
# Post-process the y_track and dy_track
y_track = y_track[0::round(conf['dt']/conf['dt_dmp'])]
dy_track = np.mean(dy_track.reshape(-1, round(conf['dt']/conf['dt_dmp'])), axis=1)
ddy_track = np.mean(ddy_track.reshape(-1, round(conf['dt']/conf['dt_dmp'])), axis=1)
if add_noise == True:
y_track += 0.2 * np.random.randn(*y_track.shape)
# dy_track += 0.2 * np.random.randn(*dy_track.shape)
joint_angle_traj.append(y_track)
joint_vel_traj.append(dy_track)
joint_acc_traj.append(ddy_track)
# ----------------------------------------Check boundaries is fulfilled or not---------------------------------------------
constraint_satisfied, constraint_punishment = DMP_trajectory_constraint_check(conf, joint_angle_traj, joint_vel_traj, joint_acc_traj)
# -----------------------------------------------------------Start simulation------------------------------------------------------
if constraint_satisfied == True:
for t in range(len(y_track)):
vrep.simxPauseCommunication(conf['clientID'],True)
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], joint_angle_traj[i][t]/180.*ms.pi, vrep.simx_opmode_streaming) #Original: vrep.simx_opmode_blocking
if returnCode != 0 :
print('Can not set joint target positions!' + str(returnCode))
vrep.simxPauseCommunication(conf['clientID'],False)
vrep.simxSynchronousTrigger(conf['clientID'])
if t == 0:
vrep.simxGetObjectOrientation(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_streaming)
startTime=time.time()
while time.time()-startTime < 5:
returnCode, cup_eulerAngles = vrep.simxGetObjectOrientation(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer)
if returnCode==vrep.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
break
time.sleep(0.01)
else:
returnCode, cup_eulerAngles = vrep.simxGetObjectOrientation(conf['clientID'], conf['cup_handle'], -1, vrep.simx_opmode_buffer)
if returnCode != 0:
print('Can not get cup orientation!' + str(returnCode))
R = vrepEulerRotation(cup_eulerAngles)
cup_directional_vector = np.matmul(R, np.array([0,0,1])) # cup_directional_vector w.r.t. global reference frame
# Compute angle between 2 vectors
# https://stackoverflow.com/questions/2827393/angles-between-two-n-dimensional-vectors-in-python/13849249#13849249
c = np.dot(cup_directional_vector,np.array([0, 0, -1]))/np.linalg.norm(cup_directional_vector)
angle_to_vertical_axis.append(np.arccos(np.clip(c, -1, 1)) * 180 / ms.pi )
#-------------------------------------------------------Collision Check -------------------------------------
for i in range(len(conf['collision_handle'])):
if t == 0:
vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_streaming)
startTime=time.time()
while time.time()-startTime < 5:
returnCode,collision_temp=vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_buffer)
if returnCode==vrep.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
break
time.sleep(0.01)
else:
returnCode,collision_temp=vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_buffer)
# returnCode, collision_temp = vrep.simxReadCollision(conf['clientID'], conf['collision_handle'][i], vrep.simx_opmode_buffer)
collision = collision or collision_temp
if returnCode != vrep.simx_return_ok:
print('Can not read collsion state!' + str(returnCode))
step_spillage_cumulative, end_effector_pose, end_effector_euler_angle = step_check_routine(conf['clientID'], conf['cup_handle'], conf['end_effector_handle'], conf['particle_handles'], conf['distance_threshold'])
step_spillage.append(step_spillage_cumulative)
# if collision == True:
# break
if (constraint_satisfied == True): #and (min(angle_to_vertical_axis) < 90):
returnCode,Target_Position = vrep.simxGetObjectPosition(conf['clientID'],conf['cup_handle'],-1,vrep.simx_opmode_buffer)
if returnCode != 0:
print('Can not get Target position!')
if collision == False:
for j in range(redundant_simulation_step):
for i in range(len(conf['joint_handles'])):
returnCode = vrep.simxSetJointTargetPosition(conf['clientID'], conf['joint_handles'][i], joint_angle_traj[i][-1]/180.*ms.pi, vrep.simx_opmode_streaming) #Original: vrep.simx_opmode_blocking
vrep.simxSynchronousTrigger(conf['clientID'])
total_spilled_amount = episodic_spillage_detection(conf['clientID'], conf['cup_handle'], conf['particle_handles'], spillage_distance_threshold)
# e = np.linalg.norm((conf['p_d'] - Target_Position), ord=2) # no longer needed for turnover
e = total_spilled_amount * (80/20) + min(angle_to_vertical_axis) + 0.2 * (180 - angle_to_vertical_axis[-1])
if collision == True:
print('colliding')
e = float(collision) * (80 * 1.2 + float(len(conf['particle_handles'])) * 80 /20) + total_spilled_amount * (80/20)
else:
e = worst_cost_function_value + constraint_punishment
record_step_spillage = constraint_satisfied and (not collision) # If one of the criterion is true, then don't record step spillage
record_episodic_spillage = constraint_satisfied
if record_step_spillage == True:
# Process the cumulative information to increamental information
step_spillage_tmp = step_spillage.copy()
step_spillage.append(step_spillage_cumulative) #
step_spillage_tmp.insert(0, step_spillage[0])
step_spillage_tmp = np.array(step_spillage_tmp)
step_spillage = np.array(step_spillage)
step_spillage = step_spillage - step_spillage_tmp
else:
step_spillage = 25 * np.ones(rollout_timesteps)
if record_episodic_spillage == True:
episodic_spillage = total_spilled_amount
max_cup_rotation_angle = 180 - min(angle_to_vertical_axis)
final_cup_resting_angle = 180 - angle_to_vertical_axis[-1]
else:
episodic_spillage = 25
max_cup_rotation_angle = 200
final_cup_resting_angle = -25
return e, record_episodic_spillage, episodic_spillage, record_step_spillage, step_spillage, max_cup_rotation_angle, final_cup_resting_angle