Chapter 6, Inverse Kinematics

Use Newton-Raphson iterative numerical root finding to perform two steps of finding the root of

def newton_method_system(f, df, initial_guess, tol=1e-6, max_iter=100):
"""
使用牛頓法求解多變量方程組的根

    參數(shù):
    f: 目標(biāo)函數(shù),輸入一個長度為n的向量并返回一個長度為n的向量的函數(shù)
    df: 目標(biāo)函數(shù)的雅可比矩陣(各個分量對各個變量的偏導(dǎo)數(shù)),輸入一個長度為n的向量并返回一個n x n的矩陣的函數(shù)
    initial_guess: 初始猜測值,一個長度為n的向量
    tol: 允許的誤差閾值
    max_iter: 最大迭代次數(shù)

    返回:
    root: 方程組的近似根,一個長度為n的向量
    iterations: 迭代次數(shù)
    """
    x = np.array(initial_guess)
    iterations = 0

    while np.linalg.norm(f(x)) > tol and iterations < max_iter:
        delta_x = np.linalg.solve(df(x), -f(x))
        x += delta_x
        iterations += 1
        print("迭代值",x,"迭代值次數(shù):",iterations)

    return x, iterations


# 示例:使用牛頓法求解方程組{x^2 - 9, y^2 - 4}的根,初始猜測值為(1, 1)
def target_function(x):
    return np.array([x[0] ** 2 - 9, x[1] ** 2 - 4])


def jacobian_matrix(x):
    return np.array([[2 * x[0], 0], [0, 2 * x[1]]])


initial_guess = [1.0, 1.0]  # 初始猜測值
root, iterations = newton_method_system(target_function, jacobian_matrix, initial_guess)

print(f"近似根: {root}")
print(f"迭代次數(shù): {iterations}")

2.Referring to the figure above,the the joint angles
"""

import numpy as np
import modern_robotics as mr
if name == 'main':

M = np.array([[1,0,0,3],
[0,1,0,0],
[0,0,1,0],
[0,0,0,1]])
T = np.array([[-0.585,-0.811,0,0.076],
[0.811,-0.5850,0,2.608],
[0,0,1,0],
[0,0,0,1]])
Slist = np.array([[0,0,1,0,0,0],
[0,0,1,0,-1,0],
[0,0,1,0,-2,0]]).transpose()

initalGuess = np.array([np.pi/4,np.pi/4,np.pi/4])
eomg = 0.001
ev = 0.0001
res = mr.IKinSpace(Slist,M,T,initalGuess,eomg,ev)
print(res)

    """

答案:[0.92519754, 0.58622516, 0.68427316]

最后編輯于
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成,瀏覽時請結(jié)合常識與多方信息審慎甄別。
平臺聲明:文章內(nèi)容(如有圖片或視頻亦包括在內(nèi))由作者上傳并發(fā)布,文章內(nèi)容僅代表作者本人觀點,簡書系信息發(fā)布平臺,僅提供信息存儲服務(wù)。

相關(guān)閱讀更多精彩內(nèi)容

友情鏈接更多精彩內(nèi)容