def get_lin_interp_poses(start_pos, end_pos, n_steps):
    xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
    xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
    xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
    quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
    hmats = [rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz]) for (xyz, quat) in zip(xyzs, quats)]
    gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
    return gripper_poses
def similar_trajectories(traj1, traj2):
    if len(traj1) != len(traj2):
        return False, "trajectory lengths don't match"
    for index, (traj1_pt, traj2_pt) in enumerate(zip(traj1, traj2)):
        traj1_pt_xyz, traj1_pt_rot = juc.hmat_to_trans_rot(traj1_pt)
        traj2_pt_xyz, traj2_pt_rot = juc.hmat_to_trans_rot(traj2_pt)
        xyz_dist = euclidean_dist(traj1_pt_xyz, traj2_pt_xyz)
        rot_dist = rot_distance(traj1_pt_rot, traj2_pt_rot)
        if xyz_dist > XYZ_TOLERANCE or rot_dist > ROT_TOLERANCE:
            error_msg = "Incorrect point (index %i): { traj1: %s %s, traj2: %s %s }, { xyz_dist: %f, rot_dist: %f }" % (index, str(traj1_pt_xyz), str(traj1_pt_rot), str(traj2_pt_xyz), str(traj2_pt_rot), xyz_dist, rot_dist)
            return False, error_msg
    return True, "matching"
Beispiel #3
0
def get_lin_interp_poses(start_pos, end_pos, n_steps):
    xyz_start, quat_start = juc.hmat_to_trans_rot(start_pos)
    xyz_end, quat_end = juc.hmat_to_trans_rot(end_pos)
    xyzs = math_utils.linspace2d(xyz_start, xyz_end, n_steps)
    quats = [
        rave.quatSlerp(quat_start, quat_end, t)
        for t in np.linspace(0, 1, n_steps)
    ]
    hmats = [
        rave.matrixFromPose(np.r_[quat[3], quat[:3], xyz])
        for (xyz, quat) in zip(xyzs, quats)
    ]
    gripper_poses = np.array([juc.hmat_to_pose(hmat) for hmat in hmats])
    return gripper_poses
Beispiel #4
0
def similar_trajectories(traj1, traj2):
    if len(traj1) != len(traj2):
        return False, "trajectory lengths don't match"
    for index, (traj1_pt, traj2_pt) in enumerate(zip(traj1, traj2)):
        traj1_pt_xyz, traj1_pt_rot = juc.hmat_to_trans_rot(traj1_pt)
        traj2_pt_xyz, traj2_pt_rot = juc.hmat_to_trans_rot(traj2_pt)
        xyz_dist = euclidean_dist(traj1_pt_xyz, traj2_pt_xyz)
        rot_dist = rot_distance(traj1_pt_rot, traj2_pt_rot)
        if xyz_dist > XYZ_TOLERANCE or rot_dist > ROT_TOLERANCE:
            error_msg = "Incorrect point (index %i): { traj1: %s %s, traj2: %s %s }, { xyz_dist: %f, rot_dist: %f }" % (
                index, str(traj1_pt_xyz), str(traj1_pt_rot), str(traj2_pt_xyz),
                str(traj2_pt_rot), xyz_dist, rot_dist)
            return False, error_msg
    return True, "matching"
Beispiel #5
0
 def lookupTransform(self, to_frame, from_frame, *args):
     if not to_frame.startswith("/"):
         to_frame = "/" + to_frame
     if not from_frame.startswith("/"):
         to_frame = "/" + from_frame
     hmat = self.transformer.lookup_transform_mat(to_frame, from_frame)
     return conversions.hmat_to_trans_rot(hmat)
Beispiel #6
0
def transform_demo_with_fingertips(f, demo):
    """
    demo has a bunch of fields with arrays
    this function uses the Transformation f to transform some of the fields of it using f
    """

    warped_demo = {}

    for lr in "lr":
        if "%s_gripper_tool_frame" % lr in demo:
            _, ori = f.transform_frames(
                demo["%s_gripper_tool_frame" % lr]["position"],
                quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
            xyz_fingertip0 = f.transform_points(
                demo["%s_gripper_l_finger_tip_link" % lr]["position"])
            xyz_fingertip1 = f.transform_points(
                demo["%s_gripper_r_finger_tip_link" % lr]["position"])

            closing_only = get_gripper_closing_only_times(demo, lr)
            assert len(closing_only) == len(ori)

            tool_xyzs = []
            tool_quats = []
            tool_angles = []

            print calc_hand_pose(xyz_fingertip0[0], xyz_fingertip1[0],
                                 ori[0])[0][:3, :3]
            first, last_xyz, last_quat = True, None, None
            for (pos0, pos1, o, cl) in zip(xyz_fingertip0, xyz_fingertip1, ori,
                                           closing_only):
                hmat, ang = calc_hand_pose(pos0, pos1, o)
                # if the gripper is closing and nothing else is moving,
                # don't change anything but the gripper angle
                if SPECIAL_GRIPPER_CLOSE and cl and not first:
                    xyz, quat = last_xyz, last_quat
                else:
                    xyz, quat = conversions.hmat_to_trans_rot(hmat)
                tool_xyzs.append(xyz)
                tool_quats.append(quat)
                tool_angles.append(ang)
                last_xyz, last_quat = xyz, quat
                first = False

            warped_demo["%s_gripper_tool_frame" % lr] = {}
            warped_demo["%s_gripper_l_finger_tip_link" % lr] = {}
            warped_demo["%s_gripper_l_finger_tip_link" %
                        lr]["position"] = xyz_fingertip1
            warped_demo["%s_gripper_r_finger_tip_link" % lr] = {}
            warped_demo["%s_gripper_r_finger_tip_link" %
                        lr]["position"] = xyz_fingertip0

            warped_demo["%s_gripper_tool_frame" %
                        lr]["position"] = np.array(tool_xyzs)
            warped_demo["%s_gripper_tool_frame" %
                        lr]["orientation"] = np.array(tool_quats)
            warped_demo["%s_gripper_joint" % lr] = np.array(tool_angles)

    return warped_demo
def get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform):
    demo_spec_pt_traj_xyzs, demo_spec_pt_traj_oriens = [], []
    for demo_spec_pt_traj_mat in demo_spec_pt_traj_mats:
        demo_spec_pt_traj_xyz, demo_spec_pt_traj_orien = juc.hmat_to_trans_rot(demo_spec_pt_traj_mat)
        demo_spec_pt_traj_xyzs.append(demo_spec_pt_traj_xyz)
        demo_spec_pt_traj_oriens.append(juc.quat2mat(demo_spec_pt_traj_orien))
    warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens = demo_to_exp_target_transform.transform_frames(np.array(demo_spec_pt_traj_xyzs), np.array(demo_spec_pt_traj_oriens))
    warped_spec_pt_traj_mats = [juc.trans_rot_to_hmat(warped_spec_pt_traj_xyz, juc.mat2quat(warped_spec_pt_traj_orien)) for warped_spec_pt_traj_xyz, warped_spec_pt_traj_orien in zip(warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens)]
    return warped_spec_pt_traj_mats
def plot_demo_and_warped_tool_spec_pt(spec_pt_in_grip, tool_stage_data, demo_to_exp_tool_transform, arm):
    demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm)
    demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip)
    demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world)
    plot_spec_pts(np.array([demo_spec_pt_in_world]), (1,0,0,1))

    warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame)
    warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame)
    plot_spec_pts(np.array([warped_spec_pt_in_world_trans]), (0,1,0,1))
Beispiel #9
0
def get_warped_spec_pt_traj_mats(demo_spec_pt_traj_mats, demo_to_exp_target_transform):
    demo_spec_pt_traj_xyzs, demo_spec_pt_traj_oriens = [], []
    for demo_spec_pt_traj_mat in demo_spec_pt_traj_mats:
        demo_spec_pt_traj_xyz, demo_spec_pt_traj_orien = juc.hmat_to_trans_rot(demo_spec_pt_traj_mat)
        demo_spec_pt_traj_xyzs.append(demo_spec_pt_traj_xyz)
        demo_spec_pt_traj_oriens.append(juc.quat2mat(demo_spec_pt_traj_orien))
    warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens = demo_to_exp_target_transform.transform_frames(np.array(demo_spec_pt_traj_xyzs), np.array(demo_spec_pt_traj_oriens))
    warped_spec_pt_traj_mats = [juc.trans_rot_to_hmat(warped_spec_pt_traj_xyz, juc.mat2quat(warped_spec_pt_traj_orien)) for warped_spec_pt_traj_xyz, warped_spec_pt_traj_orien in zip(warped_spec_pt_traj_xyzs, warped_spec_pt_traj_oriens)]
    return warped_spec_pt_traj_mats
Beispiel #10
0
def plot_demo_and_warped_tool_spec_pt(spec_pt_in_grip, tool_stage_data, demo_to_exp_tool_transform, arm):
    demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm)
    demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip)
    demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world)
    plot_spec_pts(np.array([demo_spec_pt_in_world]), (1,0,0,0.5))

    warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame)
    warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame)
    plot_spec_pts(np.array([warped_spec_pt_in_world_trans]), (0,1,0,0.5))
Beispiel #11
0
def remove_lower_poses(current_pos, gripper_poses, gripper_angles):
    xyz, quat = juc.hmat_to_trans_rot(current_pos)
    z_floor = xyz[2]
    low_index = 0
    for pose in gripper_poses:
        if pose.position.z >= z_floor:
            break
        low_index += 1
    removed_lower_poses = gripper_poses[low_index:]
    updated_angles = gripper_angles[low_index:]
    return removed_lower_poses, updated_angles
Beispiel #12
0
def remove_lower_poses(current_pos, gripper_poses, gripper_angles):
    xyz, quat = juc.hmat_to_trans_rot(current_pos)
    z_floor = xyz[2]
    low_index = 0
    for pose in gripper_poses:
        if pose.position.z >= z_floor:
            break
        low_index += 1
    removed_lower_poses = gripper_poses[low_index:]
    updated_angles = gripper_angles[low_index:]
    return removed_lower_poses, updated_angles
Beispiel #13
0
def transform_demo_with_fingertips(f, demo):
    """
    demo has a bunch of fields with arrays
    this function uses the Transformation f to transform some of the fields of it using f
    """    
    
    warped_demo = {}

    for lr in "lr":
        if "%s_gripper_tool_frame"%lr in demo:    
            _, ori = f.transform_frames(demo["%s_gripper_tool_frame"%lr]["position"], quats2mats(demo["l_gripper_tool_frame"]["orientation"]))
            xyz_fingertip0 = f.transform_points(demo["%s_gripper_l_finger_tip_link"%lr]["position"])
            xyz_fingertip1 = f.transform_points(demo["%s_gripper_r_finger_tip_link"%lr]["position"])

            closing_only = get_gripper_closing_only_times(demo, lr)
            assert len(closing_only) == len(ori)

            tool_xyzs = []
            tool_quats = []
            tool_angles = []
        
            print calc_hand_pose(xyz_fingertip0[0], xyz_fingertip1[0], ori[0])[0][:3,:3]
            first, last_xyz, last_quat = True, None, None
            for (pos0, pos1, o, cl) in zip(xyz_fingertip0, xyz_fingertip1, ori, closing_only):
                hmat, ang = calc_hand_pose(pos0, pos1, o)
                # if the gripper is closing and nothing else is moving,
                # don't change anything but the gripper angle
                if SPECIAL_GRIPPER_CLOSE and cl and not first:
                    xyz, quat = last_xyz, last_quat
                else:
                    xyz, quat = conversions.hmat_to_trans_rot(hmat)
                tool_xyzs.append(xyz)
                tool_quats.append(quat)
                tool_angles.append(ang)
                last_xyz, last_quat = xyz, quat
                first = False

            warped_demo["%s_gripper_tool_frame"%lr]={}        
            warped_demo["%s_gripper_l_finger_tip_link"%lr]={}        
            warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"]=xyz_fingertip1
            warped_demo["%s_gripper_r_finger_tip_link"%lr]={}
            warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]=xyz_fingertip0
    
            warped_demo["%s_gripper_tool_frame"%lr]["position"] = np.array(tool_xyzs)
            warped_demo["%s_gripper_tool_frame"%lr]["orientation"] = np.array(tool_quats)
            warped_demo["%s_gripper_joint"%lr] = np.array(tool_angles)
        
        
    return warped_demo
Beispiel #14
0
def get_warped_grip_traj_mats(warped_spec_pt_traj_mats, tool_stage_data, demo_to_exp_tool_transform, spec_pt_in_grip, world_to_grip_transform_func, arm):
    if demo_to_exp_tool_transform is None:
        grip_to_spec_pt_transform = np.linalg.inv(jut.translation_matrix(spec_pt_in_grip))
    else:
        demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm)
        demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip)
        demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world)
        # find the transformation from the new special point to the gripper frame
        warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame)
        warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame)
        warped_spec_pt_in_grip_trans = world_to_grip_transform_func(warped_spec_pt_in_world_trans)
        warped_spec_pt_in_grip_rot = warped_spec_pt_in_world_rot
        warped_spec_pt_in_grip_frame = juc.trans_rot_to_hmat(warped_spec_pt_in_grip_trans, warped_spec_pt_in_grip_rot)
        print_hmat_info(warped_spec_pt_in_grip_frame, "warped_spec_pt_in_grip_frame")
        # transform the warped special point trajectory back to a gripper trajectory in the experiment
        grip_to_spec_pt_transform = np.linalg.inv(warped_spec_pt_in_grip_frame)
    warped_grip_traj_mats = [np.dot(spec_pt_mat, grip_to_spec_pt_transform) for spec_pt_mat in warped_spec_pt_traj_mats]
    return warped_grip_traj_mats
def get_warped_grip_traj_mats(warped_spec_pt_traj_mats, tool_stage_data, demo_to_exp_tool_transform, spec_pt_in_grip, world_to_grip_transform_func, arm):
    if demo_to_exp_tool_transform is None:
        grip_to_spec_pt_transform = np.linalg.inv(jut.translation_matrix(spec_pt_in_grip))
    else:
        demo_tool_grip_to_world_transform = get_demo_tool_grip_to_world_transform(tool_stage_data, arm)
        demo_spec_pt_in_world = apply_mat_transform_to_xyz(demo_tool_grip_to_world_transform, spec_pt_in_grip)
        demo_spec_pt_in_world_frame = jut.translation_matrix(demo_spec_pt_in_world)
        # find the transformation from the new special point to the gripper frame
        warped_spec_pt_in_world_frame = apply_tps_transform_to_hmat(demo_to_exp_tool_transform, demo_spec_pt_in_world_frame)
        warped_spec_pt_in_world_trans, warped_spec_pt_in_world_rot = juc.hmat_to_trans_rot(warped_spec_pt_in_world_frame)
        warped_spec_pt_in_grip_trans = world_to_grip_transform_func(warped_spec_pt_in_world_trans)
        warped_spec_pt_in_grip_rot = warped_spec_pt_in_world_rot
        warped_spec_pt_in_grip_frame = juc.trans_rot_to_hmat(warped_spec_pt_in_grip_trans, warped_spec_pt_in_grip_rot)
        print_hmat_info(warped_spec_pt_in_grip_frame, "warped_spec_pt_in_grip_frame")
        # transform the warped special point trajectory back to a gripper trajectory in the experiment
        grip_to_spec_pt_transform = np.linalg.inv(warped_spec_pt_in_grip_frame)
    warped_grip_traj_mats = [np.dot(spec_pt_mat, grip_to_spec_pt_transform) for spec_pt_mat in warped_spec_pt_traj_mats]
    return warped_grip_traj_mats
Beispiel #16
0
def testTransforms (tfms, child_frames, parent_frames, time = 25):
    """
    Basic code to test transforms. Visualize on RViz or something.
    """
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(0.2)
    
    initTime  = rospy.Time.now()
    totTime = rospy.Duration(time)

    if not time:
        foreverCheck = True
    else:
        foreverCheck = False
    
    while foreverCheck or (rospy.Time.now() - initTime < totTime):
        for tfm, child_frame, parent_frame in zip(tfms, child_frames,parent_frames):
            (trans, rot) = conv.hmat_to_trans_rot(tfm)
            br.sendTransform(trans, rot,
                             rospy.Time.now(),
                             child_frame,
                             parent_frame)
            rate.sleep()
Beispiel #17
0
def rot_distance(quat1, quat2):
    quat1_mat = jut.quaternion_matrix(quat1)
    quat2_mat = jut.quaternion_matrix(quat2)
    diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
    diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
    return euclidean_dist(diff_quat, [0, 0, 0, 1])
Beispiel #18
0
def hmat_to_ros_transform(hmat):
    (x, y, z), (xx, yy, zz, ww) = conversions.hmat_to_trans_rot(hmat)
    return gm.Transform(translation=gm.Vector3(x, y, z),
                        rotation=gm.Quaternion(xx, yy, zz, ww))
Beispiel #19
0
 def lookupTransform(self, to_frame, from_frame, *args):
     if not to_frame.startswith('/'): to_frame = '/' + to_frame
     if not from_frame.startswith('/'): to_frame = '/' + from_frame
     hmat = self.transformer.lookup_transform_mat(to_frame, from_frame)
     return conversions.hmat_to_trans_rot(hmat)
Beispiel #20
0
def extract_kinematics_from_bag(bag, link_names):
    """
    Input bagfile has the following topics: /tf, /joint_states, /joy, /xxx/points
    Output dictionary has a bunch of arrays with kinematics info, e.g. 
    /r_gripper_tool_frame/position
                          orientation
                          hmat
    /joint_states/position
                  orientation
    etc.
    """

    rr = RosToRave.create()
    robot = rr.robot

    links = [robot.GetLink(name) for name in link_names]

    l_joint_inds = robot.GetManipulator("leftarm").GetArmIndices()
    r_joint_inds = robot.GetManipulator("rightarm").GetArmIndices()
    l_gripper_inds = [robot.GetJointIndex("l_gripper_joint")]
    r_gripper_inds = [robot.GetJointIndex("r_gripper_joint")]

    out = {}
    for link_name in link_names:
        out[link_name] = {}
        out[link_name]["position"] = []
        out[link_name]["orientation"] = []
        out[link_name]["hmat"] = []
    out["joint_states"] = {}
    out["joint_states"]["position"] = []
    out["leftarm"] = []
    out["rightarm"] = []
    out["l_gripper_joint"] = []
    out["r_gripper_joint"] = []

    out["time"] = []

    first_message = True
    for (topic, msg, t) in bag.read_messages(topics=['/joint_states']):

        t = t.to_sec()
        if first_message:
            rr.init(msg)
            out["joint_states"]["name"] = msg.name
            prev_positions = np.ones(len(msg.position)) * 100
            prev_t = -np.inf
            first_message = False

        if t - prev_t > MIN_TIME and np.any(
                np.abs(np.array(msg.position) -
                       prev_positions) > MIN_JOINT_CHANGE):

            rr.update(msg)

            out["joint_states"]["position"].append(msg.position)
            out["time"].append(t)

            for (link_name, link) in zip(link_names, links):
                hmat = link.GetTransform()
                xyz, quat = conversions.hmat_to_trans_rot(hmat)
                out[link_name]["position"].append(xyz)
                out[link_name]["orientation"].append(quat)
                out[link_name]["hmat"].append(hmat)

            robot.SetActiveDOFs(l_joint_inds)
            out["leftarm"].append(robot.GetActiveDOFValues())
            robot.SetActiveDOFs(r_joint_inds)
            out["rightarm"].append(robot.GetActiveDOFValues())

            robot.SetActiveDOFs(l_gripper_inds)
            out["l_gripper_joint"].append(robot.GetActiveDOFValues()[0])
            robot.SetActiveDOFs(r_gripper_inds)
            out["r_gripper_joint"].append(robot.GetActiveDOFValues()[0])

            prev_t = t
            prev_positions = np.array(msg.position)

    out = convert_lists_to_arrays(out)
    return out
Beispiel #21
0
def rot_distance(quat1, quat2):
    quat1_mat = jut.quaternion_matrix(quat1) 
    quat2_mat = jut.quaternion_matrix(quat2) 
    diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
    diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
    return (sum([x**2 for x in jut.euler_from_quaternion(diff_quat)]))**0.5
def rot_distance(quat1, quat2):
    quat1_mat = jut.quaternion_matrix(quat1) 
    quat2_mat = jut.quaternion_matrix(quat2) 
    diff_mat = np.dot(quat2_mat, np.linalg.inv(quat1_mat))
    diff_xyz, diff_quat = juc.hmat_to_trans_rot(diff_mat)
    return euclidean_dist(diff_quat, [0, 0, 0, 1])
Beispiel #23
0
def apply_tps_transform_to_hmat(tps_transform, hmat):
    xyz, quat = juc.hmat_to_trans_rot(hmat)
    warped_xyz, warped_mat = tps_transform.transform_frames(np.array([xyz]), np.array([juc.quat2mat(quat)]))
    return juc.trans_rot_to_hmat(warped_xyz[0], juc.mat2quat(warped_mat[0]))
Beispiel #24
0
def extract_kinematics_from_bag(bag, link_names):    
    """
    Input bagfile has the following topics: /tf, /joint_states, /joy, /xxx/points
    Output dictionary has a bunch of arrays with kinematics info, e.g. 
    /r_gripper_tool_frame/position
                          orientation
                          hmat
    /joint_states/position
                  orientation
    etc.
    """    
    
    rr = RosToRave.create()
    robot = rr.robot
        
    links = [robot.GetLink(name) for name in link_names]
    
    l_joint_inds = robot.GetManipulator("leftarm").GetArmIndices()
    r_joint_inds = robot.GetManipulator("rightarm").GetArmIndices()
    l_gripper_inds = [robot.GetJointIndex("l_gripper_joint")]
    r_gripper_inds = [robot.GetJointIndex("r_gripper_joint")]
    
    out = {}
    for link_name in link_names:
        out[link_name] = {}
        out[link_name]["position"] = []
        out[link_name]["orientation"] = []
        out[link_name]["hmat"] = []
    out["joint_states"] = {}
    out["joint_states"]["position"] = []
    out["leftarm"] = []
    out["rightarm"] = []
    out["l_gripper_joint"] = []
    out["r_gripper_joint"] = []

    out["time"] = []
    
    first_message = True
    for (topic, msg, t) in bag.read_messages(topics=['/joint_states']):

        t = t.to_sec()
        if first_message:
            rr.init(msg)
            out["joint_states"]["name"] = msg.name
            prev_positions = np.ones(len(msg.position))*100
            prev_t = -np.inf
            first_message = False        
                    
        if t - prev_t > MIN_TIME and np.any(np.abs(np.array(msg.position) - prev_positions) > MIN_JOINT_CHANGE):
            
            rr.update(msg)
            
            out["joint_states"]["position"].append(msg.position)
            out["time"].append(t)
            
            for (link_name, link) in zip(link_names,links):
                hmat = link.GetTransform()
                xyz, quat = conversions.hmat_to_trans_rot(hmat)
                out[link_name]["position"].append(xyz)
                out[link_name]["orientation"].append(quat)
                out[link_name]["hmat"].append(hmat)
                
            robot.SetActiveDOFs(l_joint_inds)
            out["leftarm"].append(robot.GetActiveDOFValues())
            robot.SetActiveDOFs(r_joint_inds)
            out["rightarm"].append(robot.GetActiveDOFValues())


            robot.SetActiveDOFs(l_gripper_inds)
            out["l_gripper_joint"].append(robot.GetActiveDOFValues()[0])
            robot.SetActiveDOFs(r_gripper_inds)
            out["r_gripper_joint"].append(robot.GetActiveDOFValues()[0])
                            
            prev_t = t
            prev_positions = np.array(msg.position)
            

    out = convert_lists_to_arrays(out)
    return out
def apply_tps_transform_to_hmat(tps_transform, hmat):
    xyz, quat = juc.hmat_to_trans_rot(hmat)
    warped_xyz, warped_mat = tps_transform.transform_frames(np.array([xyz]), np.array([juc.quat2mat(quat)]))
    return juc.trans_rot_to_hmat(warped_xyz[0], juc.mat2quat(warped_mat[0]))
Beispiel #26
0
def hmat_to_ros_transform(hmat):
    (x, y, z), (xx, yy, zz, ww) = conversions.hmat_to_trans_rot(hmat)
    return gm.Transform(translation=gm.Vector3(x, y, z), rotation=gm.Quaternion(xx, yy, zz, ww))
Beispiel #27
0
def make_traj_multi_stage_do_work(current_stage_info, cur_exp_clouds, clouds_frame_id, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor, to_gripper_frame_func=None):

    arms_used = current_stage_info.arms_used
    verb_stage_data = verb_data_accessor.get_demo_data(current_stage_info.stage_name)

    if stage_num == 0:
        # don't do any extra transformation for the first stage
        prev_demo_to_exp_grip_transform_lin_rigid = np.identity(4)
        # no special point translation for first stage since no tool yet
        special_point_translation = np.identity(4)
    elif stage_num > 0:

        # make sure that the tool stage only uses one arm (the one with the tool)
        assert arms_used in ['r', 'l']

        prev_stage_data = verb_data_accessor.get_demo_data(prev_stage_info.stage_name)
        prev_demo_pc = prev_stage_data["object_clouds"][prev_stage_info.item]["xyz"]
        prev_exp_pc = prev_exp_clouds[0]
        prev_demo_pc_down = voxel_downsample(prev_demo_pc, .02)
        prev_exp_pc_down = voxel_downsample(prev_exp_pc, .02)

        gripper_data_key = "%s_gripper_tool_frame" % (arms_used)

        # transform point cloud in base frame to gripper frame
        # assume right hand has the tool for now
        # use the last pose of the gripper in the stage to figure out the point cloud of the tool in the gripper frame when the tool was grabbed
        prev_demo_gripper_pos = prev_stage_data[gripper_data_key]["position"][-1]
        prev_demo_gripper_orien = prev_stage_data[gripper_data_key]["orientation"][-1]
        prev_demo_gripper_to_base_transform = juc.trans_rot_to_hmat(prev_demo_gripper_pos, prev_demo_gripper_orien)
        prev_demo_base_to_gripper_transform = np.linalg.inv(prev_demo_gripper_to_base_transform)
        prev_demo_pc_in_gripper_frame = np.array([apply_transform(prev_demo_base_to_gripper_transform, point) for point in prev_demo_pc_down])

        # get the new point cloud in the new gripper frame
        # prev_exp_pc_in_gripper_frame = [apply_transform(prev_exp_base_to_gripper_transform, point) for point in prev_exp_pc_down]
        if to_gripper_frame_func is None:
            prev_exp_pc_in_gripper_frame = to_gripper_frame_tf_listener(prev_exp_pc_down, gripper_data_key)
        else:
            prev_exp_pc_in_gripper_frame = to_gripper_frame_func(prev_exp_pc_down, gripper_data_key)

        # get the transformation from the new point cloud to the old point cloud for the previous stage
        prev_demo_to_exp_grip_transform = get_tps_transform(prev_demo_pc_in_gripper_frame, prev_exp_pc_in_gripper_frame)

        # transforms gripper trajectory point into special point trajectory point
        if prev_stage_info.special_point is None:
            # if there is no special point, linearize at origin
            prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.zeros(3))
            # don't do a special point translation if there is no specified special point
            special_point_translation = np.identity(4)
        else:
            prev_demo_to_exp_grip_transform_lin_rigid = lin_rigid_tps_transform(prev_demo_to_exp_grip_transform, np.array(prev_stage_info.special_point))
            # translation from gripper pose in world frame to special point pose in world frame
            special_point_translation = jut.translation_matrix(np.array(prev_stage_info.special_point))

    if arms_used != 'b':
        arms_used_list = [arms_used]
    else:
        arms_used_list = ['r', 'l']

    warped_stage_data = group_to_dict(verb_stage_data) # deep copy it

    resp = MakeTrajectoryResponse()
    traj = resp.traj
    traj.arms_used = arms_used

    for arm in arms_used_list:

        gripper_data_key = "%s_gripper_tool_frame" % (arm)
        
        # find the special point trajectory before the target transformation
        cur_demo_gripper_traj_xyzs = verb_stage_data[gripper_data_key]["position"]
        cur_demo_gripper_traj_oriens = verb_stage_data[gripper_data_key]["orientation"]
        cur_demo_gripper_traj_mats = [juc.trans_rot_to_hmat(trans, orien) for (trans, orien) in zip(cur_demo_gripper_traj_xyzs, cur_demo_gripper_traj_oriens)]
        cur_mid_spec_pt_traj_mats = [np.dot(gripper_mat, special_point_translation) for gripper_mat in cur_demo_gripper_traj_mats]

        # find the transformation from the new special point to the gripper frame
        cur_exp_inv_special_point_transformation = np.linalg.inv(np.dot(prev_demo_to_exp_grip_transform_lin_rigid, special_point_translation))

        # save the demo special point traj for plotting
        plot_spec_pt_traj = []
        for gripper_mat in cur_demo_gripper_traj_mats:
            spec_pt_xyz, spec_pt_orien = juc.hmat_to_trans_rot(np.dot(gripper_mat, special_point_translation))
            plot_spec_pt_traj.append(spec_pt_xyz)

        print 'grip transform:'
        print prev_demo_to_exp_grip_transform_lin_rigid
        print 'special point translation:'
        print special_point_translation
        print 'inverse special point translation:'
        print cur_exp_inv_special_point_transformation

        # find the target transformation for the experiment scene
        demo_object_clouds = [verb_stage_data["object_clouds"][obj_name]["xyz"] for obj_name in verb_stage_data["object_clouds"].keys()]
        if len(demo_object_clouds) > 1:
            raise Exception("i don't know what to do with multiple object clouds")
        x_nd = voxel_downsample(demo_object_clouds[0], .02)
        y_md = voxel_downsample(cur_exp_clouds[0], .02)
        # transformation from old target object to new target object in world frame
        cur_demo_to_exp_transform = get_tps_transform(x_nd, y_md)

        # apply the target warping transformation to the special point trajectory
        cur_mid_spec_pt_traj_xyzs, cur_mid_spec_pt_traj_oriens = [], []
        for cur_mid_spec_pt_traj_mat in cur_mid_spec_pt_traj_mats:
            cur_mid_spec_pt_traj_xyz, cur_mid_spec_pt_traj_orien = juc.hmat_to_trans_rot(cur_mid_spec_pt_traj_mat)
            cur_mid_spec_pt_traj_xyzs.append(cur_mid_spec_pt_traj_xyz)
            cur_mid_spec_pt_traj_oriens.append(juc.quat2mat(cur_mid_spec_pt_traj_orien))
        cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens = cur_demo_to_exp_transform.transform_frames(np.array(cur_mid_spec_pt_traj_xyzs), np.array(cur_mid_spec_pt_traj_oriens))
        plot_warped_spec_pt_traj = cur_exp_spec_pt_traj_xyzs #save the special point traj for plotting
        cur_exp_spec_pt_traj_mats = [juc.trans_rot_to_hmat(cur_exp_spec_pt_traj_xyz, mat2quat(cur_exp_spec_pt_traj_orien)) for cur_exp_spec_pt_traj_xyz, cur_exp_spec_pt_traj_orien in zip(cur_exp_spec_pt_traj_xyzs, cur_exp_spec_pt_traj_oriens)]

        # transform the warped special point trajectory back to a gripper trajectory in the experiment
        cur_exp_gripper_traj_mats = [np.dot(spec_pt_mat, cur_exp_inv_special_point_transformation) for spec_pt_mat in cur_exp_spec_pt_traj_mats]

        warped_stage_data[gripper_data_key]["position"] = []
        warped_stage_data[gripper_data_key]["orientation"] = []
        for exp_traj_mat in cur_exp_gripper_traj_mats:
            warped_pos, warped_orien = juc.hmat_to_trans_rot(exp_traj_mat)
            warped_stage_data[gripper_data_key]["position"].append(warped_pos)
            warped_stage_data[gripper_data_key]["orientation"].append(warped_orien)

        if arm == 'r':
            traj.r_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
            print "poses: ", len(traj.r_gripper_poses.poses)
            traj.r_gripper_angles = warped_stage_data["r_gripper_joint"]
            traj.r_gripper_poses.header.frame_id = clouds_frame_id
        elif arm == 'l':
            traj.l_gripper_poses.poses = xyzs_quats_to_poses(warped_stage_data[gripper_data_key]["position"], warped_stage_data[gripper_data_key]["orientation"])
            print "poses: ", len(traj.l_gripper_poses.poses)
            traj.l_gripper_angles = warped_stage_data["l_gripper_joint"]
            traj.l_gripper_poses.header.frame_id = clouds_frame_id

    Globals.handles = []
    plot_original_and_warped_demo_and_spec_pt(verb_stage_data, warped_stage_data, plot_spec_pt_traj, plot_warped_spec_pt_traj, traj)
    
    pose_array = conversions.array_to_pose_array(y_md, 'base_footprint')
    Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (0,0,1,1),width=.01,type=Marker.CUBE_LIST))
    return resp
Beispiel #28
0
from jds_utils.conversions import trans_rot_to_hmat,hmat_to_trans_rot
from jds_utils.transformations import euler_from_quaternion
import rospy
import roslib
roslib.load_manifest("tf")
import tf
import numpy as np

class Globals:
    listener = None
    
if rospy.get_name() == "/unnamed":
    rospy.init_node("transform_kinect_frames")
    Globals.listener = tf.TransformListener()
    rospy.sleep(1)

ONIfromCL = trans_rot_to_hmat(*Globals.listener.lookupTransform("/camera_rgb_optical_frame","camera_link", rospy.Time(0)))
GAZfromONI = trans_rot_to_hmat([0, -.15, -.24], [0, 0, 0, 1])
HEADfromGAZ = trans_rot_to_hmat(*Globals.listener.lookupTransform("/head_plate_frame", "/wide_stereo_gazebo_l_stereo_camera_optical_frame",rospy.Time(0)))

HEADfromCL = np.dot(np.dot(HEADfromGAZ, GAZfromONI), ONIfromCL)

trans_final, quat_final = hmat_to_trans_rot(HEADfromCL)
eul_final = euler_from_quaternion(quat_final)

print "translation", trans_final
print "rotation", eul_final


print "%.4f %.4f %.4f %.4f %.4f %.4f %.4f"%(tuple(trans_final) + tuple(quat_final))