👨?🎓 作者簡介：大家好，我是可可卷，歡迎大家關注，一起學習交流 ~
📜主攻領域：【python演算法】【資料分析】【數學建模】【機器學習】【深度學習】【資料可視化】
📖個人主頁：可可卷的博客

0 寫在前面

🔥多圖預警，請連WIFI！
全文圖片較多，建議大家先收藏再閱讀哦~

1 必要準備

安裝依賴：

numpy

matplotlib

嘉?然：

2 生成愛?心軌跡

計算愛?心坐標

# 0.01為步長，步長越小，愛心曲線越逼真
x_ = np.arange(-1.15,1.16,0.01).astype(np.complex64)
y1 = (1/2*((x_**2)**(1/3)+((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real
y2 = (1/2*((x_**2)**(1/3)-((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real

畫出愛?心曲線

plt.plot(x_,y1)	# 愛心上半部分
plt.plot(x_,y2)	# 愛心下半部分
plt.show()

過濾煩人的警告

# 過濾煩人的warnings
import warnings
warnings.filterwarnings('ignore')

設定愛心為粉色

plt.plot(x_,y1,c='pink')
plt.plot(x_,y2,c='pink')

效果如下

3 放入嘉?然

ax=plt.subplot(111)
ax.plot(x_,y1)
ax.plot(x_,y2)
img=plt.imread('Diana.jpg')
im = OffsetImage(img,zoom=.520)
ab = AnnotationBbox(im, (0.520, 0.520), xycoords='axes fraction')
ax.add_artist(ab)
ax.figure.savefig("Diana.svg", transparent=True, dpi=600, bbox_inches="tight")
plt.show()

效果如下

眼尖的鼠鼠可能已經發現了，為什么

im = OffsetImage(img,zoom=.520)
ab = AnnotationBbox(im, (0.520, 0.520), xycoords='axes fraction')

引數里會出現520呢？

因為這是?的坐標~

到目前為止，全部的代碼如下

import numpy as np
from matplotlib import pyplot as plt
from matplotlib.offsetbox import OffsetImage, AnnotationBbox

# 過濾煩人的warnings
import warnings
warnings.filterwarnings('ignore')

x_ = np.arange(-1.15,1.16,0.01).astype(np.complex64)
y1 = (1/2*((x_**2)**(1/3)+((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real
y2 = (1/2*((x_**2)**(1/3)-((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real

ax=plt.subplot(111)
ax.plot(x_,y1,c='pink')
ax.plot(x_,y2,c='pink')
img=plt.imread('Diana.jpg')
im = OffsetImage(img,zoom=.520)
ab = AnnotationBbox(im, (0.520, 0.520), xycoords='axes fraction')
ax.add_artist(ab)
ax.figure.savefig("Diana.svg", transparent=True, dpi=600, bbox_inches="tight")
plt.show()

4 機械臂的概覽

4.0 必要準備

import numpy as np
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
from numpy import cos, sin, arccos, arctan2
import matplotlib.pyplot as plt

# 過濾煩人的warnings
import warnings
warnings.filterwarnings('ignore')

# 目標點
(target_x, target_y) = (0, 0)

# 愛心的x,y坐標
x_ = np.arange(-1.15,1.16,0.05).astype(np.complex64)
y1 = (1/2*((x_**2)**(1/3)+((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real
y2 = (1/2*((x_**2)**(1/3)-((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real

# 用于坐標迭代的生成器
sx=(float(x) for x in x_)
sy=(float(y) for y in y1)
flag=False
forever=True

# 記錄機械臂arm2的坐標
dx,dy=[],[]

4.1 創建物件

# 創建物件,嘉然小姐~
class Diana:
    def __init__(self):
       pass

    def update_arms(self, angles):
        pass

    # 前向運動
    def forward_kinematics(self):
        pass

    # 反解位姿
    def inverse_kinematic(self, x, y):
       pass
    
    def plot(self):
        pass

    def animation(self, x, y):
        pass

    def 畫愛心(self):
        pass

4.2 來到天堂

def Heaven():
    嘉然 = Diana()
    while forever:
        嘉然.畫愛心()

if __name__ == "__main__":
    Heaven()

5 機械臂的細節

5.0 初始化物件

def __init__(self, angles=[0, 0]):
    # 第0個關節固定坐標
    self.arm0 = np.array([-0.1314, -0.520])
    # 兩段機械臂的長度
    self.link_lengths = [1, 1]
    # 機械臂初始角度
    self.update_arms(angles)

這時又會有眼尖的鼠鼠提問了，老師老師，為什么arm0的坐標是[-0.1314, -0.520]呢?

別問，問就是?的坐標

5.1 更新機械臂

def update_arms(self, angles):
    self.arm_angles = angles
    self.forward_kinematics()

5.2 前向運動

# 前向運動
def forward_kinematics(self):
    # 計算關節1的位置(a0,a1分別是第0和第1個關節的關節角)
    a0 = self.arm_angles[0]
    l0 = self.link_lengths[0]
    self.arm1 = self.arm0 + [l0 * cos(a0), l0 * sin(a0)]

    # 計算關節2的位置
    a1 = self.arm_angles[1]
    l1 = self.link_lengths[1]
    self.arm2 = self.arm1 + [l1 * cos(a0 + a1), l1 * sin(a0 + a1)]

5.3 逆向運動

# 反解位姿
def inverse_kinematic(self, x, y):
    l0 = self.link_lengths[0]
    l1 = self.link_lengths[1]
    a1 = arccos((x ** 2 + y ** 2 - l0 ** 2 - l1 ** 2) / (2 * l0 * l1))
    a0 = arctan2(y, x) - arctan2(l1 * sin(a1), l1 * cos(a1) + l0)
    return [a0, a1]

5.4 軌跡作圖

def plot(self):
    # 清理坐標系中的內容
    plt.cla()

    # 顯示嘉然小姐~
    ax = plt.subplot(111)
    ax.plot(dx[5:],dy[5:],zorder=5,c='pink')
    img = plt.imread('Diana.jpg')
    im = OffsetImage(img, zoom=0.520,zorder=0)
    ab = AnnotationBbox(im, (0.5, 0.520), xycoords='axes fraction',frameon=False)
    ax.add_artist(ab)

    # 三個關節的坐標
    x = [self.arm0[0], self.arm1[0], self.arm2[0]+0.1314]
    y = [self.arm0[1], self.arm1[1], self.arm2[1]+0.520]

    # 記錄愛心坐標
    if len(dx)<520:
        dx.append(x[-1])
        dy.append(y[-1])

    # magic point
    if dx[-1]==0.9761696712838817:
        dx[-1]*=-1
        dy[-1]=-0.04573089606848885


    # 繪制機械臂
    ax.plot(x, y, c="red", zorder=4)

    # 繪制機械臂的關節
    ax.scatter(x[1:], y[1:], c="black", marker='*',zorder=5)

    # 固定坐標系
    plt.xlim(-1.314, 1.314)
    plt.ylim(-1.314, 1.314)

讓我們來統計一下，這里面包含了多大的?~

這是巧合嗎？

這是?捏~

5.5 繪制動作

def animation(self, x, y):
    angles = self.inverse_kinematic(x, y)

    # 分解N步慢動作
    actions_num = 2
    angles_per_action = (np.array(angles) - np.array(self.arm_angles)) / actions_num
    for action_i in range(actions_num):
        self.arm_angles = np.array(self.arm_angles) + angles_per_action
        self.update_arms(self.arm_angles)
        self.plot()
        plt.pause(0.001)    # 暫停的時間

5.6 畫愛?心

def 畫愛心(self):
    global target_x, target_y
    global sx,sy,flag
    try:
        target_x = next(sx)
        target_y = next(sy)
        self.animation(target_x, target_y)
    except:
        if flag:
            sx = (float(x) for x in x_)
            sy = (float(y) for y in y1)
            flag=False
        else:
            sx = (float(x) for x in reversed(x_))
            sy = (float(y) for y in y2)
            flag=True

6 全部代碼

import numpy as np
from matplotlib.offsetbox import OffsetImage, AnnotationBbox
from numpy import cos, sin, arccos, arctan2
import matplotlib.pyplot as plt

# 過濾煩人的warnings
import warnings
warnings.filterwarnings('ignore')

# 目標點
(target_x, target_y) = (0, 0)

# 愛心的x,y坐標
x_ = np.arange(-1.15,1.16,0.05).astype(np.complex64)
y1 = (1/2*((x_**2)**(1/3)+((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real
y2 = (1/2*((x_**2)**(1/3)-((x_**4)**(1/3)-4*x_**2+4)**(1/2))).real

# 用于坐標迭代的生成器
sx=(float(x) for x in x_)
sy=(float(y) for y in y1)
flag=False
forever=True

# 記錄機械臂arm2的坐標
dx,dy=[],[]

# 嘉然小姐~
class Diana:
    def __init__(self, angles=[0, 0]):
        # 第0個關節固定坐標
        self.arm0 = np.array([-0.1314, -0.520])
        # 兩段機械臂的長度
        self.link_lengths = [1, 1]
        # 機械臂初始角度
        self.update_arms(angles)

    def update_arms(self, angles):
        self.arm_angles = angles
        self.forward_kinematics()

    # 前向運動
    def forward_kinematics(self):
        # 計算關節1的位置(a0,a1分別是第0和第1個關節的關節角)
        a0 = self.arm_angles[0]
        l0 = self.link_lengths[0]
        self.arm1 = self.arm0 + [l0 * cos(a0), l0 * sin(a0)]

        # 計算關節2的位置
        a1 = self.arm_angles[1]
        l1 = self.link_lengths[1]
        self.arm2 = self.arm1 + [l1 * cos(a0 + a1), l1 * sin(a0 + a1)]

    # 反解位姿
    def inverse_kinematic(self, x, y):
        l0 = self.link_lengths[0]
        l1 = self.link_lengths[1]
        a1 = arccos((x ** 2 + y ** 2 - l0 ** 2 - l1 ** 2) / (2 * l0 * l1))
        a0 = arctan2(y, x) - arctan2(l1 * sin(a1), l1 * cos(a1) + l0)
        return [a0, a1]

    def plot(self):
        # 清理坐標系中的內容
        plt.cla()

        # 顯示嘉然小姐~
        ax = plt.subplot(111)
        ax.plot(dx[5:],dy[5:],zorder=5,c='pink')
        img = plt.imread('Diana.jpg')
        im = OffsetImage(img, zoom=0.520,zorder=0)
        ab = AnnotationBbox(im, (0.5, 0.520), xycoords='axes fraction',frameon=False)
        ax.add_artist(ab)

        # 三個關節的坐標
        x = [self.arm0[0], self.arm1[0], self.arm2[0]+0.1314]
        y = [self.arm0[1], self.arm1[1], self.arm2[1]+0.520]

        # 記錄愛心坐標
        if len(dx)<520:
            dx.append(x[-1])
            dy.append(y[-1])

        # magic point
        if dx[-1]==0.9761696712838817:
            dx[-1]*=-1
            dy[-1]=-0.04573089606848885


        # 繪制機械臂
        ax.plot(x, y, c="red", zorder=4)

        # 繪制機械臂的關節
        ax.scatter(x[1:], y[1:], c="black", marker='*',zorder=5)

        # 固定坐標系
        plt.xlim(-1.314, 1.314)
        plt.ylim(-1.314, 1.314)

    def animation(self, x, y):
        angles = self.inverse_kinematic(x, y)

        # 分解N步慢動作
        actions_num = 2
        angles_per_action = (np.array(angles) - np.array(self.arm_angles)) / actions_num
        for action_i in range(actions_num):
            self.arm_angles = np.array(self.arm_angles) + angles_per_action
            self.update_arms(self.arm_angles)
            self.plot()
            plt.pause(0.001)    # 暫停的時間

    def 畫愛心(self):
        global target_x, target_y
        global sx,sy,flag
        try:
            target_x = next(sx)
            target_y = next(sy)
            self.animation(target_x, target_y)
        except:
            if flag:
                sx = (float(x) for x in x_)
                sy = (float(y) for y in y1)
                flag=False
            else:
                sx = (float(x) for x in reversed(x_))
                sy = (float(y) for y in y2)
                flag=True



def Heaven():
    嘉然 = Diana()
    while forever:
        嘉然.畫愛心()



if __name__ == "__main__":
    Heaven()

效果如下

7 全是細節

7.0 好學的嘉心糖

作為一個好學的嘉心糖，在寫完代碼后自然要好好總結捏~

7.1 中文變數

python3是支持中文作為變數的哦，而且不需要引入任何庫和第三方工具

大家可以測驗以下代碼，試試中文變數名的使用效果

love=True
嘉然=True
print(嘉然 is love)

''' 我的測驗結果 '''
>>> love=True
>>> 嘉然=True
>>> 嘉然 is love
True

7.2 坐標迭代

1. 因為我采用了分別畫出上、下部分愛心再結合的方式，因此在坐標迭代部分需要通過flag對上、下部分愛心做一個標記

2. 在具體迭代上，通過(float(x) for x in x_)構造生成器，通過next()取出生成器的下一個元素

   當元素取盡時會拋出StopIteration例外，這時通過except重新構造新的生成器

3. 大家可以通過下面的測驗代碼來體會生成器的用法~

   >>> x=[1,2,3]
   >>> y=(i for i in x)
   >>> next(y)
   1
   >>> next(y)
   2
   >>> next(y)
   3
   >>> next(y)
   Traceback (most recent call last):
     File "<pyshell#7>", line 1, in <module>
       next(y)
   StopIteration
   >>> 
   

7.3 zorder

matplotlib中軸的默認繪制順序是補丁，線條，文本， 此順序由zorder屬性確定， 設定以下默認值

類比PS，zorder可以看成一個控制圖層上下順序的引數

更多詳細用法，可以查看官方檔案：https://www.matplotlib.org.cn/gallery/misc/zorder_demo.html

8 寫在后面

1. 本文愛心曲線參考了@FrigidWinter大佬的博客📚工程師的浪漫：用機械臂畫一個愛心

2. 同時本文機械臂的逆運動學實作部分參考了GitHub專案：https://github.com/varyshare/easy_slam_tutorial/tree/master/joint_robot_simulation

3. 更多機器人相關課程，詳見：
   https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-geometry/

9 偷偷的說

如果把嘉然偷偷換成海子姐，會怎么樣呢QAQ