numpy, matplotlibを使った一番簡単な順運動学の例

kinematics_basic.py

# -*- coding: utf-8 -*-
"""
Created on Sun Oct  8 17:42:55 2017

@author: ryuhei
"""

import numpy as np
import matplotlib.pyplot as plt


if __name__ == '__main__':
    # Set model parameters
    link_0 = 1
    link_1 = 2
    theta_0_d = 30  # in degree
    theta_1_d = 60  # in degree

    # Compute forward kinematics
    theta_0 = np.deg2rad(theta_0_d)  # convert angles from degree to radian
    theta_1 = np.deg2rad(theta_1_d)
    p0 = np.array([0, 0])
    p1 = link_0 * np.array([np.cos(theta_0), np.sin(theta_0)])
    p2 = p1 + link_1 * np.array(
        [np.cos(theta_0 + theta_1), np.sin(theta_0 + theta_1)])

    # Show
    print('link_0 =', link_0)
    print('link_1 =', link_1)
    print('theta_0 =', theta_0_d)
    print('theta_1 =', theta_1_d)
    print('p1 =', p1)
    print('p2 =', p2)

    plt.plot(p0[0], p0[1], 'r.')
    plt.plot(p1[0], p1[1], 'r.')
    plt.plot(p2[0], p2[1], 'r.')
    plt.plot([p0[0], p1[0], p2[0]], [p0[1], p1[1], p2[1]])
    max_len = link_0 + link_1
    plt.xlim(-max_len, max_len)
    plt.ylim(-max_len, max_len)
    plt.gca().set_aspect('equal')
    plt.grid()
    plt.show()