Mujoco 机械臂进行 PBVS 基于位置的视觉伺服思路
视频讲解：https://www.bilibili.com/video/BV18zC5BNEt6/?vd_source=5ba34935b7845cd15c65ef62c64ba82f
代码仓库：GitHub - LitchiCheng/mujoco-learning
在机械臂抓取等任务中，输入通常为被抓物体的图像（VLA \ RL \ IL）、特征（IBVS）、位姿信息（PBVS）等等不同类型，接下来介绍下 PBVS ，该方式可以结合一些识别模型，如FoundationPose，CenterGrasp，GraspNet 等网络提供位姿，机械臂控制末端 ee 进行传统运动规划即可，分成两个模块。
实验基于 Mujoco，这里识别位姿为理想值（Mujoco world 坐标系下），仅进行 PBVS 的机械臂控制思路。
PBVS 控制核心思路为，通过末端和目标点的位姿误差进行 kp 控制，输出为末端速度，再通过雅可比伪逆转换为关节速度，关节不支持速度控制时也可以积分成位置进行控制。代码如下：
def dampedPinv(self, J, lambda_d=0.1):J_T = J.T# 阻尼项：λ²·I₆（6×6单位矩阵）damping = lambda_d ** 2 * np.eye(J.shape[0])# 计算 (J·Jᵀ + λ²I)⁻¹·JᵀJ_pinv_damped = np.dot(J_T, np.linalg.inv(np.dot(J, J_T) + damping))return J_pinv_dampeddef runFunc(self):np.set_printoptions(precision=3)pinocchio.forwardKinematics(self.urdf_model, self.urdf_data, self.data.qpos[:7])pinocchio.updateFramePlacements(self.urdf_model, self.urdf_data)J = pinocchio.computeFrameJacobian(self.urdf_model, self.urdf_data, self.data.qpos[:7], self.pin_id, pinocchio.ReferenceFrame.WORLD)ee_pos = self.data.body(self.end_effector_id).xpos.copy()ee_quat = self.data.body(self.end_effector_id).xquat.copy()# 线速度控制k_p_lin = 5.0x = 0.2y = 0.2z = 0.6p_des = np.array([x, y, z])e_p = ee_pos - p_desv_des_lin = -k_p_lin * e_p# 旋转速度控制k_p_rot = 3.0R_ee = utils.quat2rotmat(ee_quat)roll = np.pipitch = 0.0yaw = 0.0R_des = utils.euler2rotmat(roll, pitch, yaw)# 姿态误差e_rot = 0.5 * (np.cross(R_des[:, 0], R_ee[:, 0]) +np.cross(R_des[:, 1], R_ee[:, 1]) +np.cross(R_des[:, 2], R_ee[:, 2]))v_des_rot = -k_p_rot * e_rot# 最终期望末端速度（6维）v_e_desired = np.concatenate([v_des_lin, v_des_rot])# v_e_desired = np.concatenate([v_des_lin, [0, 0, 0]])# v_e_desired = np.concatenate([[0, 0, 0], v_des_rot])J_pinv_damped = self.dampedPinv(J, lambda_d=0.1)q_dot_core = J_pinv_damped @ v_e_desiredq_dot_max = 3.14q_dot_des = np.clip(q_dot_core, -q_dot_max, q_dot_max) # 限幅后关节速度self.integral_qpos[:7] += q_dot_des * self.model.opt.timestep # 更新关节位置可以调整的控制量有响应速度（k_p_lin，k_p_rot），控制精度（lambda_d），完整代码如下
import pinocchio
import mujoco_viewer
import time
import numpy as np
import mujoco
import utilsclass PandaPbvs(mujoco_viewer.CustomViewer):def __init__(self, scene_xml, pin_xml):super().__init__(scene_xml, 3, azimuth=-45, elevation=-30)self.scene_xml = scene_xmlself.pin_xml = pin_xmldef runBefore(self):self.model.opt.timestep = 0.02self.pin_model = pinocchio.RobotWrapper.BuildFromMJCF(self.pin_xml)self.urdf_model = self.pin_model.model# 删除8,9夹抓部分reduce_joint_ids = [8,9]q0 = pinocchio.neutral(self.urdf_model)self.urdf_model = pinocchio.buildReducedModel(self.urdf_model, reduce_joint_ids,q0, )# 简化模型的数据对象self.urdf_data = self.urdf_model.createData()BODY_NAME = "ee_center_body"self.pin_id = self.urdf_model.getFrameId(BODY_NAME) self.end_effector_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, BODY_NAME)# 设定初始位置self.initial_pos = self.model.key_qpos[0] print("Initial position: ", self.initial_pos)for i in range(self.model.nq):self.data.qpos[i] = self.initial_pos[i]# 记录上一步速度 self.prev_joint_vel = np.zeros(9, dtype=np.float32) self.keep_pos = Falseself.integral_qpos = self.data.qpos.copy()def dampedPinv(self, J, lambda_d=0.1):J_T = J.T# 阻尼项：λ²·I₆（6×6单位矩阵）damping = lambda_d ** 2 * np.eye(J.shape[0])# 计算 (J·Jᵀ + λ²I)⁻¹·JᵀJ_pinv_damped = np.dot(J_T, np.linalg.inv(np.dot(J, J_T) + damping))return J_pinv_dampeddef runFunc(self):np.set_printoptions(precision=3)pinocchio.forwardKinematics(self.urdf_model, self.urdf_data, self.data.qpos[:7])pinocchio.updateFramePlacements(self.urdf_model, self.urdf_data)J = pinocchio.computeFrameJacobian(self.urdf_model, self.urdf_data, self.data.qpos[:7], self.pin_id, pinocchio.ReferenceFrame.WORLD)ee_pos = self.data.body(self.end_effector_id).xpos.copy()ee_quat = self.data.body(self.end_effector_id).xquat.copy()# 线速度控制k_p_lin = 5.0x = 0.5y = 0.0z = 0.6p_des = np.array([x, y, z])e_p = ee_pos - p_desv_des_lin = -k_p_lin * e_p# 旋转速度控制k_p_rot = 3.0R_ee = utils.quat2rotmat(ee_quat)roll = np.pipitch = 0.0yaw = 0.0R_des = utils.euler2rotmat(roll, pitch, yaw)# 姿态误差e_rot = 0.5 * (np.cross(R_des[:, 0], R_ee[:, 0]) +np.cross(R_des[:, 1], R_ee[:, 1]) +np.cross(R_des[:, 2], R_ee[:, 2]))v_des_rot = -k_p_rot * e_rot# 最终期望末端速度（6维）v_e_desired = np.concatenate([v_des_lin, v_des_rot])# v_e_desired = np.concatenate([v_des_lin, [0, 0, 0]])# v_e_desired = np.concatenate([[0, 0, 0], v_des_rot])J_pinv_damped = self.dampedPinv(J, lambda_d=0.01)q_dot_core = J_pinv_damped @ v_e_desiredq_dot_max = 3.14q_dot_des = np.clip(q_dot_core, -q_dot_max, q_dot_max) # 限幅后关节速度self.integral_qpos[:7] += q_dot_des * self.model.opt.timestep # 更新关节位置print(f"{e_p.round(3)}, {e_rot.round(3)}")if np.linalg.norm(e_p) < 1e-3:self.keep_pos = Trueelif not self.keep_pos:placeholder = 0# self.data.ctrl[:7] = self.integral_qpos[:7]self.data.ctrl[:7] = q_dot_des[:7]if __name__ == "__main__":CONTROLER = "vel"scene_xml_path = "model/franka_emika_panda/scene_"+ CONTROLER + ".xml"pin_xml_path = "model/franka_emika_panda/panda_"+ CONTROLER + ".xml"env = PandaPbvs(scene_xml_path, pin_xml_path)env.run_loop()
