def joint_DMP(joint_number):
#Obtain w, c & D (in that order) from below function, and generate XML file
Tl_j1, Tl_j2, Tl_j3, Tl_j4, Tl_j5, Tl_g1, Tl_g2 = simple_trajectories_origina.demo_trajectory(dt, f_name)
T_all = [Tl_j1, Tl_j2, Tl_j3, Tl_j4, Tl_j5, Tl_g1, Tl_g2]
T = T_all[joint_number]
train_dmp(name, n_rfs, T, dt)
start = T[0][0]
goal = T[0][len(T[0])-1]
my_runner = DMP_runner(name,start,goal)
Y, Yd, Ydd = [], [], []
for i in np.arange(0, int(tau/dt)+1):
a, b, c = my_runner.step(tau,dt)
Y.append(my_runner.y)
# Y.append(a)
Yd.append(b)
Ydd.append(c)
return T, Y, Yd, Ydd
def ContinuousBNRegressor_prediction(self,
name,
ssbn,
X_test,
working_path,
show=True):
#############################
# Prediction
ts = time.time()
output = f'{working_path}/{name}_bn_output.json'
# output = r"../Output_BN/{}_bn_output.json".format(name)
dmp = DMP_runner(output)
return self.run_with_evidence_and_check_prediction(dmp, ssbn, X_test)
def ContinuousBNRegressor_prediction(self,
name,
ssbn,
X_test,
y_actual=None,
show=True):
#############################
# Prediction
ts = time.time()
output = r"../Output_BN/{}_bn_output.json".format(name)
dmp = DMP_runner(output)
y_predicted = self.run_with_evidence_and_check_prediction(
dmp, ssbn, X_test, y_actual)
# print("== BN was completed : Time {} ==============================".format(time.time()-ts))
if show == True and len(y_actual) > 1:
return y_predicted, self.show_results(y_predicted, y_actual)
else:
return y_predicted, self.show_results(y_predicted, y_actual=None)
def callback(data):
goal_data = data.data
point = goal_data[0, 0:3]
time = goal_data[0, 3]
joint_angles = IK_function(point)
position = np.append(position, joint_angles, axis=0)
if count == 0:
for x in range(0, 7):
start = X[x][0]
#goal = X[x][-1]
goal = position[count][x]
my_runner[x] = DMP_runner(filename[x], start, goal)
#Default.Initial_position()
tau = 1 #tau NOT YET CHANGED
for i in np.arange(0, int(tau / dt) + 1):
'''Dynamic change in goal'''
if i > 0.1 * int(tau / dt):
for i in range(7):
my_runner[i].setGoal(position[count][i], 1)
'''Dynamic change in goal'''
for i in range(len(my_runner)):
my_runner[i].step(tau, dt)
Y.append(my_runner[i].y)
Y_str = ','.join(str(e) for e in Y)
Command_Publisher.publish(Y_str)
Y = []
print('done')
count = count + 1
rospy.spin()
Linear = simple_trajectories.y_lin_trajectory(dt)
Exponential = simple_trajectories.y_exp_trajectory(dt)
Step = simple_trajectories.y_step_trajectory(dt)
Baxter_s0 = simple_trajectories.bax_trajectory(dt,x) [x = 9 to x = 15 for the 7 baxter joints]
"""
T = simple_trajectories.bax_trajectory(dt, 9)
#T = simple_trajectories.y_exp_trajectory(dt)
#Obtain w, c & D (in that order) from below function, and generate XML file
Important_values = train_dmp(name, n_rfs, T, dt)
start = 0.266
goal = 1.1539
#start = 0
#goal = 1
my_runner = DMP_runner(name, start, goal)
Y = []
tau = 1
for i in np.arange(0, int(tau / dt) + 1):
'''Dynamic change in goal'''
#new_goal = 2
#new_flag = 1
#if i > 0.6*int(tau/dt):
# my_runner.setGoal(new_goal,new_flag)
'''Dynamic change in goal'''
my_runner.step(tau, dt)
Y.append(my_runner.y)
time = np.arange(0, tau + dt, dt)
#start = 0
#goal = 1
Y = []
####BELOW IS GIVEN A NEW POSITION, YOU CAN TEST THE "GOAL" AS THIS POSITION FOR THE JOINTS TO TRY IT OUT.########
position = [
0.43707952457256116, 0.030886485048354118, -0.9100930412938544,
-0.049960844259059556, 0.11294876661665842, -0.7784898036329784,
0.026319462646594793
]
for x in range(0, 7):
start = X[x][0]
#goal = X[x][-1]
goal = position[x]
my_runner = DMP_runner(filename[x], start, goal)
tau = 1
a = []
for i in np.arange(0, int(tau / dt) + 1):
'''Dynamic change in goal'''
#if i > 0.1*int(tau/dt):
# my_runner.setGoal(position[x],1)
'''Dynamic change in goal'''
my_runner.step(tau, dt)
a.append(my_runner.y)
Y.append(a)
time = np.arange(0, tau + dt, dt)
#plt.title("2-D DMP demonstration")