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):
"""
使用牛顿法求解多变量方程组的根
参数:
f: 目标函数，输入一个长度为n的向量并返回一个长度为n的向量的函数
df: 目标函数的雅可比矩阵（各个分量对各个变量的偏导数），输入一个长度为n的向量并返回一个n x n的矩阵的函数
initial_guess: 初始猜测值，一个长度为n的向量
tol: 允许的误差阈值
max_iter: 最大迭代次数
返回:
root: 方程组的近似根，一个长度为n的向量
iterations: 迭代次数
"""
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,"迭代值次数:",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"迭代次数: {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]
