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

视频讲解：

代码仓库：GitHub - LitchiCheng/mujoco-learning

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

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 itertoolsfrom 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 = pathdef runBefore(self):# Create models and dataself.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 solveroptions = 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.pirotation_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 pathoptions = 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...")continueelse:print("Failed to plan.")# Perform trajectory optimization.# self.model.opt.timestep = 0.1dt = self.model.opt.timestepoptions = 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))breakself.index = 0self.last_qpos = self.data.qpos[:7]def plot(self, tfs):positions = []for tform in tfs:position = tform.translationpositions.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_statetarget_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_soldef 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.timestepself.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 += 1if self.index >= self.q_vec.shape[1]:self.index = 0time_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()