【MUJOCO控制篇-2】控制一个简单的机械臂

最新推荐文章于 2024-09-14 17:27:59 发布

三丿十

最新推荐文章于 2024-09-14 17:27:59 发布

阅读量2.7k

点赞数 8

文章标签： xml 学习 python

本文链接：https://blog.csdn.net/echo_Fangyb/article/details/136663742

版权

【MUJOCO控制篇-2】控制一个简单的机械臂

本篇博客将逐步分析和解释如何通过Python代码使用MuJoCo库来控制一个机械臂模型。我们将从加载模型开始，逐步深入到PID控制器的实现，最后讨论如何实现轨迹跟踪功能。上期内容为如何建模一个简单的机械臂模型，本篇文章将基于上篇内容进行深入探讨如何实现一个简单的控制。

基础设置与模型加载

首先，需要导入必要的库，并初始化两个关节的长度。然后加载MuJoCo模型，并创建模拟器（MjSim）和视图器（MjViewer）对象。

import math
import time
import mujoco_py
from mujoco_py import load_model_from_path, MjSim, MjViewer
import glfw  # 用于检查窗口关闭事件
import os

L1 = 0.7
L2 = 0.5

# 加载模型
model_path = '/home/fang/xml_demo/arm.xml'  # 更新为你的模型文件路径
model = load_model_from_path(model_path)
sim = MjSim(model)
viewer = MjViewer(sim)

model_path 是模型文件的路径。
MjSim 创建一个模拟实例。
MjViewer 用于渲染和查看模拟。

PID控制器的实现

接下来，定义了一个 PIDController 类来实现PID控制逻辑。该控制器根据目标角度和当前角度来计算所需的扭矩。

# 定义PID控制器类
class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.integral_error = 0
        self.prev_error = 0
        self.dt = 0.01  # 控制周期，需要与模拟步长相匹配

    def calculate(self, target, current):
        error = target - current
        self.integral_error += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        output = (self.Kp * error) + (self.Ki * self.integral_error) + (self.Kd * derivative)
        self.prev_error = error
        return output

Kp, Ki, Kd 是PID控制器的比例、积分和微分系数。
calculate 方法计算并返回控制输出。

逆运动学的计算

此处定义了一个 inverse_kinematics 函数来计算给定末端点位置时两个关节的角度。

def inverse_kinematics(x, z, L1, L2, base_height=1.2):
    """
    根据末端点坐标在X-Z平面上计算两个关节的角度（逆解算）。
    
    参数:
    - x: 末端点的X坐标。
    - z: 末端点的Z坐标。
    - L1: 第一节长度。
    - L2: 第二节长度。
    - base_height: 基座的高度。
    
    返回:
    - theta1: 第一个关节的角度（弧度）。
    - theta2: 第二个关节的角度（弧度）。
    """
    # 计算末端点到基座的相对高度
    z_rel = z - base_height
    D = math.sqrt(x**2 + z_rel**2)  # 末端点到原点的距离在X-Z平面上计算
    
    # 使用余弦定理计算第二个关节的角度
    cos_theta2 = (D**2 - L1**2 - L2**2) / (2 * L1 * L2)
    theta2 = math.acos(min(1, max(cos_theta2, -1)))  # 确保acos的参数在合法范围内
    
    # 计算第一个关节的角度
    sin_theta2 = math.sqrt(1 - cos_theta2**2)
    theta1 = math.atan2(z_rel, x) - math.atan2(L2 * sin_theta2, L1 + L2 * cos_theta2)

    # print('D=',D,'theta1=',theta1,'theta2=',theta2)

    return theta1, theta2

这个函数通过数学计算得出每个关节应达到的角度，以便机械臂的末端达到目标位置。

轨迹跟踪

轨迹跟踪功能通过 trajectory_tracking 函数实现，它使用 segment_trajectory 函数来逐段处理轨迹，每一段都使用三次插值来平滑运动。插值法可以在别的博客中找到具体的算法与解释，此处不再做过多赘述。

def trajectory_tracking(points, duration):
    """
    轨迹跟踪函数，控制机械臂依次沿多个点移动。
    points: 点列表，每个点为(x, z)格式。
    duration: 每段轨迹的持续时间。
    """
    for i in range(len(points) - 1):
        start_pos = points[i]
        end_pos = points[i + 1]
        segment_trajectory(start_pos, end_pos, duration)

points 是机械臂应通过的点列表。
duration 是每段轨迹的持续时间。

逻辑部分

综合以上函数代码，结合解算逻辑，包括UI显示等功能以及PID控制器功能，实现总体的控制效果。

def sim_run(target_x, target_z):
    while not glfw.window_should_close(viewer.window):
        theta1, theta2 = inverse_kinematics(target_x, target_z, L1, L2)
        joint_targets = {'joint1': -(math.pi/2 + theta1), 'joint2': theta2}
        for joint_name, target_angle in joint_targets.items():
            pid_control(joint_name, target_angle)
        sim.step()
        viewer.render()
        time.sleep(0.01)

主程序

最后，在主程序中，通过设置机械臂需要通过的点列表和每段轨迹的持续时间，然后调用 trajectory_tracking 函数来执行轨迹跟踪。

if __name__ == "__main__":  

    # 设置通过的点列表和每段轨迹的持续时间
    points = [(0.0,0.0),(1.2, 1.2),(0.0, 2.4),(-1.2, 1.2)]
    duration = 5  # 每段持续3秒
    trajectory_tracking(points, duration)