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")