Esempio n. 1
0
 def setUp(self):
     self.dh_table = DH_TABLE
     self.dh_table['tool'] = None
     self.joints = (0, 0, 0, 0, 0, 0)
     self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp']
     self.flange = forward_kinematics(*self.joints,
                                      **self.dh_table)['flange']
    def __calibrate(self, num_points=-1, random=False):
        self.robot_infos = map(lambda x: forward_kinematics(*x, **dh_table),
                               self.parser.robot_joints)
        fk = self.flange

        # anoto coords
        pentip_2d = self.parser.pen_pos

        # convert to mm, first point is origin
        #pentip_2d = (pentip_2d - pentip_2d[0])*0.3
        f_ad = 7.0 * (25.4 / 600.0)
        pentip_2d = (pentip_2d) * f_ad
        if not num_points == -1:
            if random:
                unique_rand = list(set(randint(0, len(fk), num_points)))
                fk = fk[unique_rand]
                pentip_2d = pentip_2d[unique_rand]
        self.geometry_info = {
            'data': {
                'forward_kinematics': fk,
                'pentip_2d': pentip_2d
            }
        }
        self._res, self.cond = calib.find_solution_pen_tip(
            self.geometry_info, num_points)
def main():
    j1 = 0
    j2 = 0
    j3 = 0
    j4 = 0
    j5 = 0
    j6 = 0

    joint_values = [j1, j2, j3, j4, j5, j6]
    tool = hom(0, 0, 0, [0.1, 0, 0])

    dh_table['tool'] = tool
    info = forward_kinematics(*joint_values, **dh_table)
    # multiplication from left: rotation in base to base
    # multiplication from right: rotation in tool to base
    tcp = info['tcp'].dot(hom(ori(-10, 30, 40)))
    pose = hom(tcp[:3, :3], [0.6, 0, 0.3])

    s = calc_invkin_irb140(pose, raw_solutions=True)
    ik_up = forward_kinematics(*s[0], **dh_table)
    ik_down = forward_kinematics(*s[5], **dh_table)
    ik_up_back = forward_kinematics(*s[10], **dh_table)
    ik_down_back = forward_kinematics(*s[15], **dh_table)

    #plot_robot_geometry(info)
    plot_robot_geometry(ik_up, 'b')
    plot_robot_geometry(ik_up_back, 'b--')
    plot_robot_geometry(ik_down, 'r')
    plot_robot_geometry(ik_down_back, 'r--')
    xlabel('x [m]', fontsize=LABEL_SIZE)
    ylabel('z [m]', fontsize=LABEL_SIZE)
    xticks(fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    grid()
    axes().set_aspect('equal', 'datalim')
    # save figure plot
    figpath = r"C:\Users\***REMOVED***\Dropbox\exjobb\rapport\images"
    savefig(path.join(figpath, "invkin-elbowupdown2.png"), bbox_inches='tight')
Esempio n. 4
0
def jacobian_from_joints(*joints):
    fk = forward_kinematics(*joints, **DH_TABLE)['robot_geometry_global']
    if not (DH_TABLE['tool'] is None):
        fk[-1] = fk[-1].dot(DH_TABLE['tool'])
    fk = mat(fk)
    zi = fk[:-1, :3, 2]
    Jw = zi.T

    on = fk[-1, :3, 3]
    oi = fk[:-1, :3, 3]
    odiff = on - oi
    Jv = nmap(lambda x: reduce(cross, x), zip(zi, odiff)).T
    J = mat([Jv, Jw])
    J = J.reshape((6, 6))
    return J
def flange(pose):
    '''
    Given a pose, calulate the forward-kinematics frame obtained
    from reaching the point by inverse-kinematics.
    '''
    return forward_kinematics(*ik(pose), **dh_params)['flange']
                    'forward_kinematics': flanges,
                    'pentip_2d': penxyanoto,
                    'global_tool_orientation': penori
                }
        }

    # save data to pickle
    pickle_data = []
    for k in xrange(len(flanges)):
        tmp = {
            'pen_pos': geometry_info['data']['pentip_2d'][k],
            'pen_ori': geometry_info['data']['global_tool_orientation'][k][:3,:3],
            'pen_fsr': numpy.NAN,
            'pen_fsr_adc': numpy.NAN,
            'robot_joints': ik(global_tars[k]),
            'robot_tcp': forward_kinematics(*global_tars[0], **dh_params)['tcp'],
            'robot_flange': forward_kinematics(*global_tars[0], **dh_params)['flange'],
            }
        pickle_data.append(tmp)

    with open('calibration_verifitcation.pickle', 'wb+') as fp:
        pickle.dump(pickle_data, fp)

    with open('tool_wobj.yaml', 'w+') as fp:
        json.dump({
            'tool': {
                'pos': list(dh_params['tool'][:3,3]),
                'q': list(rot_to_quat(dh_params['tool'][:3,:3]))
                },
            'wobj': {
                'pos': list(wobj[:3,3]),
Esempio n. 7
0
def fk(pose):
    return forward_kinematics(*pose, **dh_params)
Esempio n. 8
0
 def setUp(self):
     self.dh_table = DH_TABLE
     self.dh_table['tool'] = hom(0, 0, 0, [0.1, 0.12, 0.13])
     self.joints = (0, 0, 0, 0, 0, 0)
     self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp']
Esempio n. 9
0
def robot_frames(*joints, **dh_table):
    return forward_kinematics(*joints, **dh_table)['robot_geometry_global']
Esempio n. 10
0
def flange_from_joints(*joints, **dh_table):
    return forward_kinematics(*joints, **dh_table)['flange']
Esempio n. 11
0
def tcp_from_joints(*joints, **dh_table):
    return forward_kinematics(*joints, **dh_table)['tcp']
Esempio n. 12
0
def calc_robot_tcp(*joints):
    return forward_kinematics(*joints, **dh_table)['tcp']
Esempio n. 13
0
def main():
    dh_table['tool'] = hom(0, 0, 0, [0.0, 0.0, 0.1])
    wobj = hom(-90, 180, 0, [0.6, 0, 0.5])
    # robot movement
    ##robot_movement_j1, vw, dts = calc_robot_curve_j1()
    ##trajectory = robot_movement_j1

    curve = generate_symmetric_curve()
    N = len(curve)
    T = 12.56
    L = 2 * pi
    dt = T / N  # seconds / dx
    rads_per_second = L / T

    frame = calc_robot_tcp(0, 0, 0, 0, 90, 0)
    center = frame[:3, 3]
    curve = nmap(frame.dot, curve)
    r = curve[:, :3] - center
    frames = n.tile(frame, (50, 1)).reshape(50, 4, 4)
    frames[:, :, 3] = curve
    trajectory = frames
    t = linspace(0, T, N)
    w = mat([[0, 0, -1]] * len(trajectory)) * rads_per_second
    v = nmap(lambda x: reduce(n.cross, x), zip(w, r))
    w = mat([[0, 0, 0]] * len(trajectory))
    vw = n.hstack((v, w))

    #inverse kinematics over curve
    result = inverse_kinematics_curve(trajectory)
    path = find_single_path(result)
    J = nmap(lambda x: jacobian_from_joints(*x), path)
    Jinv = nmap(inv, J)
    joint_angular_vel = nmap(lambda x: reduce(n.dot, x), zip(Jinv,
                                                             vw)) * 180 / pi

    print 'Path found: \n {}'.format(path)
    print 'Joint angular velocities: \n {}'.format(joint_angular_vel)

    index = 0
    subplots_adjust(hspace=0.55)
    subplot(211)
    custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
    subplot(212)
    custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
                'r',
                linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_W, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$\dot{q}_' + str(index + 1) + '$'],
           loc='lower right',
           fontsize=LEGEND_SIZE)
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q1.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    index = 1
    subplots_adjust(hspace=0.55)
    subplot(211)
    custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
    subplot(212)
    custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
                'r',
                linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_W, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$\dot{q}_' + str(index + 1) + '$'],
           loc='upper right',
           fontsize=LEGEND_SIZE)
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q2.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    index = 2
    subplots_adjust(hspace=0.55)
    subplot(211)
    custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$q_' + str(index + 1) + '$'],
           loc='lower right',
           fontsize=LEGEND_SIZE)
    subplot(212)
    custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
                'r',
                linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_W, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$\dot{q}_' + str(index + 1) + '$'],
           loc='lower right',
           fontsize=LEGEND_SIZE)
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q3.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    index = 3
    subplots_adjust(hspace=0.55)
    subplot(211)
    custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
    subplot(212)
    custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
                'r',
                linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_W, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$\dot{q}_' + str(index + 1) + '$'],
           loc='upper right',
           fontsize=LEGEND_SIZE)
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q4.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    index = 4
    subplots_adjust(hspace=0.55)
    subplot(211)
    custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
    subplot(212)
    custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
                'r',
                linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_W, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$\dot{q}_' + str(index + 1) + '$'],
           loc='lower right',
           fontsize=LEGEND_SIZE)
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q5.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    index = 5
    subplots_adjust(hspace=0.55)
    subplot(211)
    custom_plot(n.round(path[:, index], decimals=3), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_Q, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$q_' + str(index + 1) + '$'], fontsize=LEGEND_SIZE)
    subplot(212)
    custom_plot(n.round(joint_angular_vel[:, index], decimals=3),
                'r',
                linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel(YLABEL_W, fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    legend(['$\dot{q}_' + str(index + 1) + '$'],
           loc='lower right',
           fontsize=LEGEND_SIZE)
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\q6.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    custom_plot(nmap(det, J), 'b', linewidth=1.5)
    xlabel(XLABEL, fontsize=LABEL_SIZE)
    ylabel('Jacobian determinant value', fontsize=LABEL_SIZE)
    xticks(range(0, 50, 10) + [49], linspace(0, T, 6), fontsize=TICK_SIZE)
    yticks(fontsize=TICK_SIZE)
    xlim((0, 49))
    maximize_plot()
    savefig(
        'C:\\Users\\***REMOVED***\\Dropbox\\exjobb\\results\\inverse_kinematics_over_curve\\J.png',
        bbox_inches='tight',
        pad_inches=0)
    clf()

    pl = StPlot()
    joints = path[13]
    robot_info = forward_kinematics(*joints, **dh_table)
    pl.draw_robot(robot_info['robot_geometry_global'])
    pl.draw_trajectory(trajectory)
    pl.draw_tool(robot_info['flange'], dh_table['tool'])
Esempio n. 14
0
tilt = numpy.linspace(-40,40)
skew = numpy.linspace(0,0)
angles = nzip(rot, tilt, skew)

if __name__ == '__main__':
    dh_table['tool'] = hom(0,0,0,[0.0,0.0,0.1])
    wobj = hom(-90,180,0,[0.3,0,0.5])

    # generate a curve in the last global robot-frame
    curve = create_circle_curve(diameter=0.3)

    oris = orientation_frames(angles)
    frames = attach_frames(oris, curve)

    # tansform frames - paper -> robot
    trajectory = attach_to_base_frame(wobj, *frames)
    result = inverse_kinematics_curve(trajectory)
    path = find_single_path(result)
    print path

    for count in xrange(0,50,11):
        print count
        pl = StPlot()
        robot_info = forward_kinematics(*path[count], **dh_table)
        pl.draw_robot(robot_info['robot_geometry_global'])
        pl.draw_trajectory(trajectory)
        pl.draw_tool(robot_info['flange'],
                           dh_table['tool'])
        pl.show()
    pl.draw_joint_paths(path)
Esempio n. 15
0
        j3 = rand_range(-230, 50)
        j4 = rand_range(-200, 200)
        j5 = rand_range(-115, 115)
        j6 = rand_range(-400, 400)

        j1 = 10
        j2 = 20
        j3 = 30
        j4 = 40
        j5 = 50
        j6 = 60

        joint_values = j1, j2, j3, j4, j5, j6

        # get forward kinematics i.e. last global robot-frame
        robot_info = forward_kinematics(*joint_values, **DH_TABLE)
        T44 = robot_info['tcp']
        IK_angles = inverse_kinematics_irb140(DH_TABLE, T44)

        # sanity check of forward kinematics
        for angles in IK_angles.T:
            info = forward_kinematics(*joint_values, **DH_TABLE)
            t44 = info['tcp']
            assert (norm(T44 - t44) < 1e-7)

        # list of global-robot-frames
        global_robot_frames = construct_robot_geometry(
            info['robot_geometry_local'])

        # generate a curve in the last global robot-frame
        num_p = 50
 def gen_robot_flanges(self):
     robot_infos = map(lambda x: forward_kinematics(*x, **dh_table),
                       self.robot_joints)
     flange = mat([x['flange'] for x in robot_infos])
     flange[:, :3, 3] = flange[:, :3, 3] * 1000  #convert to mm
     return flange