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):
"""
使用牛顿法求解多变量方程组的根

    参数:
    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]