roll.py

import pickle
import os, datetime
import rospy
import numpy as np
np.set_printoptions(suppress=True, linewidth=120)
import rospy
import intera_interface as ii
from sensor_msgs.msg import Image, PointCloud2
import time
import argparse
from cv_bridge import CvBridge
import cv2
import ros_numpy
import tf2_ros
from scipy.spatial.transform import Rotation as R
from roll_utils import (
    create_goal_overlay,
    quaternion_rotation_matrix,
    roller_sim_forward,
    get_interaction_args,
    int2bool,
    generate_goal_locations
)
from video_recorder import VideoRecorder as VR
from geometry_msgs.msg import (
    PoseStamped,
    Pose,
    Point,
    Quaternion
)
from intera_core_msgs.srv import (
    SolvePositionIK,
    SolvePositionIKRequest
)
from intera_motion_interface import (
    MotionTrajectory,
    MotionWaypoint,
    MotionWaypointOptions,
    InteractionOptions,
    InteractionPublisher
)
from intera_core_msgs.msg import InteractionControlCommand
from intera_motion_msgs.msg import TrajectoryOptions
from std_msgs.msg import Header

# From Carl, not using.
#from dough.srv import *

# --------------------- Various constants. -------------------------- #
centre = np.array([0.567, -0.325, 0.0])
centre_sim = np.array([0.5, 0.06,  0.5])
ROBOT_INIT_POS = np.array([0.58, -0.05, 0.3265])
TOOL_REL_POS = np.array([0.015, 0, 0.215])
# FIXED_EE_Q = Quaternion(
#                     x=-0.641,
#                     y=0.641,
#                     z=0.299,
#                     w=0.299
#                     )
# FIXED_EE_Q = Quaternion(
#                     x=-0.664,
#                     y=0.664,
#                     z=0.242,
#                     w=0.242
#                     )
FIXED_EE_Q = Quaternion(
                    x=-0.707,
                    y=0.707,
                    z=0.,
                    w=0.
                    )
FIXED_EE_Q_SCIPY = R.from_quat([FIXED_EE_Q.x, FIXED_EE_Q.y, FIXED_EE_Q.z, FIXED_EE_Q.w])

EE_RESET_POSE = np.array([0.56, -0.05, 0.3, FIXED_EE_Q.w, FIXED_EE_Q.x, FIXED_EE_Q.y, FIXED_EE_Q.z])


def solver(pose):
    ns = 'ExternalTools/right/PositionKinematicsNode/IKService'
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    header = Header(stamp = rospy.Time.now(), frame_id = 'base')
    poses = {
        'right': PoseStamped(
            header = header,
            pose = Pose(
                position = Point(
                    x=pose[0],
                    y=pose[1],
                    z=max(pose[2],0.245),
                ),
                orientation = Quaternion(
                    w=pose[3],
                    x=pose[4],
                    y=pose[5],
                    z=pose[6]
                )
                )
            )
        }
    ikreq.pose_stamp.append(poses['right'])
    ikreq.tip_names.append('right_hand')
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
        return resp
    except (rospy.ServiceException, rospy.ROSException) as e:
        print('Failed due to:', e)
        return False


def request_action(reqs):
    rospy.wait_for_service('policy_action')
    try:
        get_action_sim = rospy.ServiceProxy('policy_action', PolicyAct, persistent=True)
        resp = get_action_sim(reqs)

        return (
                np.array(resp.action),
                resp.done
        )
    except rospy.ServiceException as e:
        print(e)


def request_chamfer_distance(reqs):
    rospy.wait_for_service('calc_performance')
    try:
        get_cd = rospy.ServiceProxy('calc_performance', CalcPerformance, persistent=True)
        resp = get_cd(reqs)

        return (
                resp.performance
        )
    except rospy.ServiceException as e:
        print(e)


def get_cd(dough_points, goal_points):
    reqs = CalcPerformanceRequest()
    reqs.dough_x, reqs.dough_y, reqs.dough_z = dough_points[:, 0], dough_points[:, 1], dough_points[:, 2]
    reqs.goal_x, reqs.goal_y, reqs.goal_z = goal_points[:, 0], goal_points[:, 1], goal_points[:, 2]
    init_cd = request_chamfer_distance(reqs)
    return init_cd


def sample_init_tool_particles():
    n = 100
    n_sqrt = int(np.sqrt(n))
    r, h = 0.02 ,0.057
    linsp1 = np.linspace(-h, h, n_sqrt)
    linsp2 = np.linspace(0, np.pi * 2, n_sqrt)
    pos = np.empty((n, 3))
    i = 0
    for k in range(n_sqrt):
        for l in range(n_sqrt):
            pos[i] = np.array([r*np.cos(linsp2[k]), linsp1[l], r*np.sin(linsp2[k])])
            i+=1
    return pos


def sample_goal_particles(init_pos, radius=0.1):
    n_particles = 30000
    r = radius * np.sqrt(np.random.random([n_particles, 1]))
    theta = np.random.random([n_particles, 1]) * 2 * np.pi
    x, y = (np.cos(theta) * r).reshape(-1, 1), (np.sin(theta) * r).reshape(-1, 1)
    p = np.hstack([x, y, np.zeros_like(x)]) + init_pos
    return p


def get_dough_observation(camera_ns, subsample=True):
    # Reset robot arm, generate goal
    dough_xyz = camera_ns+'/filtered_dough_world_xyz'
    dough_xyz_data = rospy.wait_for_message(dough_xyz, PointCloud2)

    pc = ros_numpy.numpify(dough_xyz_data)
    points=np.zeros((pc.shape[0],3))
    points[:,0]=pc['x']
    points[:,1]=pc['y']
    points[:,2]=pc['z']

    if subsample and len(points) > 1000:
        choices = np.random.choice(len(points), size=1000, replace=False)
        points = points[choices]
    return points


def capture_image(dir, camera_ns, filename, goal_points=None):
    color_data = rospy.wait_for_message(camera_ns+'/rgb/image_rect_color', Image)
    rgb_im = bridge.imgmsg_to_cv2(color_data).copy()[:,:,:3]
    img_path = os.path.join(dir, camera_ns+'_'+filename+'.jpg')
    if goal_points is not None:
        overlay = create_goal_overlay(buffer, goal_points, rgb_im.copy(), camera_ns)
        rgb_im = 0.3*rgb_im[:,:,:] + 0.7*overlay[:,:,:]
    cv2.imwrite(img_path, rgb_im)
    return img_path


def get_ee_pose(buffer):
    while not rospy.is_shutdown():
        try:
            ee_transform = buffer.lookup_transform('base', 'reference/right_connector_plate_base', rospy.Time(0))
            ee_pose = np.array([ee_transform.transform.translation.x,
            ee_transform.transform.translation.y,
            ee_transform.transform.translation.z,
            ee_transform.transform.rotation.w,
            ee_transform.transform.rotation.x,
            ee_transform.transform.rotation.y,
            ee_transform.transform.rotation.z])
            break
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            continue
    ee_pose[2] -=0.004
    return ee_pose


def get_tool_pose(buffer):
    while not rospy.is_shutdown():
        try:
            ee_transform = buffer.lookup_transform('base', 'reference/right_connector_plate_base', rospy.Time(0))
            ee_pose = np.array([ee_transform.transform.translation.x,
            ee_transform.transform.translation.y,
            ee_transform.transform.translation.z,
            ee_transform.transform.rotation.w,
            ee_transform.transform.rotation.x,
            ee_transform.transform.rotation.y,
            ee_transform.transform.rotation.z])
            break
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
            continue
    ee_pose[2] -=0.004
    return ee_to_tool(ee_pose)


def get_tool_particles(tool_pose, init_tool_points):
    pos, Q = tool_pose[:3], tool_pose[3:]
    r_mat = quaternion_rotation_matrix(Q)
    trans_tool_points = init_tool_points.dot(r_mat.T)
    trans_tool_points += pos
    return trans_tool_points


def ee_to_tool(ee_pose):
    pos, Q = ee_pose[:3], ee_pose[3:]
    r_mat = quaternion_rotation_matrix(Q)
    TOOL_REL_POS_world = r_mat.dot(TOOL_REL_POS)

    tool_pos_world = pos + TOOL_REL_POS_world

    return np.concatenate([tool_pos_world, Q])


def tool_to_ee(tool_pose):
    pos, Q = tool_pose[:3], tool_pose[3:]
    r_mat = quaternion_rotation_matrix(Q)
    ee_rel_pos = -TOOL_REL_POS
    ee_rel_pos_world = r_mat.dot(ee_rel_pos)
    ee_pos_world = pos + ee_rel_pos_world
    return np.concatenate([ee_pos_world, Q])


def real_tool_to_sim(pose):
    ret_pose = np.zeros(7)

    curr_rot = R.from_quat([pose[4], pose[5], pose[6], pose[3]])
    angle = (curr_rot * FIXED_EE_Q_SCIPY.inv()).as_euler('zyx', degrees=True)[0]
    print("rotated angle:", angle)
    # import pdb; pdb.set_trace()
    rot = R.from_euler('zyx', [0, angle, 0], degrees=True)
    sim_init_rot = R.from_quat([0.707, 0, 0, 0.707])
    q = (rot * sim_init_rot).as_quat()
    ret_pose[3:] = [q[-1], q[0], q[1], q[2]]

    ret_pose[:3] = real_points_to_sim(pose[:3])

    return ret_pose


def sim_tool_to_real(pose):
    ret_pose = np.zeros(7)

    curr_rot = R.from_quat([pose[4], pose[5], pose[6], pose[3]])
    init_rot = R.from_quat([0.707, 0, 0, 0.707])
    rotated_angle = (curr_rot * init_rot.inv()).as_euler('zyx', degrees=True)[1]
    print("rotated angle:", rotated_angle)

    real_init_rot = FIXED_EE_Q_SCIPY
    rot = R.from_euler('zyx', [rotated_angle, 0, 0], degrees=True)
    q = (rot * real_init_rot).as_quat()
    ret_pose[3:] = [q[-1], q[0], q[1], q[2]]

    ret_pose[:3] = sim_points_to_real(pose[:3])
    return ret_pose


def real_points_to_sim(points):
    rotation = R.from_euler('ZYX', [np.pi / 2, 0, np.pi / 2]).inv()
    ret_points = points - centre
    ret_points = rotation.apply(ret_points)

    ret_points += centre_sim
    return ret_points


def sim_points_to_real(points):
    rotation = R.from_euler('ZYX', [np.pi / 2, 0, np.pi / 2])
    ret_points = points - centre_sim
    # ret_points = points
    ret_points = rotation.apply(ret_points)
    ret_points += centre
    return ret_points


def reset_robot(pose):
    print("Resetting robot to position: {}".format(pose[:3]))
    # execute_solver_resp(solver(pose=pose))
    execute_waypoint(solver(pose=pose))
    # motion_traj_plan(pose)

def turn_along_z(tool_pose, radian):
    new_tool_pose = np.zeros(7)
    new_tool_pose[:3] = tool_pose[:3]
    transform = R.from_rotvec(radian * np.array([0, 0, -1]))
    new_quat = (transform * R.from_quat([tool_pose[4], tool_pose[5], tool_pose[6], tool_pose[3]])).as_quat()
    new_tool_pose[3:] = [new_quat[-1], new_quat[0], new_quat[1], new_quat[2]]
    print('policy action: ', new_tool_pose[:3] - tool_pose[:3])
    target_pose = tool_to_ee(new_tool_pose)
    resp = solver(target_pose)
    # execute_solver_resp(resp)
    execute_waypoint(resp)
    return target_pose


def execute_solver_resp(resp):
    print('IK Response: ', resp.joints[0].position)

    temp_positions = {}

    for joint in range(0, 7):
        temp_positions['right_j'+str(joint)] = resp.joints[0].position[joint]

    motion_executed = 0
    while not rospy.is_shutdown() and motion_executed != 1:
        if motion_executed!=1:
            '''If the motion has been executed then increment the value and
            exit the loop'''
            limb.move_to_joint_positions(temp_positions)
            motion_executed = 1


def execute_waypoint(resp):
    try:
        traj.clear_waypoints()
        joint_angles = limb.joint_ordered_angles()
        waypoint.set_joint_angles(joint_angles = joint_angles)
        traj.append_waypoint(waypoint.to_msg())
        temp_positions = {}
        joint_angles = []
        for joint in range(0, 7):
            joint_angles.append(resp.joints[0].position[joint])

        waypoint.set_joint_angles(joint_angles = joint_angles)
        traj.append_waypoint(waypoint.to_msg())

        result = traj.send_trajectory()
        if result is None:
            rospy.logerr('Trajectory FAILED to send!')
            return

        if result.result:
            rospy.loginfo('Motion controller successfully finished the trajectory with interaction options set!')
        else:
            rospy.logerr('Motion controller failed to complete the trajectory with error %s',
                            result.errorId)
    except rospy.ROSInterruptException:
        rospy.logerr('Keyboard interrupt detected from the user. %s',
                     'Exiting before trajectory completion.')


def generate_gif(filenames):
    gif_req = GenerateGifRequest(filenames)
    rospy.wait_for_service('generate_gif')

    try:
        generate = rospy.ServiceProxy('generate_gif', GenerateGif, persistent=True)
        generate(gif_req)
        print("generated rolling gif")
    except rospy.ServiceException as e:
        print(e)


def shutdown_func():
        print("Exit rolling")
        # ic_pub.send_position_mode_cmd()


def roll(args):
    # Logging
    if args.save_photo:
        os.makedirs(args.data_dir)
    dir = args.data_dir

    global limb, motion_executed, traj, waypoint, bridge, buffer

    # Rospy setups
    rospy.init_node('thing1', anonymous=True)
    rospy.Rate(100)
    rospy.on_shutdown(shutdown_func)
    buffer = tf2_ros.Buffer()
    tf2_ros.TransformListener(buffer)

    limb = ii.Limb('right')

    traj = MotionTrajectory(limb = limb)
    wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.1, max_joint_accel=0.1)
    waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
    bridge = CvBridge()


    # Go back to initial position
    reset_robot(np.concatenate([ROBOT_INIT_POS, EE_RESET_POSE[3:]]))
    time.sleep(2)
    # i = 0
    # for r in [0.08, 0.1]:
    #     for _, goal in enumerate(generate_goal_locations()):
    #         points = sample_goal_particles(goal, r)
    #         capture_image(dir, 'k4a_top', 'goal_{}'.format(i), goal_points=points)
    #         capture_image(dir, 'k4a', 'goal_{}'.format(i), goal_points=points)
    #         i += 1
    # exit(0)

    init_dough_points = get_dough_observation('k4a', subsample=False)
    init_tool_points = sample_init_tool_particles()
    all_goal_points = sample_goal_particles(generate_goal_locations()[args.goal_id], args.radius)
    choices = np.random.choice(len(all_goal_points), size=1000, replace=False)
    goal_points = all_goal_points[choices]

    init_cd = get_cd(init_dough_points, all_goal_points)
    print("init chamfer dist:", init_cd)

    if args.save_photo:
        capture_image(dir, 'k4a_top', 'init_dough', goal_points=all_goal_points)
        capture_image(dir, 'k4a', 'init_dough', goal_points=all_goal_points)
        if args.save_video:
            vr = VR(dir, buffer, goal_points=all_goal_points)
            vr.start_recording('k4a/rgb/image_rect_color')

    if args.heuristic:
        roll_heuristic(args)
        T = 0
    else:
        T = 102
        idx_max = np.argmax(init_dough_points[:, 2])
        init_position = init_dough_points[idx_max]
        print("init position:", init_position)
        init_position[2] += 0.02
        EE_RESET_POSE[:3] = tool_to_ee(np.concatenate([init_position, EE_RESET_POSE[3:]]))[:3]
        reset_robot(EE_RESET_POSE)
        done = False
        # raw_input("Press Enter to continue...")

    obs, actions, poses = [], [], []
    for i in range(T):
        print(i)
        if True and i == 50:
            reset_robot(EE_RESET_POSE)
            time.sleep(0.1)
            continue
        elif True and i == 51:
            tool_pose = get_tool_pose(buffer)
            turn_along_z(tool_pose, np.pi/4)
            time.sleep(0.1)
            continue
        else:
            # get observation of the dough point
            if i == 0 or not args.open_loop:
                dough_points = get_dough_observation('k4a')
                print(dough_points.shape)

                # generate tool points
                tool_pose = get_tool_pose(buffer)
                tool_points = get_tool_particles(tool_pose, init_tool_points)
                # pass into the policy, get action
                sim_tool_pose = real_tool_to_sim(tool_pose)
                sim_tool_points, sim_dough_points, sim_goal_points = real_points_to_sim(tool_points), real_points_to_sim(dough_points), real_points_to_sim(goal_points)

                # rescaling
                scale = 2.0
                sim_tool_points -= centre_sim
                sim_dough_points -= centre_sim
                sim_goal_points -= centre_sim
                sim_tool_points, sim_dough_points, sim_goal_points = sim_tool_points * scale, sim_dough_points * scale, sim_goal_points * scale
                sim_tool_points += centre_sim
                sim_dough_points += centre_sim
                sim_goal_points += centre_sim
                # scene_points = np.concatenate([sim_dough_points, sim_tool_points, sim_goal_points], axis=0)
                # scene_min, scene_max = np.min(scene_points, axis=0), np.max(scene_points, axis=0)
                # sim_tool_points -= scene_min + (scene_max - scene_min) / 2
                # sim_dough_points -= scene_min + (scene_max - scene_min) / 2
                # sim_goal_points -= scene_min + (scene_max - scene_min) / 2
                # sim_tool_points, sim_dough_points, sim_goal_points = sim_tool_points * scale, sim_dough_points * scale, sim_goal_points * scale
                # sim_tool_points += scene_min + (scene_max - scene_min) / 2
                # sim_dough_points += scene_min + (scene_max - scene_min) / 2
                # sim_goal_points += scene_min + (scene_max - scene_min) / 2



                reqs = PolicyActRequest()
                reqs.dough_x, reqs.dough_y, reqs.dough_z = sim_dough_points[:, 0], sim_dough_points[:, 1], sim_dough_points[:, 2]
                reqs.tool_x, reqs.tool_y, reqs.tool_z = sim_tool_points[:, 0], sim_tool_points[:, 1], sim_tool_points[:, 2]
                reqs.goal_x, reqs.goal_y, reqs.goal_z = sim_goal_points[:, 0], sim_goal_points[:, 1], sim_goal_points[:, 2]
                reqs.tool_xyz = sim_tool_pose[:3]

                action, pol_done = request_action(reqs)
                if args.open_loop:
                    actions = np.array(action).reshape(-1 ,6)
                    action = actions[0]
                else:
                    actions.append(action)

            else:
                t = 0 if i < 50 else 2
                action = actions[i - t]
                # generate tool points
                tool_pose = get_tool_pose(buffer)
                tool_points = get_tool_particles(tool_pose, init_tool_points)
                # pass into the policy, get action
                sim_tool_pose = real_tool_to_sim(tool_pose)

            #adding
            poses.append(tool_pose)
            obs.append(np.concatenate([sim_dough_points, sim_tool_points, sim_goal_points], axis=0))

            # map action to robot ee position, use ik to solve
            new_sim_tool_pose = roller_sim_forward(sim_tool_pose, action)
            new_sim_tool_points = get_tool_particles(new_sim_tool_pose, init_tool_points)

            new_sim_tool_points -= centre_sim
            new_sim_tool_points *= scale
            new_sim_tool_points += centre_sim
            # new_sim_tool_points -= scene_min + (scene_max - scene_min) / 2
            # new_sim_tool_points *= scale
            # new_sim_tool_points += scene_min + (scene_max - scene_min) / 2
            # import pdb; pdb.set_trace()
            new_tool_pose = sim_tool_to_real(new_sim_tool_pose)
            new_tool_pose[:3] = (new_tool_pose[:3] - tool_pose[:3]) / scale  + tool_pose[:3]

            # import open3d as o3d
            # pcl = np.concatenate([sim_dough_points, sim_tool_points, sim_goal_points, new_sim_tool_points], axis=0)

            # pcd = o3d.geometry.PointCloud()
            # pcd.points = o3d.utility.Vector3dVector(pcl[:, :3])
            # o3d.visualization.draw_geometries([pcd])

            # if args.frame == 'tool':
            #     tool_xyz = sim_tool_pose[:3].reshape(1, 3)
            #     sim_dough_points -= tool_xyz
            #     sim_tool_points -= tool_xyz
            #     sim_goal_points -= tool_xyz
            #     new_sim_tool_points -= tool_xyz
            # import matplotlib.pyplot as plt
            # fig = plt.figure()
            # from mpl_toolkits.mplot3d import Axes3D

            # ax = fig.add_subplot(111, projection='3d')
            # # ax.scatter(dough_points[:, 0], dough_points[:, 1], dough_points[:, 2], alpha=0.5, color='r',label='dough')
            # # ax.scatter(tool_points[:, 0], tool_points[:, 1], tool_points[:, 2], alpha=0.5, color='yellow', label='tool')
            # # ax.scatter(goal_points[:, 0], goal_points[:, 1], goal_points[:, 2], alpha=0.5, label='goal')
            # ax.set_xlim(0.3, 0.8)
            # ax.set_ylim(0., 0.5)
            # ax.set_zlim(0.3, 0.8)
            # ax.scatter(sim_dough_points[:, 0], sim_dough_points[:, 1], sim_dough_points[:, 2], alpha=0.5, color='r',label='dough')
            # ax.scatter(sim_tool_points[:, 0], sim_tool_points[:, 1], sim_tool_points[:, 2], alpha=0.5, color='yellow', label='tool')
            # ax.scatter(sim_goal_points[:, 0], sim_goal_points[:, 1], sim_goal_points[:, 2], alpha=0.5, label='goal')
            # ax.scatter(new_sim_tool_points[:, 0], new_sim_tool_points[:, 1], new_sim_tool_points[:, 2], alpha=0.5, color='green', label='new_tool')
            # plt.legend()
            # plt.show()
            # exit(0)

            print('policy action: ', new_tool_pose[:3] - tool_pose[:3])
            target_pose = tool_to_ee(new_tool_pose)
            # break
            resp = solver(target_pose)
            # execute_solver_resp(resp)
            execute_waypoint(resp)

    reset_robot(EE_RESET_POSE)
    reset_robot(np.concatenate([ROBOT_INIT_POS, EE_RESET_POSE[3:]]))

    final_dough_points = get_dough_observation('k4a_top', subsample=False)
    if args.save_photo:
        capture_image(dir, 'k4a_top', 'final_dough', goal_points=all_goal_points)
        capture_image(dir, 'k4a', 'final_dough', goal_points=all_goal_points)
        if args.save_video:
            vr.stop_recording()
            vr.get_video()

    final_cd = get_cd(final_dough_points, all_goal_points)
    print("final chamfer dist:", final_cd)
    print("normalized performance:", (init_cd - final_cd) / init_cd)
    with open(os.path.join(dir, 'traj.pkl'), 'wb') as handle:
        pickle.dump({'normalized_performance':(init_cd - final_cd) / init_cd,
                    'goal_id': args.goal_id,
                    'radius': args.radius,
                    'obs':np.array(obs),
                    'actions':np.array(actions),
                    'tool_poses':np.array(poses),
                    'all_goal_points':all_goal_points,
                    'init_dough_points':init_dough_points,
                    'final_dough_points':final_dough_points}, handle)


def roll_heuristic(args):
    dir = args.data_dir
    n_roll = 2

    for r in range(n_roll):
        i = 0
        dough_points = get_dough_observation('k4a_top')
        idx_max = np.argmax(dough_points[:, 2])
        target_position = dough_points[idx_max]
        target_position[2] += 0.02

        target_position = tool_to_ee(np.concatenate([target_position, EE_RESET_POSE[3:]]))[:3]

        reset_robot(np.concatenate([target_position, EE_RESET_POSE[3:]]))
        i += 1

        radian = np.pi/4 * r
        tool_pose = get_tool_pose(buffer)
        tool_pose = turn_along_z(tool_pose, radian)
        dough_points = R.from_rotvec(radian * np.array([0, 0, -1])).apply(dough_points)
        i += 1

        roll_length = (np.max(dough_points[:, 1]) - np.min(dough_points[:, 1])) / 2
        roll_depth = (np.max(dough_points[:, 2]) - np.min(dough_points[:, 2]) ) / 2
        print(roll_depth, roll_length)

        waypts = np.array([[target_position[0], target_position[1], target_position[2] -z] for z in [roll_depth]])
        for wp in waypts:
            reset_robot(np.concatenate([wp, tool_pose[3:]]))
            i += 1

        target_position = waypts[-1]
        waypts = np.array([[target_position[0] -l*np.sin(radian), target_position[1] -l*np.cos(radian), target_position[2]] for l in [roll_length]])
        for wp in waypts:
            reset_robot(np.concatenate([wp, tool_pose[3:]]))
            i += 1

        target_position = waypts[-1]
        waypts = np.array([[target_position[0]+2*l*np.sin(radian), target_position[1]+2*l*np.cos(radian), target_position[2]] for l in [roll_length]])
        for wp in waypts:
            reset_robot(np.concatenate([wp, tool_pose[3:]]))
            i += 1

        reset_robot(np.concatenate([ROBOT_INIT_POS, EE_RESET_POSE[3:]]))
        i += 1
        # time.sleep(1)

    return


def parse_args():
    parser = argparse.ArgumentParser(description='Rolling parameters')
    parser.add_argument('--save_photo', dest='save_photo', action='store_true')
    parser.add_argument('--no-save_photo', dest='save_photo', action='store_false')
    parser.set_defaults(save_photo=True)
    parser.add_argument('--save_video', dest='save_video', action='store_true')
    parser.add_argument('--no-save_video', dest='save_video', action='store_false')
    parser.set_defaults(save_video=False)
    parser.add_argument('--heuristic', dest='heuristic', action='store_true')
    parser.add_argument('--no-heuristic', dest='heuristic', action='store_false')
    parser.set_defaults(heuristic=False)
    parser.add_argument('--open_loop', dest='open_loop', action='store_true')
    parser.add_argument('--no-open_loop', dest='open_loop', action='store_false')
    parser.set_defaults(open_loop=False)
    parser.add_argument('--data_dir', type=str, default=os.path.join(os.getcwd(), 'data/debug'))
    parser.add_argument('--frame', type=str, default='world')
    parser.add_argument('--goal_id', type=int, default=-1)
    parser.add_argument('--radius', type=float, default=0.1)
    args = parser.parse_args()
    if args.goal_id == -1:
        print("no valid goal specified !!!!!!!!!!")
        args.data_dir = os.path.join(args.data_dir, datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S'))
    else:
        args.data_dir = os.path.join(args.data_dir,  'goal_{}'.format(args.goal_id), datetime.datetime.now().strftime('%Y-%m-%d_%H-%M-%S'))
    return args


def debug_robot_movement():
    """Debugging robot movement."""
    pass


if __name__ == "__main__":
    try:
        # From Carl
        args = parse_args()
        roll(args)

        # From me
        #test()
    except rospy.ROSInterruptException as e:
        print(e)