Beispiel #1
0
    def reposition_robot(self,hook_location,turn_angle,hook_angle,position_number=2):
        h = self.workspace_dict[hook_angle]['height']
        max_z_height = 1.3
        min_z_height = 0.5
        h_desired = self.z.get_position_meters()+hook_location[2,0]-h
        print 'h_desired:',h_desired
        print 'h:',h
        h_achieve = ut.bound(h_desired,max_z_height,min_z_height)
        h_err = h_desired-h_achieve
        h = h+h_err
        print 'h_achieve:',h_achieve

        wkd = self.workspace_dict[hook_angle]['pts']
        k = wkd.keys()
        k_idx = np.argmin(np.abs(np.array(k)-h))
        wrkspc_pts = wkd[k[k_idx]]

#        good_location = np.matrix([0.45,-0.35,h]).T
        good_location = wrkspc_pts.mean(1)
        good_location = np.matrix(good_location.A1.tolist()+[h]).T

        if position_number == 1:
            good_location[0,0] += 0.0
            good_location[1,0] -= 0.1
        elif position_number == 2:
            good_location[0,0] += 0.0
            good_location[1,0] -= 0.0
        elif position_number == 3:
            good_location[0,0] += 0.0
            good_location[1,0] += 0.1
        else:
            good_location[0,0] -= 0.1
            good_location[1,0] -= 0.0

        good_location = mcf.mecanumTglobal(mcf.globalTtorso(good_location))
        hook_location = mcf.mecanumTglobal(mcf.globalTtorso(hook_location))

        v = tr.Rz(-turn_angle)*good_location
        move_vector = hook_location - v

        pose1 = self.segway_pose.get_pose()
        self.segway_command_node.go_xya_pos_local(move_vector[0,0],move_vector[1,0],
                                                  turn_angle,blocking=False)

        self.z.torque_move_position(h_achieve)
        print 'before waiting for segway to stop moving.'
        self.segway_command_node.wait_for_tolerance_pos(0.03,0.03,math.radians(4))
        print 'after segway has stopped moving.'
        
        pose2 = self.segway_pose.get_pose()
        hl_new = vop.transform_pts_local(hook_location[0:2,:],pose1,pose2)
        h_err = h_desired-self.z.get_position_meters()

        g = np.matrix(hl_new.A1.tolist()+[good_location[2,0]+h_err]).T
        g = mcf.torsoTglobal(mcf.globalTmecanum(g))
        angle_turned = pose2[2]-pose1[2]
        angle_error = tr.angle_within_mod180(turn_angle-angle_turned)
        self.segway_pose.set_origin() # re-origining for the manipulation behaviors
        return g, angle_error
Beispiel #2
0
    def segway_motion_local(self, curr_pos_tl, sa):
        k = self.wkd['pts'].keys()
        z = min(self.eq_pt_cartesian[2,0], curr_pos_tl[2,0])
        k_idx = np.argmin(np.abs(np.array(k)-z))
        wrkspc_pts = self.wkd['pts'][k[k_idx]]
        bndry = self.wkd['bndry'][k[k_idx]]

        self.eq_pt_close_to_bndry = False
        if z < -0.4:
            # don't open the mechanism
            self.eq_motion_vec = np.matrix([0.,0.,0.]).T
            self.eq_pt_close_to_bndry = True

        if self.zenither_client.height() > 0.6:
            if z < -0.35 and self.zenither_moving == False:
                #self.z.nadir(1)
                self.z.nadir(0)
                self.zenither_moving = True
            if z > -0.25 and self.zenither_moving == True:
                self.z.estop()
                self.zenither_moving = False
        else:
            if self.zenither_moving:
                self.z.estop()
                self.zenither_moving = False

        ht = smc.segway_motion_repulse(curr_pos_tl, self.eq_pt_cartesian, bndry,
                                       wrkspc_pts)

        self.vec_bndry = smc.vec_from_boundary(self.eq_pt_cartesian, bndry)
        self.dist_boundary = smc.dist_from_boundary(self.eq_pt_cartesian, bndry,
                                                    wrkspc_pts)
        dist_bndry_ee = smc.dist_from_boundary(curr_pos_tl,bndry,wrkspc_pts)
        self.vec_bndry = self.vec_bndry/self.dist_boundary
        vec_bndry = self.vec_bndry
        dist_boundary = self.dist_boundary

        avel = -sa * 0.05 # maintaining the orientation of the segway.

        eq_mec = mcf.mecanumTglobal(mcf.globalTtorso(self.eq_pt_cartesian))
        svel_rot_y = -avel*eq_mec[0,0] # segway vel in y direction due to rotation
        svel_rot_x = avel*eq_mec[1,0] # segway vel in x direction due to rotation

        if self.zenither_moving:
            sp = 0.075
        else:
            sp = 0.075
        st = ht*sp + np.matrix([svel_rot_x,svel_rot_y]).T
        st[0,0] = min(st[0,0],0.) # disallowing +ve translation.

        self.move_segway_lock.acquire()
        self.segway_motion_tup = (st[0,0],st[1,0],avel)
        self.new_segway_command = True
        self.move_segway_lock.release()

        proj = (self.eq_motion_vec[0:2,0].T*vec_bndry)[0,0]
        proj_threshold = math.cos(math.radians(100))
        if (dist_boundary<= 0.04 and proj<proj_threshold) or \
            dist_boundary<0.03:
            #dist_boundary<0.01 or dist_bndry_ee < 0.04:
            self.eq_motion_vec = np.matrix([0.,0.,0.]).T
            self.eq_pt_close_to_bndry = True

        if self.eq_pt_close_to_bndry:
            if not self.zenither_moving:
                self.eq_pt_not_moving_counter += 1
            else:
                self.eq_pt_not_moving_counter = 0
        else:
            self.eq_pt_not_moving_counter = 0


        print '-------------------------------------'
        print 'segway_translation:',st[0,0],st[1,0]
        print 'avel:',math.degrees(avel)
        print 'dist_boundary:', dist_boundary
        print 'proj,proj_threshold:',proj,proj_threshold
        print 'self.eq_pt_cartesian:',self.eq_pt_cartesian.A1.tolist()
        print 'self.eq_pt_close_to_bndry?',self.eq_pt_close_to_bndry
        print 'dist_bndry_ee:',dist_bndry_ee
Beispiel #3
0
def tlRts(vecs_ts, a):
    vecs_ms = mcf.mecanumTglobal(mcf.globalTtorso(vecs_ts, True), True)
    vecs_ml = tr.Rz(a) * vecs_ms
    vecs_tl = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ml, True), True)
    return vecs_tl
Beispiel #4
0
def tsTtl(pts_tl,x,y,a):
    pts_ml = mcf.mecanumTglobal(mcf.globalTtorso(pts_tl))
    v_org_ms = np.matrix([x,y,0.]).T
    pts_ms = tr.Rz(-a) * pts_ml + v_org_ms
    pts_ts = mcf.torsoTglobal(mcf.globalTmecanum(pts_ms))
    return pts_ts
Beispiel #5
0
def tsRtl(vecs_tl, a):
    vecs_ml = mcf.mecanumTglobal(mcf.globalTtorso(vecs_tl, True), True)
    vecs_ms = tr.Rz(-a) * vecs_ml
    vecs_ts = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ms, True), True)
    return vecs_ts
Beispiel #6
0
def tlTts(pts_ts,x,y,a):
    pts_ms = mcf.mecanumTglobal(mcf.globalTtorso(pts_ts))
    v_org_ms = np.matrix([x,y,0.]).T
    pts_ml = tr.Rz(a)*(pts_ms-v_org_ms)
    pts_tl = mcf.torsoTglobal(mcf.globalTmecanum(pts_ml))
    return pts_tl
Beispiel #7
0
def tlRts(vecs_ts, a):
    vecs_ms = mcf.mecanumTglobal(mcf.globalTtorso(vecs_ts, True), True)
    vecs_ml = tr.Rz(a) * vecs_ms
    vecs_tl = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ml, True), True)
    return vecs_tl
Beispiel #8
0
def tsRtl(vecs_tl, a):
    vecs_ml = mcf.mecanumTglobal(mcf.globalTtorso(vecs_tl, True), True)
    vecs_ms = tr.Rz(-a) * vecs_ml
    vecs_ts = mcf.torsoTglobal(mcf.globalTmecanum(vecs_ms, True), True)
    return vecs_ts
Beispiel #9
0
def tsTtl(pts_tl, x, y, a):
    pts_ml = mcf.mecanumTglobal(mcf.globalTtorso(pts_tl))
    v_org_ms = np.matrix([x, y, 0.]).T
    pts_ms = tr.Rz(-a) * pts_ml + v_org_ms
    pts_ts = mcf.torsoTglobal(mcf.globalTmecanum(pts_ms))
    return pts_ts
Beispiel #10
0
def tlTts(pts_ts, x, y, a):
    pts_ms = mcf.mecanumTglobal(mcf.globalTtorso(pts_ts))
    v_org_ms = np.matrix([x, y, 0.]).T
    pts_ml = tr.Rz(a) * (pts_ms - v_org_ms)
    pts_tl = mcf.torsoTglobal(mcf.globalTmecanum(pts_ml))
    return pts_tl