def baxter_dmp_publisher():
#Name of the file
filename = []
for i in range(0, 7):
A = '{}{}{}'.format('joint', i, '.xml')
filename.append(A)
#Set no. of basis functions
n_rfs = 200
#Set the time-step
dt = 0.001
##### TRAJECTORY FOR TRAINING ########
""" Available trajectories: ('ok')
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]
"""
X = []
for i in range(1, 8):
T = simple_trajectories.bax_trajectory(dt, i + 8)
X.append(T)
#Obtain w, c & D (in that order) from below function, and generate XML file
for i in range(0, 7):
Important_values = train_dmp(filename[i], n_rfs, X[i], 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.########
my_runner = [[]] * 7
Command_Publisher = rospy.Publisher('commands', String, queue_size=10)
rospy.init_node('commands', anonymous=True)
Data_Sub = rospy.Subscriber('/otp_dynamic', pos, callback, queue_size=10)
count = 0
position = []
name = 'test.xml'
#Set no. of basis functions
n_rfs = 200
#Set the time-step
dt = 0.001
##### TRAJECTORY FOR TRAINING ########
""" Available trajectories:
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'''