def main():
limb_name = "right"
rospy.init_node("trac_ik_dmp_node", anonymous=True)
# verify robot is enabled
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
init_state = rs.state().enabled
print("Enabling robot... ")
rs.enable()
limb = baxter_interface.Limb(limb_name)
current_joint_angles = limb.joint_angles()
joint_names = [
limb_name + "_s0", limb_name + "_s1", limb_name + "_e0",
limb_name + "_e1", limb_name + "_w0", limb_name + "_w1",
limb_name + "_w2"
]
current_joint_states = [
current_joint_angles[limb_name + "_s0"],
current_joint_angles[limb_name + "_s1"],
current_joint_angles[limb_name + "_e0"],
current_joint_angles[limb_name + "_e1"],
current_joint_angles[limb_name + "_w0"],
current_joint_angles[limb_name + "_w1"],
current_joint_angles[limb_name + "_w2"]
]
#### ik service
joint_client = rospy.ServiceProxy('get_joint_from_trac_ik', get_joint)
rospy.wait_for_service('get_joint_from_trac_ik')
rospy.loginfo("Geting joint service is ready ")
#####
# this file is recoded by baxter
# run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name
global data_path
train_set = pd.read_csv(data_path) #using pandas read data
train_len = len(train_set) # the lengh of data
resample_t = np.linspace(0, train_set.values[-1, 0],
train_len) # resampling the time
joint0_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 9])
joint1_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 10])
joint2_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 11])
joint3_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 12])
joint4_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 13])
joint5_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 14])
joint6_data = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 15])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
joint0_data[i], joint1_data[i], joint2_data[i], joint3_data[i],
joint4_data[i], joint5_data[i], joint6_data[i]
]
train_set = np.array([
np.array([
joint0_data, joint1_data, joint2_data, joint3_data, joint4_data,
joint5_data, joint6_data
]).T
])
param_w, base_function = train(train_set)
global flag
joint_req = get_jointRequest()
joint_req.flag = flag
joint_state = joint_client(joint_req)
rospy.loginfo("raw_jointstate %s" % joint_state)
ending_point_vision = [0, 0, 0, 0, 0, 0, 0]
for i in range(7):
ending_point_vision[i] = joint_state.joint_state.position[i]
rospy.loginfo("ending_point_vision %s" % ending_point_vision)
flag = False
start_point = current_joint_states
# start_point = traj[0]
ending_point_degree = np.array(traj[-1]) / np.pi * 180
ending_point_degree_ = np.array(ending_point_vision) / np.pi * 180
print("original joint states is")
print ending_point_degree
print "vision_based joint states"
print ending_point_degree_
ending_point = ending_point_vision
# ending_point_hard_code = [ 0.6250971710633061, -0.8429224429430349, 0.9058156552463368, 1.3211409535663126,-0.5840631849873713, 1.3326458094754532, 0.7911505913519021]
ending_point = traj[-1]
# y_track = dmp_imitate(starting_pose=start_point, ending_pose=ending_point, weight_mat=param_w, tau=1, n_dmps=7 )
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
for i in range(7):
dmp.y0[i] = start_point[i] #set the initial state
dmp.goal[i] = ending_point[i] # set the ending goal
ipdb.set_trace()
y_track, dy_track, ddy_track = dmp.rollout(tau=0.2) # ?
time_dmp = np.linspace(0.2, 6.3, 500) #?
lines = np.column_stack((time_dmp, y_track))
keys = [
'times', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
_cmd, rcmd_start, _raw = clean_line(lines[0], keys)
rate = rospy.Rate(1000)
print("moving to start position")
limb.move_to_joint_positions(rcmd_start)
i = 0
start_time = rospy.get_time()
for values in (lines[0:]):
i += 1
sys.stdout.write("\r Record %d of %d" % (i, len(lines) - 1))
sys.stdout.flush()
cmd, rcmd, values = clean_line(values, keys)
#command this set of commands until the next frame
while (rospy.get_time() - start_time) < values[0]:
if rospy.is_shutdown():
print("\n Aborting - ROS shutdown")
return False
if len(rcmd):
# pass
limb.set_joint_positions(rcmd)
rate.sleep()
# {'right_s0': 1.3483103233007299, 'right_s1': -1.1026646285700645,
# 'right_w0': 0.1503306903853853, 'right_w1': 1.5856406978589865, 'right_w2': 0.2999005810979858, 'right_e0': -0.34786733022148175, 'right_e1': 1.0831079510624184
#1.3475303474795948, -1.100049758731219, -0.3475166430296643, 1.0789709540159511, 0.15063928197300847, 1.5889827180212805, 0.30286672811813503
# y_track = []
# start_time = rospy.get_time()
# for t in range(dmp.timesteps):
# y, _, _ = dmp.step()
# y_track.append(np.copy(y))
# rcmd = make_command(y_track)
# while (rospy.get_time() - start_time) < time_dmp[t]:
# limb.set_joint_positions(rcmd)
# y_track = []
# dmp.goal = ending_point_vision
# print dmp.goal
####################################### For plotting
#creat fig
# fig=plt.figure()
# ax = Axes3D(fig)
# plt.xlabel('X')
# plt.ylabel('Y')
#
# #plot traj fig
# ax.plot(Colum0_Traj,Colum1_Traj,Colum2_Traj,linewidth=4,alpha=0.3)
# #Plot plan fig
# ax.plot(y_track[:,0],y_track[:,1],y_track[:,2],"--")
# #Plot plan fig
# #show the plot
# plt.draw()
# plt.show()
plt.figure(1)
plt.subplot(711)
plt.plot(resample_t,
joint0_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 0], label='DMP imitation', lw=1)
plt.xlabel("time")
plt.ylabel("right_s0")
plt.grid(True)
plt.legend(loc='upper right')
plt.subplot(712)
plt.plot(resample_t,
joint1_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 1], label='DMP imitation', lw=1)
plt.ylabel("right_s1")
plt.grid(True)
plt.subplot(713)
plt.plot(resample_t,
joint2_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 2], label='DMP imitation', lw=1)
plt.ylabel("right_e0")
plt.grid(True)
plt.subplot(714)
plt.plot(resample_t,
joint3_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 3], label='DMP imitation', lw=1)
plt.ylabel("right_e1")
plt.grid(True)
plt.subplot(715)
plt.plot(resample_t,
joint4_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 4], label='DMP imitation', lw=1)
plt.ylabel("right_w0")
plt.grid(True)
plt.subplot(716)
plt.plot(resample_t,
joint5_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 5], label='DMP imitation', lw=1)
plt.ylabel("right_w1")
plt.grid(True)
plt.subplot(717)
plt.plot(resample_t,
joint6_data,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 6], label='DMP imitation', lw=1)
plt.ylabel("right_w2")
plt.grid(True)
plt.show()
####################################### For plotting
####################################### saving data to a file
#
#
data_path = os.path.join(dir_path, "./plot/baxter_dmp_runing.txt")
WriteFileDir = data_path ## the path of generated dmp traj
plan_len = len(y_track[:, 0])
f = open(WriteFileDir, 'w')
f.write('time,')
f.write('right_s0,')
f.write('right_s1,')
f.write('right_e0,')
f.write('right_e1,')
f.write('right_w0,')
f.write('right_w1,')
f.write('right_w2\n')
for i in range(plan_len):
f.write("%f," % (time_dmp[i], ))
f.write(
str(y_track[:, 0][i]) + ',' + str(y_track[:, 1][i]) + ',' +
str(y_track[:, 2][i]) + ',' + str(y_track[:, 3][i]) + ',' +
str(y_track[:, 4][i]) + ',' + str(y_track[:, 5][i]) + ',' +
str(y_track[:, 6][i]) + '\n')
f.close()
def main():
rospy.init_node("trac_ik_dmp_cart_node", anonymous=True)
# verify robot is enabled
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
init_state = rs.state().enabled
print("Enabling robot... ")
rs.enable()
# this file is recoded by baxter
# run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name
global data_path, train_len, tau, limb_name
limb = baxter_interface.Limb(limb_name)
train_set = pd.read_csv(data_path) #using pandas read data
resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0],
train_len) # resampling the time
postion_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 1])
postion_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 2])
postion_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 3])
orientation_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 4])
orientation_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 5])
orientation_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 6])
orientation_w = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 7])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
postion_x[i], postion_y[i], postion_z[i], orientation_x[i],
orientation_y[i], orientation_z[i], orientation_w[i]
]
train_set_T = np.array([
np.array([
postion_x, postion_y, postion_z, orientation_x, orientation_y,
orientation_z, orientation_w
]).T
])
param_w, base_function = train(train_set_T)
object_vision = get_base_camera_pose(flag=True)
end_pose = limb.endpoint_pose()
# get current angles
endpoint_pose = [
end_pose['position'].x, end_pose['position'].y, end_pose['position'].z,
end_pose['orientation'].x, end_pose['orientation'].y,
end_pose['orientation'].z, end_pose['orientation'].w
]
# ipdb.set_trace()
start_point = traj[0]
# start_point = endpoint_pose
# print start_point
# ending_point = object_vision
ending_point = traj[-1]
# print ending_point
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
for i in range(7):
dmp.y0[i] = start_point[i] #set the initial state
dmp.goal[i] = ending_point[i] # set the ending goal
y_track, dy_track, ddy_track = dmp.rollout(tau=tau) # ?
# diff = []
# abs_len = np.sqrt((np.square(y_track[-1,0] -y_track[0,0])+np.square(y_track[-1,1] -y_track[0,1])+np.square(y_track[-1,2] -y_track[0,2])))
# for i in range(1,train_len):
# dif = np.sqrt((np.square(y_track[i,0] -y_track[i-1,0])+np.square(y_track[i,1] -y_track[i-1,1])+np.square(y_track[i,2] -y_track[i-1,2])))
# diff.append(dif)
# diff_np = np.array(diff)
# diff_sum = diff_np.sum()
# ipdb.set_trace()
# num_point = diff_sum/0.001
# ipdb.set_trace()
# ipdb.set_trace()
time_dmp = np.linspace(0, train_set.values[-1, 0], train_len)
# ipdb.set_trace()
cmd = trac_ik_request(pose_sets=y_track, limb_name=limb_name)
print("moving to start position")
limb.move_to_joint_positions(cmd[0])
print("Done")
rate = rospy.Rate(100)
for idx in range(train_len):
if rospy.is_shutdown():
print("\n Aborting - ROS shutdown")
return False
# limb.set_joint_positions(cmd[idx])
rate.sleep()
# start_time = rospy.get_time()
# for idx in range(train_len):
# while (rospy.get_time() - start_time) < time_dmp[idx]:
# limb.set_joint_positions(cmd[idx])
# if rospy.is_shutdown():
# print("\n Aborting - ROS shutdown")
# return False
# rate.sleep()
# lines = np.column_stack((time_dmp,y_track))
# keys = ['times','right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
# keys = ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
# rate = rospy.Rate(1000)
# pose = Pose()
# for idx in range(1):
# pose.position.x = y_track[idx,0]
# pose.position.y = y_track[idx,1]
# pose.position.z = y_track[idx,2]
# pose.orientation.x = y_track[idx,3]
# pose.orientation.y = y_track[idx,4]
# pose.orientation.z = y_track[idx,5]
# pose.orientation.w = y_track[idx,6]
# lcmd_start= trac_ik_request(pose=pose)
#
#
#
#
# start_time = rospy.get_time()
## ipdb.set_trace()
# for idx in range(1,len(time_dmp)):
# pose.position.x = y_track[idx,0]
# pose.position.y = y_track[idx,1]
# pose.position.z = y_track[idx,2]
# pose.orientation.x = y_track[idx,3]
# pose.orientation.y = y_track[idx,4]
# pose.orientation.z = y_track[idx,5]
# pose.orientation.w = y_track[idx,6]
# lcmd = trac_ik_request(pose=pose)
# #command this set of commands until the next frame
# print ((rospy.get_time() - start_time) < time_dmp[idx])
# while (rospy.get_time() - start_time) < time_dmp[idx]:
# limb.set_joint_positions(cmd[i])
# if rospy.is_shutdown():
# print("\n Aborting - ROS shutdown")
# return False
# rate.sleep()
#
# {'right_s0': 1.3483103233007299, 'right_s1': -1.1026646285700645,
# 'right_w0': 0.1503306903853853, 'right_w1': 1.5856406978589865, 'right_w2': 0.2999005810979858, 'right_e0': -0.34786733022148175, 'right_e1': 1.0831079510624184
#1.3475303474795948, -1.100049758731219, -0.3475166430296643, 1.0789709540159511, 0.15063928197300847, 1.5889827180212805, 0.30286672811813503
# y_track = []
# start_time = rospy.get_time()
# for t in range(dmp.timesteps):
# y, _, _ = dmp.step()
# y_track.append(np.copy(y))
# rcmd = make_command(y_track)
# while (rospy.get_time() - start_time) < time_dmp[t]:
# limb.set_joint_positions(rcmd)
# y_track = []
# dmp.goal = ending_point_vision
# print dmp.goal
####################################### For plotting
#creat fig
fig = plt.figure()
ax = Axes3D(fig)
plt.xlabel('X')
plt.ylabel('Y')
#plot traj fig
ax.plot(postion_x, postion_y, postion_z, linewidth=4, alpha=0.3)
#Plot plan fig
ax.plot(y_track[:, 0], y_track[:, 1], y_track[:, 2], "--")
#Plot plan fig
#show the plot
plt.draw()
plt.show()
def main():
rospy.init_node("dmp_ability_test", anonymous=True)
# this file is recoded by baxter
# run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name
global data_path, train_len, tau, limb_name
limb = baxter_interface.Limb(limb_name)
train_set = pd.read_csv(data_path) #using pandas read data
resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0],
train_len) # resampling the time
postion_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 1])
postion_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 2])
postion_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 3])
orientation_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 4])
orientation_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 5])
orientation_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 6])
orientation_w = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 7])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
postion_x[i], postion_y[i], postion_z[i], orientation_x[i],
orientation_y[i], orientation_z[i], orientation_w[i]
]
train_set_T = np.array([
np.array([
postion_x, postion_y, postion_z, orientation_x, orientation_y,
orientation_z, orientation_w
]).T
])
param_w, base_function = train(train_set_T)
# object_vision = get_base_camera_pose(flag = True)
end_pose = limb.endpoint_pose()
# get current angles
endpoint_pose = [
end_pose['position'].x, end_pose['position'].y, end_pose['position'].z,
end_pose['orientation'].x, end_pose['orientation'].y,
end_pose['orientation'].z, end_pose['orientation'].w
]
# ipdb.set_trace()
start_point = traj[0]
# start_point = endpoint_pose
# print start_point
# ending_point = object_vision
ending_point = traj[-1]
# print ending_point
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
for i in range(7):
dmp.y0[i] = start_point[i] #set the initial state
dmp.goal[i] = ending_point[i] # set the ending goal
y_track, dy_track, ddy_track = dmp.rollout(tau=tau) # ?
time_dmp = np.linspace(0, train_set.values[-1, 0], train_len)
dmp_data_w_time = np.column_stack((time_dmp, y_track))
# ipdb.set_trace()
####################################### For plotting
#creat fig
fig = plt.figure(1)
ax = Axes3D(fig)
plt.xlabel('X')
plt.ylabel('Y')
#plot traj fig
ax.plot(postion_x, postion_y, postion_z, linewidth=2, alpha=0.3)
#Plot plan fig
ax.plot(y_track[:, 0],
y_track[:, 1],
y_track[:, 2],
linewidth=2,
alpha=0.3)
#Plot plan fig
#show the plot
plt.draw()
plt.show() # uncomment to plot
return dmp_data_w_time, limb_name
def main():
limb_name = "left"
rospy.init_node("trac_ik_dmp_node", anonymous=True)
limb = baxter_interface.Limb(limb_name)
# verify robot is enabled
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
init_state = rs.state().enabled
print("Enabling robot... ")
rs.enable()
# this file is recoded by baxter
# run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name
global data_path
train_set = pd.read_csv(data_path) #using pandas read data
train_len = len(train_set) # scaling factor
resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0],
train_len) # resampling the time
postion_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 1])
postion_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 2])
postion_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 3])
orientation_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 4])
orientation_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 5])
orientation_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 6])
orientation_w = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 7])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
postion_x[i], postion_y[i], postion_z[i], orientation_x[i],
orientation_y[i], orientation_z[i], orientation_w[i]
]
train_set_T = np.array([
np.array([
postion_x, postion_y, postion_z, orientation_x, orientation_y,
orientation_z, orientation_w
]).T
])
param_w, base_function = train(train_set_T)
object_vision = get_base_camera_pose(flag=True)
current_joint_angles = limb.joint_angles()
# get current angles
joint_names = [
limb_name + "_s0", limb_name + "_s1", limb_name + "_e0",
limb_name + "_e1", limb_name + "_w0", limb_name + "_w1",
limb_name + "_w2"
]
current_joint_states = [
current_joint_angles[limb_name + "_s0"],
current_joint_angles[limb_name + "_s1"],
current_joint_angles[limb_name + "_e0"],
current_joint_angles[limb_name + "_e1"],
current_joint_angles[limb_name + "_w0"],
current_joint_angles[limb_name + "_w1"],
current_joint_angles[limb_name + "_w2"]
]
# ipdb.set_trace()
start_point = traj[0]
# start_point = current_joint_states
print start_point
# ending_point = traj[-1]
ending_point = object_vision
print ending_point
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
for i in range(7):
dmp.y0[i] = start_point[i] #set the initial state
dmp.goal[i] = ending_point[i] # set the ending goal
y_track, dy_track, ddy_track = dmp.rollout(tau=1) # ?
# ipdb.set_trace()
time_dmp = np.linspace(0, train_set.values[-1, 0], 100) #?
# lines = np.column_stack((time_dmp,y_track))
# keys = ['times','right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
keys = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
rate = rospy.Rate(1000)
pose = Pose()
for idx in range(1):
pose.position.x = y_track[idx, 0]
pose.position.y = y_track[idx, 1]
pose.position.z = y_track[idx, 2]
pose.orientation.x = y_track[idx, 3]
pose.orientation.y = y_track[idx, 4]
pose.orientation.z = y_track[idx, 5]
pose.orientation.w = y_track[idx, 6]
lcmd_start = trac_ik_request(pose=pose)
print("moving to start position")
limb.move_to_joint_positions(lcmd_start)
print("Done")
start_time = rospy.get_time()
# ipdb.set_trace()
for idx in range(1, len(time_dmp)):
pose.position.x = y_track[idx, 0]
pose.position.y = y_track[idx, 1]
pose.position.z = y_track[idx, 2]
pose.orientation.x = y_track[idx, 3]
pose.orientation.y = y_track[idx, 4]
pose.orientation.z = y_track[idx, 5]
pose.orientation.w = y_track[idx, 6]
lcmd = trac_ik_request(pose=pose)
#command this set of commands until the next frame
print((rospy.get_time() - start_time) < time_dmp[idx])
while (rospy.get_time() - start_time) < time_dmp[idx]:
limb.set_joint_positions(lcmd)
if rospy.is_shutdown():
print("\n Aborting - ROS shutdown")
return False
rate.sleep()
# {'right_s0': 1.3483103233007299, 'right_s1': -1.1026646285700645,
# 'right_w0': 0.1503306903853853, 'right_w1': 1.5856406978589865, 'right_w2': 0.2999005810979858, 'right_e0': -0.34786733022148175, 'right_e1': 1.0831079510624184
#1.3475303474795948, -1.100049758731219, -0.3475166430296643, 1.0789709540159511, 0.15063928197300847, 1.5889827180212805, 0.30286672811813503
# y_track = []
# start_time = rospy.get_time()
# for t in range(dmp.timesteps):
# y, _, _ = dmp.step()
# y_track.append(np.copy(y))
# rcmd = make_command(y_track)
# while (rospy.get_time() - start_time) < time_dmp[t]:
# limb.set_joint_positions(rcmd)
# y_track = []
# dmp.goal = ending_point_vision
# print dmp.goal
####################################### For plotting
#creat fig
# fig=plt.figure()
# ax = Axes3D(fig)
# plt.xlabel('X')
# plt.ylabel('Y')
#
# #plot traj fig
# ax.plot(Colum0_Traj,Colum1_Traj,Colum2_Traj,linewidth=4,alpha=0.3)
# #Plot plan fig
# ax.plot(y_track[:,0],y_track[:,1],y_track[:,2],"--")
# #Plot plan fig
# #show the plot
# plt.draw()
# plt.show()
plt.figure(1)
plt.subplot(711)
plt.plot(resample_t,
postion_x,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 0], label='DMP imitation', lw=1)
plt.xlabel("time")
plt.ylabel("right_s0")
plt.grid(True)
plt.legend(loc='upper right')
plt.subplot(712)
plt.plot(resample_t,
postion_y,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 1], label='DMP imitation', lw=1)
plt.ylabel("right_s1")
plt.grid(True)
plt.subplot(713)
plt.plot(resample_t,
postion_z,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 2], label='DMP imitation', lw=1)
plt.ylabel("right_e0")
plt.grid(True)
plt.subplot(714)
plt.plot(resample_t,
orientation_x,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 3], label='DMP imitation', lw=1)
plt.ylabel("right_e1")
plt.grid(True)
plt.subplot(715)
plt.plot(resample_t,
orientation_y,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 4], label='DMP imitation', lw=1)
plt.ylabel("right_w0")
plt.grid(True)
plt.subplot(716)
plt.plot(resample_t,
orientation_z,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 5], label='DMP imitation', lw=1)
plt.ylabel("right_w1")
plt.grid(True)
plt.subplot(717)
plt.plot(resample_t,
orientation_w,
linewidth=1,
linestyle="-",
label="demestration")
plt.plot(time_dmp, y_track[:, 6], label='DMP imitation', lw=1)
plt.ylabel("right_w2")
plt.grid(True)
plt.show()
traj[i] = [Colum0_Traj[i], Colum1_Traj[i], Colum2_Traj[i]]
y_des = np.array([Colum0_Traj, Colum1_Traj, Colum2_Traj]) #
y_des = y_des.T
'''
If you wanna put in a set data, do it like this y_des = np.array([[Colum0_Traj,Colum1_Traj,Colum2_Traj]])
If you wanna put in many sets data, do it like this y_des = np.array([[Colum0_Traj,Colum1_Traj,Colum2_Traj],[Colum0_Traj,Colum1_Traj,Colum2_Traj]])
'''
######
#dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=3, n_bfs=500, ay=np.ones(3)*10.0)
#dmp.imitate_path(y_des=y_des, plot=True)
#y_track, dy_track, ddy_track = dmp.rollout()
######
train_set = [y_des]
param, base_function = train(train_set)
start_point = traj[0]
ending_point = traj[-1]
y_track = dmp_imitate(starting_pose=start_point,
ending_pose=ending_point,
weight_mat=param)
#creat fig
fig = plt.figure()
ax = Axes3D(fig)
plt.xlabel('X')
plt.ylabel('Y')
#plot traj fig
def main():
global data_path, train_len, tau, limb_name
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('grab_object_moveit')
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("left_arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
marker_pub = rospy.Publisher("/visualization_marker",
Marker,
queue_size=100)
############## get data from DMP
# limb = baxter_interface.Limb(limb_name)
train_set = pd.read_csv(data_path) # using pandas read data
resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0],
train_len) # resampling the time
postion_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 1])
postion_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 2])
postion_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 3])
orientation_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 4])
orientation_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 5])
orientation_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 6])
orientation_w = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 7])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
postion_x[i], postion_y[i], postion_z[i], orientation_x[i],
orientation_y[i], orientation_z[i], orientation_w[i]
]
train_set_T = np.array([
np.array([
postion_x, postion_y, postion_z, orientation_x, orientation_y,
orientation_z, orientation_w
]).T
])
param_w, base_function = train(train_set_T)
# object_vision = get_base_camera_pose(flag=True)
# end_pose = limb.endpoint_pose()
# get current angles
# endpoint_pose = [end_pose['position'].x,
# end_pose['position'].y,
# end_pose['position'].z,
# end_pose['orientation'].x,
# end_pose['orientation'].y,
# end_pose['orientation'].z,
# end_pose['orientation'].w]
start_point = traj[0]
# start_point = endpoint_pose
# print start_point
# ending_point = object_vision
ending_point = traj[-1]
# print ending_point
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
for i in range(7):
dmp.y0[i] = start_point[i] # set the initial state
dmp.goal[i] = ending_point[i] # set the ending goal
y_track, dy_track, ddy_track = dmp.rollout(tau=tau)
############## get data from DMP
# rospy.loginfo("move_to_start_pose... ")
# move_to_start_pose(start_pose = y_track[0],limb_name = limb_name)
# rospy.loginfo("Done... ")
# step_length = 0.01
list_of_poses = []
# list_of_poses.append(group.get_current_pose().pose)
for idx in range(train_len):
traj_pose = Pose()
traj_pose.position.x = y_track[idx, 0]
traj_pose.position.y = y_track[idx, 1]
traj_pose.position.z = y_track[idx, 2]
traj_pose.orientation.x = y_track[idx, 3]
traj_pose.orientation.y = y_track[idx, 4]
traj_pose.orientation.z = y_track[idx, 5]
traj_pose.orientation.w = y_track[idx, 6]
# visualize the pose in rviz using marker
alpha = float(idx) / train_len * 0.5 + 0.5
rgba_tuple = ((0.5 * idx), 0.5, (0.5 * idx), alpha)
util.send_traj_point_marker(marker_pub=marker_pub,
pose=traj_pose,
id=idx,
rgba_tuple=rgba_tuple)
rospy.loginfo("add one pose")
list_of_poses.append(copy.deepcopy(traj_pose))
# ipdb.set_trace()
rospy.loginfo("============ Generating plan")
group.set_max_acceleration_scaling_factor(0.01)
group.set_max_velocity_scaling_factor(0.01)
(plan, fraction) = group.compute_cartesian_path(
list_of_poses, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan.."
# group.plan()
# ipdb.set_trace()
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
rospy.loginfo("============ Visulaize plan")
display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(10)
# ipdb.set_trace()
rospy.loginfo(
"============ Enter \"ok\" to execute plan, \"no\" to cancel execute plan "
)
s = raw_input()
if s == 'ok':
rospy.loginfo("============ Gonna execute plan")
group.execute(plan)
rospy.loginfo("============ Done")
if s == 'no':
rospy.loginfo("============ already cancel")
def main():
rospy.init_node("trac_ik_dmp_cart_node", anonymous=True)
# verify robot is enabled
# print("Getting robot state... ")
# rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
# init_state = rs.state().enabled
# print("Enabling robot... ")
# rs.enable()
# this file is recoded by baxter
# run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name
global data_path, train_len, tau, limb_name
# limb = baxter_interface.Limb(limb_name)
train_set = pd.read_csv(data_path) #using pandas read data
resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0],
train_len) # resampling the time
postion_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 1])
postion_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 2])
postion_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 3])
orientation_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 4])
orientation_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 5])
orientation_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 6])
orientation_w = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 7])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
postion_x[i], postion_y[i], postion_z[i], orientation_x[i],
orientation_y[i], orientation_z[i], orientation_w[i]
]
train_set_T = np.array([
np.array([
postion_x, postion_y, postion_z, orientation_x, orientation_y,
orientation_z, orientation_w
]).T
])
param_w, base_function = train(train_set_T)
#object_vision = get_base_camera_pose(flag = True)
# end_pose = limb.endpoint_pose()
# # get current angles
# endpoint_pose = [ end_pose['position'].x,
# end_pose['position'].y,
# end_pose['position'].z,
# end_pose['orientation'].x,
# end_pose['orientation'].y,
# end_pose['orientation'].z,
# end_pose['orientation'].w ]
# ipdb.set_trace()
start_point = traj[0]
# start_point = endpoint_pose
# print start_point
# ending_point = object_vision
ending_point = traj[-1]
# print ending_point
# dmp_0 = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
# ipdb.set_trace()
num = 10
y0 = [0.04, 0.05, 0.99]
y1 = [0.05, 0.05, 0.99]
y2 = [0.06, 0.05, 0.99]
y3 = [0.02, 0.1, 0.99]
y4 = [0.02, 0.2, 0.99]
y5 = [-0.02, -0.05, -0.99]
y6 = [-0.02, -0.05, -1.2]
y7 = [0.02, 0.05, 1.3]
y8 = [0.1, 0.05, 0.99]
y9 = [0.15, 0.05, 0.99]
list_of_dmp = [None] * num
list_of_track = [None] * num
for i in range(num):
list_of_dmp[i] = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7,
n_bfs=500,
w=param_w)
for j in range(7):
list_of_dmp[i].y0[j] = start_point[j] #set the initial state
list_of_dmp[i].goal[j] = ending_point[j] + (
0.01 * i) # set the ending goal
start_point[j] = list_of_dmp[i].y0[j] + 0.01
ending_point[j] = list_of_dmp[i].goal[j]
list_of_track[i], _, _ = list_of_dmp[i].rollout(tau=tau) # ?
fig = plt.figure()
ax = Axes3D(fig)
plt.xlabel('X')
plt.ylabel('Y')
ax.plot(postion_x, postion_y, postion_z, linewidth=4, alpha=0.3)
for idx in range(num):
ax.plot(list_of_track[idx][:, 0], list_of_track[idx][:, 1],
list_of_track[idx][:, 2], "--")
plt.draw()
plt.show()