Ejemplo n.º 1
0
    def go_angle(self, target_odom, tolerance=math.radians(5.), max_ang_vel=math.radians(20.), func=None):
        self.tl.waitForTransform('base_footprint', 'odom_combined', rospy.Time(), rospy.Duration(10))
        rate = rospy.Rate(100)
        k = math.radians(80)

        #current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2]
        #target_odom = current_ang_odom + delta_ang

        while not rospy.is_shutdown():
            robot_odom = tfu.transform('base_footprint', 'odom_combined', self.tl)
            current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2]
            diff = ut.standard_rad(current_ang_odom - target_odom)
            p = k * diff
            #print diff
            if func != None:
                func(diff)
            if np.abs(diff) < tolerance:
                break

            tw = gm.Twist()
            vels = [p, np.sign(p) * max_ang_vel]
            tw.angular.z = vels[np.argmin(np.abs(vels))]
            #rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z)))
            self.tw_pub.publish(tw)
            #rospy.loginfo('commanded %s' % str(tw))
            rate.sleep()
        rospy.loginfo('finished %.3f' % math.degrees(diff))
        return diff
Ejemplo n.º 2
0
    def go_angle(self,
                 target_odom,
                 tolerance=math.radians(5.),
                 max_ang_vel=math.radians(20.),
                 func=None):
        self.tl.waitForTransform('base_footprint', 'odom_combined',
                                 rospy.Time(), rospy.Duration(10))
        rate = rospy.Rate(100)
        k = math.radians(80)

        #current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2]
        #target_odom = current_ang_odom + delta_ang

        while not rospy.is_shutdown():
            robot_odom = tfu.transform('base_footprint', 'odom_combined',
                                       self.tl)
            current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3],
                                                    'sxyz')[2]
            diff = ut.standard_rad(current_ang_odom - target_odom)
            p = k * diff
            #print diff
            if func != None:
                func(diff)
            if np.abs(diff) < tolerance:
                break

            tw = gm.Twist()
            vels = [p, np.sign(p) * max_ang_vel]
            tw.angular.z = vels[np.argmin(np.abs(vels))]
            #rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z)))
            self.tw_pub.publish(tw)
            #rospy.loginfo('commanded %s' % str(tw))
            rate.sleep()
        rospy.loginfo('finished %.3f' % math.degrees(diff))
        return diff
Ejemplo n.º 3
0
def diff_arm_pose(pose1, pose2):
    pcpy = pose2.copy()
    pcpy[4,0] = unwrap2(pose1[4,0], pose2[4,0])
    pcpy[6,0] = unwrap2(pose1[6,0], pose2[6,0])
    diff = pose1 - pose2
    for i in range(pose1.shape[0]):
        diff[i,0] = ut.standard_rad(diff[i,0])
    return diff
Ejemplo n.º 4
0
def diff_arm_pose(pose1, pose2):
    pcpy = pose2.copy()
    pcpy[4, 0] = unwrap2(pose1[4, 0], pose2[4, 0])
    pcpy[6, 0] = unwrap2(pose1[6, 0], pose2[6, 0])
    diff = pose1 - pose2
    for i in range(pose1.shape[0]):
        diff[i, 0] = ut.standard_rad(diff[i, 0])
    return diff
Ejemplo n.º 5
0
    def go_angle(self, target_base, tolerance=math.radians(5.), max_ang_vel=math.radians(20.), func=None):
        self.tl.waitForTransform('odom_combined', 'base_footprint', rospy.Time(), rospy.Duration(10))
        rate = rospy.Rate(100)
        k = math.radians(80)

        od_T_bf = tfu.transform('odom_combined', 'base_footprint', self.tl)
        target_odom_mat = od_T_bf * tfu.tf_as_matrix( ([0, 0, 0.], tr.quaternion_from_euler(0, 0, target_base)) )
        target_odom = tr.euler_from_quaternion(tfu.matrix_as_tf(target_odom_mat)[1])[2]
        #target_odom = (od_T_bf * np.row_stack([target_base, np.matrix([0,1.]).T]))[0:2,0]

        #current_ang_odom = tr.euler_from_matrix(tfu.transform('base_footprint', 'odom_combined', self.tl)[0:3, 0:3], 'sxyz')[2]
        #target_odom = current_ang_odom + delta_ang

        robot_odom = tfu.transform('odom_combined', 'base_footprint', self.tl)
        current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2]
        diff = ut.standard_rad(current_ang_odom - target_odom)
        rospy.loginfo('go_angle: target %.2f' %  np.degrees(target_odom))
        rospy.loginfo('go_angle: current %.2f' %  np.degrees(current_ang_odom))
        rospy.loginfo('go_angle: diff %.2f' %  np.degrees(diff))

        while not rospy.is_shutdown():
            robot_odom = tfu.transform('odom_combined', 'base_footprint', self.tl)
            current_ang_odom = tr.euler_from_matrix(robot_odom[0:3, 0:3], 'sxyz')[2]
            diff = ut.standard_rad(target_odom - current_ang_odom)
            rospy.loginfo('go_angle: current %.2f diff %.2f' %  (np.degrees(current_ang_odom), np.degrees(diff)))
            p = k * diff
            if func != None:
                func(diff)
            if np.abs(diff) < tolerance:
                break

            if self.go_ang_server.is_preempt_requested():
                self.go_ang_server.set_preempted()
                break

            tw = gm.Twist()
            vels = [p, np.sign(p) * max_ang_vel]
            tw.angular.z = vels[np.argmin(np.abs(vels))]
            #rospy.loginfo('diff %.3f vel %.3f' % (math.degrees(diff), math.degrees(tw.angular.z)))
            self.tw_pub.publish(tw)
            #rospy.loginfo('commanded %s' % str(tw))
            rate.sleep()
        rospy.loginfo('go_ang: finished %.3f' % math.degrees(diff))
        return diff
Ejemplo n.º 6
0
def extract_object_localization_features2(start_conditions, tflistener,
                                          arm_used, p_base_map):
    mid_contact_bf, jstate_msgs = find_contacts_and_fk(tflistener, arm_used)
    model_surf_loc, model_surf_descriptors, surf_loc3d_pro, point_cloud_2d_pro = find3d_surf(
        start_conditions)

    #Find frame
    surf_loc3d_bf = (np.linalg.inv(start_conditions['pro_T_bf']) \
            * np.row_stack((surf_loc3d_pro, 1+np.zeros((1,surf_loc3d_pro.shape[1])))))[0:3,:]
    frame_bf = create_frame(surf_loc3d_bf, p=np.matrix([-1, 0, 0.]).T)
    center_bf = np.mean(surf_loc3d_bf, 1)

    #Find out what the SURF features point to in this new frame
    bf_R_pro = (start_conditions['pro_T_bf'][0:3, 0:3]).T
    bf_R_obj = frame_bf
    x_bf = frame_bf[:, 0]
    x_pro = bf_R_pro.T * x_bf
    x_ang_pro = math.atan2(x_pro[1, 0], x_pro[0, 0])

    center_pro = tfu.transform_points(start_conditions['pro_T_bf'], center_bf)
    center2d_pro = start_conditions['camera_info'].project(center_pro)

    surf_directions = []
    surf_dir_center = []
    for loc, lap, size, direction, hess in model_surf_loc:
        surf_directions.append(
            ut.standard_rad(np.radians(direction) - x_ang_pro))
        direction_to_center = center2d_pro - np.matrix(loc).T
        surf_dir_center.append(direction_to_center)

    surf_dir_center = np.column_stack(surf_dir_center)
    return {
        'contact_bf': mid_contact_bf,
        'surf_loc3d_pro': surf_loc3d_pro,
        'surf_loc2d_pro': model_surf_loc,
        'point_cloud_2d_pro': point_cloud_2d_pro,
        'surf_directions': surf_directions,  #Orientation
        'surf_pose_dir2d': surf_dir_center,  #Position
        'descriptors': model_surf_descriptors,
        'jtransforms': jstate_msgs,
        'frame_bf': frame_bf,
        'center_bf': center_bf
    }
Ejemplo n.º 7
0
def extract_object_localization_features2(start_conditions, tflistener, arm_used, p_base_map):
    mid_contact_bf, jstate_msgs = find_contacts_and_fk(tflistener, arm_used)
    model_surf_loc, model_surf_descriptors, surf_loc3d_pro, point_cloud_2d_pro = find3d_surf(start_conditions)

    #Find frame
    surf_loc3d_bf = (np.linalg.inv(start_conditions['pro_T_bf']) \
            * np.row_stack((surf_loc3d_pro, 1+np.zeros((1,surf_loc3d_pro.shape[1])))))[0:3,:]
    frame_bf = create_frame(surf_loc3d_bf, p=np.matrix([-1, 0, 0.]).T)
    center_bf = np.mean(surf_loc3d_bf, 1)

    #Find out what the SURF features point to in this new frame
    bf_R_pro = (start_conditions['pro_T_bf'][0:3, 0:3]).T
    bf_R_obj = frame_bf
    x_bf     = frame_bf[:,0]
    x_pro    = bf_R_pro.T * x_bf
    x_ang_pro = math.atan2(x_pro[1,0], x_pro[0,0])

    center_pro = tfu.transform_points(start_conditions['pro_T_bf'], center_bf)
    center2d_pro = start_conditions['camera_info'].project(center_pro)

    surf_directions = []
    surf_dir_center = []
    for loc, lap, size, direction, hess in model_surf_loc:
        surf_directions.append(ut.standard_rad(np.radians(direction) - x_ang_pro))
        direction_to_center = center2d_pro - np.matrix(loc).T
        surf_dir_center.append(direction_to_center)

    surf_dir_center = np.column_stack(surf_dir_center)
    return {
            'contact_bf': mid_contact_bf,
            'surf_loc3d_pro': surf_loc3d_pro,
            'surf_loc2d_pro': model_surf_loc,
            'point_cloud_2d_pro': point_cloud_2d_pro,

            'surf_directions': surf_directions, #Orientation
            'surf_pose_dir2d': surf_dir_center,   #Position
            'descriptors': model_surf_descriptors,

            'jtransforms': jstate_msgs,
            'frame_bf': frame_bf,
            'center_bf': center_bf
            }
Ejemplo n.º 8
0
    def go_ang(self, ang, speed):
        dt = math.radians(ang)

        if dt > 0:
            sign = -1
        elif dt < 0:
            sign = 1
        else:
            sign = 0

        self.tl.waitForTransform('base_footprint', 'odom_combined',
                                 rospy.Time(), rospy.Duration(10))
        p0_base = tfu.transform('base_footprint', 'odom_combined',
                                self.tl)  # \
        start_ang = tr.euler_from_matrix(p0_base[0:3, 0:3], 'sxyz')[2]
        r = rospy.Rate(100)
        dist_so_far = 0.

        last_ang = start_ang
        while not rospy.is_shutdown():
            pcurrent_base = tfu.transform('base_footprint', 'odom_combined',
                                          self.tl)  #\
            current_ang = tr.euler_from_matrix(pcurrent_base[0:3, 0:3],
                                               'sxyz')[2]
            dist_so_far = dist_so_far + (ut.standard_rad(current_ang -
                                                         last_ang))
            if dt > 0 and dist_so_far > dt:
                rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
                break
            elif dt < 0 and dist_so_far < dt:
                rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
                break
            elif dt == 0:
                rospy.loginfo('stopped! %f %f' % (dist_so_far, dt))
                break

            tw = gm.Twist()
            tw.angular.z = math.radians(speed * sign)

            self.tw_pub.publish(tw)
            r.sleep()
            last_ang = current_ang
Ejemplo n.º 9
0
    def go_ang(self, ang, speed):
        dt = math.radians(ang)

        if dt > 0:
            sign = -1
        elif dt < 0:
            sign = 1
        else:
            sign = 0

        self.tl.waitForTransform("base_footprint", "odom_combined", rospy.Time(), rospy.Duration(10))
        p0_base = tfu.transform("base_footprint", "odom_combined", self.tl)  # \
        start_ang = tr.euler_from_matrix(p0_base[0:3, 0:3], "sxyz")[2]
        r = rospy.Rate(100)
        dist_so_far = 0.0

        last_ang = start_ang
        while not rospy.is_shutdown():
            pcurrent_base = tfu.transform("base_footprint", "odom_combined", self.tl)  # \
            current_ang = tr.euler_from_matrix(pcurrent_base[0:3, 0:3], "sxyz")[2]
            dist_so_far = dist_so_far + (ut.standard_rad(current_ang - last_ang))
            if dt > 0 and dist_so_far > dt:
                rospy.loginfo("stopped! %f %f" % (dist_so_far, dt))
                break
            elif dt < 0 and dist_so_far < dt:
                rospy.loginfo("stopped! %f %f" % (dist_so_far, dt))
                break
            elif dt == 0:
                rospy.loginfo("stopped! %f %f" % (dist_so_far, dt))
                break

            tw = gm.Twist()
            tw.angular.z = math.radians(speed * sign)

            self.tw_pub.publish(tw)
            r.sleep()
            last_ang = current_ang