Beispiel #1
0
    def update_eq_point(self, arm, motion_vec, step_size, rot_mat):
        x,y,a,dx,dy,da = 0.,0.,0.,0.,0.,0.
        st = self.segway_trajectory
        if len(st.x_list)>0:
            x,y,a = st.x_list[-1],st.y_list[-1],st.a_list[-1]
        if len(st.x_list)>1:
            dx = x-st.x_list[-2]
            dy = y-st.y_list[-2]
            da = tr.angle_within_mod180(x-st.a_list[-2])

        self.eq_pt_cartesian = smc.tlTts(self.eq_pt_cartesian_ts,x,y,a)
        if len(self.zenither_list) > 2:
            h = self.zenither_list[-1] - self.zenither_list[-2]
            self.eq_pt_cartesian[2,0] -= h

        next_pt = self.eq_pt_cartesian + step_size * motion_vec

        if self.q_guess != None:
            if arm == 'right_arm':
                self.q_guess[1] = ut.bound(self.q_guess[1],math.radians(20),math.radians(5))

        q_eq = self.firenze.IK(arm,next_pt,rot_mat,self.q_guess)
        self.eq_pt_cartesian = next_pt
        self.eq_pt_cartesian_ts = smc.tsTtl(self.eq_pt_cartesian,x,y,a)
        self.q_guess = q_eq
        return q_eq
Beispiel #2
0
def calc_motion_all():
    for i in range(len(ee_tl.p_list)):
        curr_pos_tl = np.matrix(ee_tl.p_list[i]).T
        eq_pt_tl = np.matrix(eq_tl.p_list[i]).T

        pts_ts = np.matrix(ee_ts.p_list[0:i+1]).T
        pts_2d_ts = pts_ts[0:2,:]
        rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts,
                                         method='fmin_bfgs',verbose=False)
        c_ts = np.matrix([cx_ts,cy_ts,0.]).T
        x,y,a = st.x_list[i],st.y_list[i],st.a_list[i]
        c_tl = smc.tlTts(c_ts,x,y,a)
        cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]
        vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
                            start_pos_ts,eq_pt_tl,bndry)
        print 'angle:',math.degrees(a)
Beispiel #3
0
def plot_single_point():
    n_pts = 115
    pts_ts = np.matrix(ee_ts.p_list[0:n_pts]).T
    pts_2d_ts = pts_ts[0:2,:]
    rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts,
                                     method='fmin_bfgs',verbose=False)
    print 'rad_opt,cx_ts,cy_ts:',rad_opt,cx_ts,cy_ts

    c_ts = np.matrix([cx_ts,cy_ts,0.]).T
    x,y,a = st.x_list[n_pts-1],st.y_list[n_pts-1],st.a_list[n_pts-1]
    c_tl = smc.tlTts(c_ts,x,y,a)
    cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]

    curr_pos_tl = np.matrix(ee_tl.p_list[n_pts-1]).T
    eqpt_tl = np.matrix(eq_tl.p_list[n_pts-1]).T

    plot_hook_translation(curr_pos_tl,cx_tl,cy_tl,cy_ts,start_pos_ts,
                          eqpt_tl,bndry,wrkspc_pts)
Beispiel #4
0
def plot_eq_pt_motion_tl():
    vec_list = []
    for i in range(len(ee_tl.p_list)):
#    for i in range(5):
        curr_pos_tl = np.matrix(ee_tl.p_list[i]).T
        eq_pt_tl = np.matrix(eq_tl.p_list[i]).T

        pts_ts = np.matrix(ee_ts.p_list[0:i+1]).T
        pts_2d_ts = pts_ts[0:2,:]
#        rad_opt,cx_ts,cy_ts = at.fit_circle(rad_guess,x_guess,y_guess,pts_2d_ts,
#                                         method='fmin_bfgs',verbose=False)
        rad_opt = 1.0
        cx_ts,cy_ts = 0.5,-1.3
        c_ts = np.matrix([cx_ts,cy_ts,0.]).T
        x,y,a = st.x_list[i],st.y_list[i],st.a_list[i]
        c_tl = smc.tlTts(c_ts,x,y,a)
        cx_tl,cy_tl = c_tl[0,0],c_tl[1,0]
        t0 = time.time()
        vt,a = smc.segway_motion_repulse(curr_pos_tl,cx_tl,cy_tl,cy_ts,
                            start_pos_ts,eq_pt_tl,bndry,wrkspc_pts)
        t1 = time.time()
#        print 'time to segway_motion_repulse:',t1-t0
        nrm = np.linalg.norm(vt)
#        if nrm > 0.005:
        vt = vt/nrm
        vec_list.append(vt.A1.tolist())

    v = np.matrix(vec_list).T

    eq_pts = np.matrix(eq_tl.p_list).T
    ee_pts = np.matrix(ee_tl.p_list).T
    mpu.plot_yx(eq_pts[1,:].A1,eq_pts[0,:].A1,linewidth=1,color='g',label='eq')
    mpu.plot_yx(ee_pts[1,:].A1,ee_pts[0,:].A1,linewidth=1,color='b',label='FK')
    mpu.plot_yx(bndry[1,:].A1,bndry[0,:].A1,linewidth=0,color='y')
    mpu.plot_quiver_yxv(eq_pts[1,:].A1,eq_pts[0,:].A1,v,scale=30)
    mpu.legend()
    mpu.show()
Beispiel #5
0
    def equi_pt_generator_control_radial_force(self, arm, rot_mat,
                           h_force_possible, v_force_possible, cep_vel):
        self.log_state(arm)
        step_size = 0.1 * cep_vel # 0.1 is the time interval between calls to the equi_generator function (see pull)
        q_eq = self.update_eq_point(arm, self.eq_motion_vec, step_size,
                                    rot_mat)

        stop = self.common_stopping_conditions()

        wrist_force = self.firenze.get_wrist_force(arm, base_frame=True)
        mag = np.linalg.norm(wrist_force)

        curr_pos_tl = self.firenze.FK(arm,self.pull_trajectory.q_list[-1])
        st = self.segway_trajectory
        x,y,a = st.x_list[-1],st.y_list[-1],st.a_list[-1]
        curr_pos_ts = smc.tsTtl(curr_pos_tl,x,y,a)
        curr_pos = curr_pos_ts
        if len(self.zenither_list) > 1:
            h = self.zenither_list[-1] - self.zenither_list[0]
            curr_pos[2,0] += h

        if len(self.pull_trajectory.q_list)>1:
            start_pos = np.matrix(self.cartesian_pts_list[0]).T
        else:
            start_pos = curr_pos

        #mechanism kinematics.
        self.mech_traj_pub.publish(Point32(curr_pos[0,0],
                                        curr_pos[1,0], curr_pos[2,0]))

        if self.use_jacobian:
            self.mech_kinematics_lock.acquire()

            self.cartesian_pts_list.append(curr_pos.A1.tolist())
            tangential_vec_ts = self.tangential_vec_ts
            force_vec_ts = self.constraint_vec_1_ts + self.constraint_vec_2_ts

            # this is specifically for the right arm, and lots of
            # other assumptions about the hook etc. (Advait, Feb 28, 2010)
            if h_force_possible:
                force_vec_ts[2,0] = 0.
            if v_force_possible:
                force_vec_ts[1,0] = 0.
            force_vec_ts = force_vec_ts / np.linalg.norm(force_vec_ts)
            if force_vec_ts[2,0] < 0.: # only allowing upward force.
                force_vec_ts = -force_vec_ts
            if force_vec_ts[1,0] < 0.: # only allowing force to the left
                force_vec_ts = -force_vec_ts

            self.mech_kinematics_lock.release()

            force_vec = smc.tlRts(force_vec_ts, a)
            tangential_vec = smc.tlRts(tangential_vec_ts, a)
            print 'tangential vec right at the transformation:', tangential_vec.A1
            print 'tangential vec ts right at the transformation:', tangential_vec_ts.A1
            print 'a:', a

        if self.use_rotation_center:
            self.fit_circle_lock.acquire()
            self.cartesian_pts_list.append(curr_pos.A1.tolist())
            rad = self.rad
            cx_start, cy_start = self.cx_start, self.cy_start
            cz_start = self.cz_start
            self.fit_circle_lock.release()
            c_ts = np.matrix([cx_start,cy_start,0.]).T
            c_tl = smc.tlTts(c_ts,x,y,a)
            cx,cy = c_tl[0,0],c_tl[1,0]
            cz = cz_start

            z_start = start_pos[2,0]
            parallel_to_floor = abs(z_start - cz) < 0.1
            print 'abs(z_start - cz)', abs(z_start - cz)
            if parallel_to_floor:
                print 'Mechanism is in the XY plane.'
                # trajectory is parallel to the ground.
                # find tangential direction.
                radial_vec_tl = curr_pos_tl-np.matrix([cx,cy,cz]).T
                radial_vec = radial_vec_tl
                radial_vec = radial_vec/np.linalg.norm(radial_vec)
                if cy_start<start_pos[1,0]:
            #        if cy<0.:
                    tan_x,tan_y = -radial_vec[1,0],radial_vec[0,0]
                else:
                    tan_x,tan_y = radial_vec[1,0],-radial_vec[0,0]
                
                if tan_x > 0. and (start_pos[0,0]-curr_pos[0,0]) < 0.09:
                    tan_x = -tan_x
                    tan_y = -tan_y

                if cy_start>start_pos[1,0]:
                    radial_vec = -radial_vec # axis to the left, want force in
                                           # anti-radial direction.
                rv = radial_vec
                force_vec = np.matrix([rv[0,0], rv[1,0], 0.]).T
                tangential_vec = np.matrix([tan_x, tan_y, 0.]).T
            else:
                print 'Mechanism is in the XZ plane.'
                # here the mechanism does not move in a plane parallel
                # to the ground.
                radial_vec_tl = curr_pos_tl-np.matrix([cx,cy,cz]).T
                radial_vec = radial_vec_tl
                radial_vec = radial_vec/np.linalg.norm(radial_vec)
                tan_x, tan_z = -radial_vec[2,0], radial_vec[0,0]
                
                if tan_x > 0. and (start_pos[0,0]-curr_pos[0,0]) < 0.09:
                    tan_x = -tan_x
                    tan_z = -tan_z

                rv = radial_vec
                force_vec = np.matrix([rv[0,0], 0., rv[2,0]]).T
                tangential_vec = np.matrix([tan_x, 0., tan_z]).T
            
            if abs(curr_pos[2,0] - z_start) > 0.03 and parallel_to_floor:
                force_vec += np.matrix([0.,0.,1]).T
                parallel_to_floor = False

            tangential_vec_ts = smc.tsRtl(tangential_vec, a)
            radial_vec_ts = smc.tsRtl(radial_vec, a)
            force_vec_ts = smc.tsRtl(force_vec, a)

        f_vec = -1*np.array([wrist_force[0,0], wrist_force[1,0],
                             wrist_force[2,0]])
        f_rad_mag = np.dot(f_vec, force_vec.A1)
        err = f_rad_mag-5.
        if err>0.:
            kp = -0.1
        else:
            kp = -0.2
        radial_motion_mag = kp * err # radial_motion_mag in cm (depends on eq_motion step size)

        if self.use_rotation_center:
            if h_force_possible == False and parallel_to_floor:
                radial_motion_mag = 0.
            if v_force_possible == False and parallel_to_floor == False:
                radial_motion_mag = 0.

        radial_motion_vec =  force_vec * radial_motion_mag
        self.eq_motion_vec = copy.copy(tangential_vec)
        self.eq_motion_vec += radial_motion_vec
        
        if self.move_segway_flag:
            self.segway_motion_local(curr_pos_tl, a)

        self.prev_force_mag = mag

        if self.init_tangent_vector == None:
            self.init_tangent_vector = copy.copy(tangential_vec_ts)
        c = np.dot(tangential_vec_ts.A1, self.init_tangent_vector.A1)
        ang = np.arccos(c)

        dist_moved = np.dot((curr_pos - start_pos).A1, self.tangential_vec_ts.A1)
        ftan = abs(np.dot(wrist_force.A1, tangential_vec.A1))
#        print 'wrist force:', wrist_force.A1
#        print 'tangential_vec:', tangential_vec.A1
#        print 'tangential_vec_ts:', tangential_vec_ts.A1
#        print 'ftan:', ftan
#        print 'force magnitude:', mag
        if abs(dist_moved) > 0.09 and self.hooked_location_moved == False:
            # change the force threshold once the hook has started pulling.
            self.hooked_location_moved = True
            self.eq_force_threshold = ut.bound(mag+30.,20.,80.)
            self.ftan_threshold = self.ftan_threshold + max(10., 1.5*ftan)
        if self.hooked_location_moved:
            if abs(tangential_vec_ts[2,0]) < 0.2 and ftan > self.ftan_threshold:
#                print 'ftan threshold exceed.'
                stop = 'ftan threshold exceed: %f'%ftan
        else:
            self.ftan_threshold = max(self.ftan_threshold, ftan)


        if self.hooked_location_moved and ang > math.radians(90.):
            print 'Angle:', math.degrees(ang)
            self.open_ang_exceed_count += 1
            if self.open_ang_exceed_count > 2:
                stop = 'opened mechanism through large angle: %.1f'%(math.degrees(ang))
        else:
            self.open_ang_exceed_count = 0

        self.move_segway_lock.acquire()
        if stop != '' and stop != 'reset timing':
            self.segway_motion_tup = (0.0,0.0,0.0)
            self.new_segway_command = True
        self.move_segway_lock.release()

        self.mech_time_list.append(time.time())
        self.mech_x_list.append(ang)
        self.f_rad_list.append(f_rad_mag)
        self.f_tan_list.append(np.dot(f_vec, tangential_vec.A1))
        self.tan_vec_list.append(tangential_vec_ts.A1.tolist())
        self.rad_vec_list.append(force_vec_ts.A1.tolist())

        return stop, q_eq