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]
