def chart_current_v_next(demo_path):
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
demo_paths = image_demo_paths(exp_config["demo_folder"],
im_params["file_glob"],
from_demo=0,
to_demo=1)
# Load all of the poses into a list, reuse some load-data functions here
demo_trajectories = [
get_pose_hist(d, "l_wrist_roll_link") for d in demo_paths
]
demo_trajectories = [
np.array([quat_pose_to_rpy(p) for p in dt]).transpose(1, 0)
for dt in demo_trajectories
]
pose_dim_names = ["x", "y", "z", "r-x", "r-y", "r-z"]
for i in range(6):
plt.subplot(2, 3, i + 1)
plt.xlabel("current {}".format(pose_dim_names[i]))
plt.ylabel("next {}".format(pose_dim_names[i]))
for d in demo_trajectories:
plt.scatter(d[i][0:-5], d[i][5:], alpha=0.05, s=10, color="blue")
plt.show()
def chart_all_poses_3d(demo_path):
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
demo_paths = image_demo_paths(exp_config["demo_folder"],
im_params["file_glob"],
from_demo=0,
to_demo=5)
# Load all of the poses into a list, reuse some load-data functions here
demo_trajectories = [
get_pose_hist(d, "l_wrist_roll_link") for d in demo_paths
]
demo_trajectories = [
np.array([quat_pose_to_rpy(p) for p in dt]).transpose(1, 0)
for dt in demo_trajectories
]
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("t")
for d in demo_trajectories:
time = np.linspace(0, 1, len(d[2]))
ax.scatter(d[0], d[1], time)
plt.show()
def reset_pose():
rospy.init_node('pr2_mover', anonymous=True)
r = rospy.Rate(1)
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
l_group = moveit_commander.MoveGroupCommander('left_arm')
l_group.set_pose_reference_frame("base_link")
r_group = moveit_commander.MoveGroupCommander('right_arm')
r_group.set_pose_reference_frame("base_link")
exp_config = byteify(load_json("config/experiment_config.json"))
left_command = rospy.Publisher('/l_arm_controller/command',
JointTrajectory,
queue_size=10)
right_command = rospy.Publisher('/r_arm_controller/command',
JointTrajectory,
queue_size=10)
right_joints = [
"r_shoulder_pan_joint", "r_shoulder_lift_joint",
"r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint",
"r_wrist_flex_joint", "r_wrist_roll_joint"
]
# left_start = [ 0.2242, 0.3300, 1.4105, -0.8090, 0.1163, -0.9732, 0.2731]
# right_start = [-0.7920, 0.5493, -1.1246, -1.0972, -0.8366, -1.0461, -0.0410]
# rospy.sleep(3)
# send_arm_goal(left_start, left_command, exp_config["nn_joint_names"])
# send_arm_goal(right_start, right_command, right_joints)
# rospy.sleep(3)
r_start_pos = np.array([0.6, -0.45, 0.9])
r_start_rpy = np.array([-1.75, 0.08, 1.17])
l_start_pos = np.array([0.4, 0.2, 0.8])
l_start_rpy = r_start_rpy + np.array([np.pi / 2, 0, -np.pi / 2])
move_to_position_rotation(r_group, r_start_pos, r_start_rpy)
move_to_position_rotation(l_group, l_start_pos, l_start_rpy)
print('Robot policy Prepared.')
# model.eval()
while not rospy.is_shutdown():
l_start_rpy[0] += 0.1
move_to_position_rotation(l_group, l_start_pos, l_start_rpy)
r.sleep()
print('Done.')
def chart_demo_joint_trajectories(demo_path, demo_num):
exp_config = load_json("config/experiment_config.json")
# TODO: This is in multiple places, so I think it needs to be config
im_params = exp_config["image_config"]
im_trans = Compose([
Crop(im_params["crop_top"], im_params["crop_left"],
im_params["crop_height"], im_params["crop_width"]),
Resize(im_params["resize_height"], im_params["resize_width"])
])
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
demo_set, demo_loader = load_demos(demo_path,
im_params["file_glob"],
exp_config["batch_size"],
exp_config["nn_joint_names"],
im_trans,
False,
device,
from_demo=demo_num,
to_demo=demo_num + 1)
joint_poses = []
next_poses = []
with torch.no_grad():
for ins, targets in demo_loader:
_, in_pos = ins
joint_pos = [x.cpu().numpy() for x in in_pos]
next_pos = [x.cpu().numpy() for x in targets]
joint_poses.extend(joint_pos)
next_poses.extend(next_pos)
joint_poses = np.array(joint_poses).transpose()
next_poses = np.array(next_poses).transpose()
fig = plt.figure()
for i, _ in enumerate(joint_poses):
c_ax = fig.add_subplot(ceil(len(joint_poses) / 3.0), 3, i + 1)
c_ax.plot(joint_poses[i], label="Joint Poses")
c_ax.plot(next_poses[i], label="Next Poses")
c_ax.legend()
c_ax.title.set_text(exp_config["nn_joint_names"][i])
c_ax.set_xlabel("t")
c_ax.set_ylabel("Normed Angle")
plt.show()
def chart_all_poses(demo_path):
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
demo_paths = image_demo_paths(exp_config["demo_folder"],
im_params["file_glob"],
from_demo=0)
# Load all of the poses into a list, reuse some load-data functions here
demo_trajectories = [
get_pose_hist(d, "l_wrist_roll_link") for d in demo_paths
]
# demo_trajectories = [np.array([quat_pose_to_rpy(p) for p in dt]).transpose(1,0) for dt in demo_trajectories]
smoothed_trajectories = []
for dt in demo_trajectories:
smoothed_dt = [dt[0]]
for i in range(1, len(dt)):
# print(i)
current_pos, current_quat = dt[i][0:3], dt[i][3:]
prev_quat = smoothed_dt[i - 1][3:]
orig_diff = np.abs(current_quat - prev_quat).sum()
flipped_diff = np.abs(-current_quat - prev_quat).sum()
if flipped_diff <= orig_diff:
print("Orig diff {}, Flipped diff{} at {} ".format(
orig_diff, flipped_diff, i))
print("Original quat {}, Flipped {} ".format(
current_quat, -current_quat))
smoothed_dt.append(np.concatenate(
(current_pos, -current_quat)))
print(len(smoothed_dt))
else:
smoothed_dt.append(np.concatenate((current_pos, current_quat)))
smoothed_trajectories.append(np.array(smoothed_dt))
# smoothed_trajectories = demo_trajectories
# demo_trajectories = [dt.transpose(1,0) for dt in demo_trajectories]
demo_trajectories = [dt.transpose(1, 0) for dt in smoothed_trajectories]
# pose_dim_names = ["x", "y", "z", "r-x", "r-y", "r-z"]
pose_dim_names = ["x", "y", "z", "q-x", "q-y", "q-z", "q-w"]
for i in range(len(pose_dim_names)):
plt.subplot(3, 3, i + 1)
plt.xlabel("time")
plt.ylabel((pose_dim_names[i]))
for d in demo_trajectories:
plt.plot(d[i], alpha=0.15, color="green")
plt.show()
# plt.plot(training_df.error, label="Results")
# # plt.plot(validation_df.error, label="Validation")
plt.xlabel("Epoch")
plt.ylabel("Losses")
plt.legend()
if save_path is not None:
plt.savefig(save_path)
if show_fig:
plt.show()
plt.close()
if __name__ == "__main__":
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
im_trans = get_trans(im_params, distorted=True)
train_set, train_loader = load_demos(exp_config["demo_folder"],
im_params["file_glob"],
exp_config["batch_size"],
exp_config["nn_joint_names"],
im_trans,
True,
torch.device("cuda"),
from_demo=0,
to_demo=60,
skip_count=5)
# for i in range(12):
def chart_demo_predictions(model_path, demo_path, demo_num):
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
exp_config = load_json("config/experiment_config.json")
# TODO: This is in multiple places, so I think it needs to be config
im_params = exp_config["image_config"]
im_trans = Compose([
Crop(im_params["crop_top"], im_params["crop_left"],
im_params["crop_height"], im_params["crop_width"]),
Resize(im_params["resize_height"], im_params["resize_width"])
])
model = load_model(model_path, device, im_params["resize_height"],
im_params["resize_width"], exp_config["nn_joint_names"])
demo_set, demo_loader = load_demos(demo_path,
im_params["file_glob"],
exp_config["batch_size"],
exp_config["nn_joint_names"],
im_trans,
False,
device,
from_demo=demo_num,
to_demo=demo_num + 1)
# show_torched_im(demo_set[0][0][0])
model.eval()
currents = []
ests = []
trues = []
with torch.no_grad():
for (ins, targets) in demo_loader:
_, current_pos = ins
est_next = model(*ins)
est_next = [x.cpu().numpy() for x in est_next]
true_next = [x.cpu().numpy() for x in targets]
current_pos = [x.cpu().numpy() for x in current_pos]
ests.extend(est_next)
trues.extend(true_next)
currents.extend(current_pos)
ests = np.array(ests).transpose()
trues = np.array(trues).transpose()
currents = np.array(currents).transpose()
fig = plt.figure()
for i, _ in enumerate(ests):
c_ax = fig.add_subplot(ceil(len(ests) / 3.0), 3, i + 1)
c_ax.plot()
# c_ax.plot(currents[i], label="Current Joint")
c_ax.plot(ests[i], label="Estimated Next Joints")
c_ax.plot(trues[i], label="True Next Joints")
c_ax.legend()
c_ax.title.set_text(exp_config["nn_joint_names"][i])
c_ax.set_xlabel("t")
c_ax.set_ylabel("Normed Radians")
plt.show()
def chart_mdn_means(model_path, demo_path, demo_num):
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
_, demo_loader = load_pose_state_demos(
image_demo_paths(exp_config["demo_folder"],
im_params["file_glob"],
from_demo=demo_num,
to_demo=demo_num + 1), exp_config["batch_size"],
"l_wrist_roll_link", "r_wrist_roll_link", False, torch.device("cuda"))
model = PosePlusStateNet(100, 2)
model.load_state_dict(
torch.load(model_path, map_location=torch.device("cuda")))
model.to(torch.device("cuda"))
model.eval()
mu_all = []
std_all = []
pis_all = []
targets_all = []
with torch.no_grad():
for ins in demo_loader:
current_pose, goal_pose, targets = ins
pis, stds, mus = model(current_pose, goal_pose)
print("mu {} std {} pis {}".format(mus.shape, stds.shape,
pis.shape))
mu_all.append(mus)
std_all.append(stds)
pis_all.append(pis)
targets_all.append(targets)
mu_all = torch.cat(mu_all)
std_all = torch.cat(std_all)
pis_all = torch.cat(pis_all)
targets_all = torch.cat(targets_all)
# Im expecting: mu: N x 5 X 7, std: N x 5, pis: N * 5, targets_all: N * 7
# print("Shapes: mu: {}, std: {}, pis {}, targets{}".format(mu_all.shape, std_all.shape, pis_all.shape, targets_all.shape))
# I want : mu: 7 X 5 X N, std: 5 X N, pis: 5 X N, targets_all: 7 * N
mu_all = mu_all.permute(2, 1, 0).cpu().numpy()
std_all = std_all.permute(2, 1, 0).cpu().numpy()
pis_all = pis_all.permute(1, 0).cpu().numpy()
targets_all = targets_all.permute(1, 0).cpu().numpy()
print("Shapes: mu: {}, std: {}, pis {}, targets{}".format(
mu_all.shape, std_all.shape, pis_all.shape, targets_all.shape))
fig = plt.figure()
print("STD example", std_all[0][0])
n_joints, n_components, n_samples = mu_all.shape[0], mu_all.shape[
1], mu_all.shape[2]
print("joints {} components {} samples {}".format(n_joints, n_components,
n_samples))
for joint_id in range(n_joints):
plt.subplot(ceil(n_joints / 3.0), 4, joint_id + 1)
for pi_id in range(n_components):
plt.plot(mu_all[joint_id, pi_id], label="pi-{}".format(pi_id))
plt.fill_between(
range(n_samples),
mu_all[joint_id, pi_id] - std_all[joint_id, pi_id],
mu_all[joint_id, pi_id] + std_all[joint_id, pi_id],
alpha=0.1)
weighted_mus = mu_all[joint_id] * pis_all
averaged_mus = weighted_mus.sum(0)
print("muall: {}, pis_all: {} Weighted: {}, Averaged: {}".format(
mu_all.shape, pis_all.shape, weighted_mus.shape,
averaged_mus.shape))
plt.plot(averaged_mus, label="Averaged")
plt.plot(targets_all[joint_id], label="Targets")
plt.legend()
plt.title(["x", "y", "z", "r-x", "r-y", "r-z"][joint_id])
plt.xlabel("t")
plt.ylabel("Normed Radians")
plt.subplot(ceil(n_joints / 3.0), 4, n_joints + 1)
for pi_id in range(n_components):
plt.plot(pis_all[pi_id], label="pi_{}".format(pi_id))
plt.legend()
plt.xlabel("t")
plt.ylabel("Component Weight")
plt.title("Pis")
plt.show()
def chart_pred_pose(model_path, demo_path, demo_num):
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
train_paths = image_demo_paths(exp_config["demo_folder"],
im_params["file_glob"],
from_demo=demo_num,
to_demo=demo_num + 1)
train_set = ImagePoseFuturePose(train_paths, "l_wrist_roll_link",
get_trans(im_params, distorted=True), 10)
demo_loader = DeviceDataLoader(
DataLoader(train_set, exp_config["batch_size"], shuffle=False),
torch.device("cuda"))
model = ImageOnlyNet(im_params["resize_height"], im_params["resize_width"],
6)
model.load_state_dict(
torch.load(model_path, map_location=torch.device("cuda")))
model.to(torch.device("cuda"))
model.eval()
next_pred_all = []
# current_pred_all = []
current_targets_all = []
next_targets_all = []
with torch.no_grad():
for ins in demo_loader:
rgb_ins, current_ins, target_ins = ins
next_pred = model(rgb_ins)
next_pred_all.append(next_pred)
# current_pred_all.append(current_pred)
current_targets_all.append(current_ins)
next_targets_all.append(target_ins)
next_pred_all = torch.cat(next_pred_all)
# current_pred_all = torch.cat(current_pred_all)
current_targets_all = torch.cat(current_targets_all)
next_targets_all = torch.cat(next_targets_all)
# Shapes: N X 7
next_pred_all = next_pred_all.permute(1, 0).cpu().numpy()
# current_pred_all = current_pred_all.permute(1,0).cpu().numpy()
current_targets_all = current_targets_all.permute(1, 0).cpu().numpy()
next_targets_all = next_targets_all.permute(1, 0).cpu().numpy()
n_pose_dims, n_samples = next_pred_all.shape[0], next_pred_all.shape[1]
print(n_pose_dims)
dim_names = ["p-x", "p-y", "p-z", "roll", "pitch", "yaw"]
for dim_id in range(n_pose_dims):
plt.subplot(ceil(n_pose_dims / 3.0), 3, dim_id + 1)
plt.plot(next_pred_all[dim_id], label="Predicted Pose")
# plt.plot(current_pred_all[dim_id], label="Aux Prediction")
plt.plot(current_targets_all[dim_id], label="Current Pose")
plt.plot(next_targets_all[dim_id], label="Target Pose")
plt.legend()
plt.xlabel("t")
plt.ylabel(dim_names[dim_id])
plt.show()
def main(model_path):
exp_config = load_json("config/experiment_config.json")
im_params = exp_config["image_config"]
device = torch.device("cpu")
rgb_trans = get_trans(im_params, distorted=False)
# depth_trans = get_grayscale_trans(im_params)
# train_set, train_loader = load_rgbd_demos(
# exp_config["demo_folder"],
# im_params["file_glob"],
# exp_config["batch_size"],
# "l_wrist_roll_link",
# rgb_trans,
# depth_trans,
# True,
# device,
# from_demo=0,
# to_demo=1)
model = ImagePlusPoseNet(
(im_params["resize_height"], im_params["resize_width"]), 100)
model.load_state_dict(torch.load(model_path, map_location=device))
model.to(device)
model.eval() # Imporant if you have dropout / batchnorm layers!
state_cache = RobotStateCache()
rgb_sub = rospy.Subscriber('/kinect2/qhd/image_color_rect', Image,
state_cache.update_rgb_img)
# depth_sub = rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, state_cache.update_depth_img)
l_pose_sub = rospy.Subscriber('/l_wrist_roll_link_as_posestamped',
gm.PoseStamped, state_cache.update_l_pose)
r_pose_sub = rospy.Subscriber('/r_wrist_roll_link_as_posestamped',
gm.PoseStamped, state_cache.update_r_pose)
print("Subscribed")
rospy.init_node("testMover")
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
l_group = moveit_commander.MoveGroupCommander('left_arm')
l_group.set_pose_reference_frame("base_link")
r_group = moveit_commander.MoveGroupCommander('right_arm')
r_group.set_pose_reference_frame("base_link")
# print(l_group.get_end_effector_link())
# print(l_group.get_pose_reference_frame())
r = rospy.Rate(1.5)
# left_start_pos = np.array([0.68368, 0.1201, 0.8733])
# left_start_quat = np.array([0.49261, -0.5294, -0.4433, 0.529611])
left_start_pos = np.array([6.88e-01, 1.62e-01,
8.38e-01]) # + (np.random.rand(3) - 0.5) * 0.1
# left_start_quat = np.array([3.746527957264187553e-02, -1.138874327630349376e-02, -5.064174634791493990e-01, 8.613988634984809378e-01])
left_start_rpy = np.array([0.0, 0.0, -np.pi / 2.0])
right_start_pos = np.array([
4.350030651697819328e-01, -3.619320769156886275e-01,
9.532789909342967993e-01
]) #+ (np.random.rand(3) - 0.5) * 0.1
# right_start_quat = np.array([7.092342993967834519e-02,-1.200804160961578410e-01,5.726577827747867389e-01, 8.078450498599533125e-01])
# right_start_rpy = tf.transformations.euler_from_quaternion(right_start_quat)
right_start_rpy = np.array([0.0, 0.0, np.pi / 2.0])
move_to_pos_rpy(l_group, left_start_pos, left_start_rpy)
move_to_pos_rpy(r_group, right_start_pos, right_start_rpy)
rospy.sleep(3)
# Closed Loop
while not rospy.is_shutdown():
print("Step")
if state_cache.l_pose is not None and state_cache.r_pose is not None and state_cache.rgb_im is not None:
with torch.no_grad():
current_pose = torch.tensor(state_cache.l_pose,
dtype=torch.float)
goal_pose = torch.tensor(state_cache.r_pose, dtype=torch.float)
print("Current: {}".format(current_pose))
next_pose = model(
rgb_trans(state_cache.rgb_im).unsqueeze(0),
current_pose.unsqueeze(0))[0]
# pis, stds, mus = model( current_pose.unsqueeze(0), goal_pose.unsqueeze(0))
# next_pose = approx_ml(pis[0], stds[0], mus[0], num_samples=1000)
print("Next {}".format(next_pose))
next_position = next_pose[0:3].numpy().astype(np.float64)
next_rpy = next_pose[3:].numpy().astype(np.float64) * np.pi
# print("For sanity {}\n{}\n{}".format(next_pose[3:], next_pose[3:].numpy(), next_pose[3:].numpy().astype(np.float64)))
# print("Next rpy {}".format(next_rpy))
next_quat = R.from_euler("xyz", next_rpy).as_quat()
move_to_pos_quat(l_group, next_position, next_quat)
r.sleep()
