MuJoCo关节角速度记录与可视化：监控机械臂运动状态

关节空间的轨迹优化，实际上是对于角速度起到加减速规划的控制，故一般来说具有该效果的速度变化会显得丝滑一些，不会那么生硬，这里我们结合评论区的疑问，将关节速度进行记录及可视化，可以比较直观的看到关节速度，下面通过两种方式计算关节速度：

1. 手动计算关节速度，其中timestep为仿真步长

calc_qvel = (self.last_qpos - self.q_vec[:7, self.index]) / self.model.opt.timestep

2. 使用data.qvel直接取得关节速度

self.joint_velocities[self.index] = self.data.qvel[:7]

两种方式计算出的关节速度基本一致，如下为完整代码：

import mujoco_viewer
import mujoco,time,threading
import numpy as np
import pinocchio
import matplotlib
# matplotlib.use('TkAgg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import itertools

from pyroboplan.core.utils import (
    get_random_collision_free_state,
    extract_cartesian_poses,
)
from pyroboplan.ik.differential_ik import DifferentialIk, DifferentialIkOptions
from pyroboplan.ik.nullspace_components import (
    joint_limit_nullspace_component,
    collision_avoidance_nullspace_component,
)

from pyroboplan.models.panda import (
    load_models,
    add_self_collisions,
    add_object_collisions,
)
from pyroboplan.planning.rrt import RRTPlanner, RRTPlannerOptions
from pyroboplan.trajectory.trajectory_optimization import (
    CubicTrajectoryOptimization,
    CubicTrajectoryOptimizationOptions,
)

class Test(mujoco_viewer.CustomViewer):
    def __init__(self, path):
        super().__init__(path, 3, azimuth=180, elevation=-30)
        self.path = path
    
    def runBefore(self):
        # Create models and data
        self.model_roboplan, self.collision_model, visual_model = load_models(use_sphere_collisions=True)
        add_self_collisions(self.model_roboplan, self.collision_model)
        add_object_collisions(self.model_roboplan, self.collision_model, visual_model, inflation_radius=0.1)

        data = self.model_roboplan.createData()
        collision_data = self.collision_model.createData()

        self.target_frame = "panda_hand"
        ignore_joint_indices = [
            self.model_roboplan.getJointId("panda_finger_joint1") - 1,
            self.model_roboplan.getJointId("panda_finger_joint2") - 1,
        ]
        np.set_printoptions(precision=3)

        # Set up the IK solver
        options = DifferentialIkOptions(
            max_iters=200,
            max_retries=10,
            damping=0.0001,
            min_step_size=0.05,
            max_step_size=0.1,
            ignore_joint_indices=ignore_joint_indices,
            rng_seed=None,
        )
        self.ik = DifferentialIk(
            self.model_roboplan,
            data=data,
            collision_model=self.collision_model,
            options=options,
            visualizer=None,
        )
        self.nullspace_components = [
            lambda model_roboplan, q: collision_avoidance_nullspace_component(
                model_roboplan,
                data,
                self.collision_model,
                collision_data,
                q,
                gain=1.0,
                dist_padding=0.05,
            ),
            lambda model_roboplan, q: joint_limit_nullspace_component(
                model_roboplan, q, gain=1.0, padding=0.025
            ),
        ]
        
        theta = np.pi
        rotation_matrix = np.array([
            [1, 0, 0],
            [0, np.cos(theta), -np.sin(theta)],
            [0, np.sin(theta), np.cos(theta)]
        ])
        
        q_start = self.getIk(self.random_valid_state(), rotation_matrix, [0.4, 0.0, 0.4])
        q_goal = self.getIk(self.random_valid_state(), rotation_matrix, [0.3, 0.0, 0.5])

        while True:
            # Search for a path
            options = RRTPlannerOptions(
                max_step_size=0.05,
                max_connection_dist=0.5,
                rrt_connect=False,
                bidirectional_rrt=False,
                rrt_star=True,
                max_rewire_dist=0.5,
                max_planning_time=5.0,
                fast_return=True,
                goal_biasing_probability=0.25,
                collision_distance_padding=0.0001,
            )
            print("")
            print(f"Planning a path...")
            planner = RRTPlanner(self.model_roboplan, self.collision_model, options=options)

            q_path = planner.plan(q_start, q_goal)
            print(f"Path found with {len(q_path)} waypoints")
            if q_path is not None and len(q_path) > 0:
                print(f"Got a path with {len(q_path)} waypoints")
                if len(q_path) > 50:
                    print("Path is too long, skipping...")
                    continue
            else:
                print("Failed to plan.")

            # Perform trajectory optimization.
            # self.model.opt.timestep = 0.1
            dt = self.model.opt.timestep
            options = CubicTrajectoryOptimizationOptions(
                num_waypoints=len(q_path),
                samples_per_segment=1,
                min_segment_time=0.5,
                max_segment_time=10.0,
                min_vel=-1.5,
                max_vel=1.5,
                min_accel=-0.75,
                max_accel=0.75,
                min_jerk=-1.0,
                max_jerk=1.0,
                max_planning_time=1.0,
                check_collisions=False,
                min_collision_dist=0.001,
                collision_influence_dist=0.05,
                collision_avoidance_cost_weight=0.0,
                collision_link_list=[
                    "obstacle_box_1",
                    "obstacle_box_2",
                    "obstacle_sphere_1",
                    "obstacle_sphere_2",
                    "ground_plane",
                    "panda_hand",
                ],
            )
            print("Optimizing the path...")
            optimizer = CubicTrajectoryOptimization(self.model_roboplan, self.collision_model, options)
            traj = optimizer.plan([q_path[0], q_path[-1]], init_path=q_path)

            if traj is None:
                print("Retrying with all the RRT waypoints...")
                traj = optimizer.plan(q_path, init_path=q_path)

            if traj is not None:
                print("Trajectory optimization successful")
                traj_gen = traj.generate(dt)
                self.q_vec = traj_gen[1]
                print(f"path has {self.q_vec.shape[1]} points")
                self.tforms = extract_cartesian_poses(self.model_roboplan, "panda_hand", self.q_vec.T)
                self.plot(self.tforms)
                self.joint_velocities = np.zeros((self.q_vec.shape[1], 7))
                break
        self.index = 0
        self.last_qpos = self.data.qpos[:7]

    def plot(self, tfs):
        positions = []
        for tform in tfs:
            position = tform.translation
            positions.append(position)
        positions = np.array(positions)
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')
        ax.plot(positions[:, 0], positions[:, 1], positions[:, 2], marker='o')

        ax.set_xlabel('X')
        ax.set_ylabel('Y')
        ax.set_zlabel('Z')

        plt.show(block=False)
        plt.pause(0.001)

    def getIk(self, init_state, rotation_matrix, pose):
        while True:
            self.init_state = init_state
            target_tform = pinocchio.SE3(rotation_matrix, np.array(pose))
            q_sol = self.ik.solve(
                self.target_frame,
                target_tform,
                init_state=self.init_state,
                nullspace_components=self.nullspace_components,
                verbose=True,
            )
            if q_sol is not None:
                print("IK solution found")
                return q_sol

    def random_valid_state(self):
        return get_random_collision_free_state(
            self.model_roboplan, self.collision_model, distance_padding=0.001
        )

    def runFunc(self):
        calc_qvel = (self.last_qpos - self.q_vec[:7, self.index]) / self.model.opt.timestep
        self.data.qpos[:7] = self.q_vec[:7, self.index]
        self.last_qpos = self.data.qpos[:7]   
        print(f"qpos:{self.data.qpos[:7]}")     
        print(f"qvel:{self.data.qvel[:7]}")
        print(f"calc_qvel:{calc_qvel}")
        print(self.model.nv, self.model.nq)
        self.joint_velocities[self.index] = self.data.qvel[:7]
        self.index += 1
        if self.index >= self.q_vec.shape[1]:
            self.index = 0
            time_steps = np.arange(self.q_vec.shape[1])
            fig, axes = plt.subplots(7, 1, figsize=(10, 10))
            for j in range(7):
                axes[j].plot(time_steps, self.joint_velocities[:, j], label=f'Joint {j + 1}', color=f'C{j}')
                axes[j].set_ylabel('Angular Velocity (rad/s)')
                axes[j].set_title(f'Joint {j + 1} Angular Velocity')
                axes[j].grid(True)
                axes[j].legend()
            plt.tight_layout()
            plt.show()

if __name__ == "__main__":
    test = Test("./franka_emika_panda/scene.xml")
    test.run_loop()

以上代码展示了如何使用MuJoCo进行关节角速度的记录与可视化，以监控机械臂的运动状态。通过手动计算和直接获取两种方式，可以验证关节速度数据的一致性，并利用Matplotlib进行可视化分析。

如需查看详细视频讲解、完整代码仓库及更多相关图片，请访问原帖子：MuJoCo 关节角速度记录与可视化，监控机械臂运动状态。

原帖子内容来源：https://bbs.eeworld.com.cn/thread-1313496-1-1.html