阻抗控制

1、关节空间阻抗控制器

#robot:机器人计算的KDL的类  sim:mujoco初始化的仿真环境  k: 刚度 d:阻尼  desired_pos:期望的位置  desired_ori:期望的姿态  tau_last：传入一个力矩 
def torque_joint(robot, sim, k, d, desired_pos, desired_ori, tau_last):
    q = np.array(sim.data.qpos[:])#当前关节的位置
    qd = np.array(sim.data.qvel[:])#速度
    q_target = ik(robot, q, desired_pos, desired_ori)#计算当前关节的目标位置
    M = robot.mass_matrix()#机器人的质量矩阵
    ok = False
    tau = tau_last
    if len(q_target) > 0:
        #robot的关节空间控制的计算公式（multiply等同于向量相乘）
        tau = np.multiply(k, q_target - q) - np.multiply(d, qd)         
        tau = np.dot(M, tau)#乘上质量矩阵会更稳定一些
        tau += robot.coriolis_gravity()#加上科氏力和重力矩
        ok = True #标记用于判断解算是否成功
    return tau, ok

2、笛卡尔空间阻抗控制器

#robot:机器人计算的KDL的类  sim:mujoco初始化的仿真环境  k: 刚度 d:阻尼  desired_pos:期望的位置  desired_ori:期望的姿态  tau_last：传入一个力矩 eef_nam：对应机器人末端位置的site名称
def torque_cartesian(robot, sim, k, d, eef_name, desired_pos, desired_ori):
    M = robot.mass_matrix()
    qd = np.array(sim.data.qvel[:])
    J = robot.jacobian()
    J_inv = np.linalg.inv(J)#雅各比矩阵的逆
    Jd = robot.jacobian_dot()#雅各比矩阵的微分
    Md = np.dot(J_inv.T, np.dot(M, J_inv))#目标质量矩阵，在讲解里边
    tau = sim.data.qfrc_bias[:]
    #获取末端的位置/姿态/速度/
    x_pos = np.array(sim.data.get_site_xpos(eef_name))
    x_ori = np.array(sim.data.site_xmat[sim.model.site_name2id(eef_name)].reshape([3, 3]))
    x_pos_vel = np.array(sim.data.site_xvelp[sim.model.site_name2id(eef_name)])
    x_ori_vel = np.array(sim.data.site_xvelr[sim.model.site_name2id(eef_name)])

    coef = np.dot(M, J_inv)
    xd_error = np.concatenate([-x_pos_vel, -x_ori_vel])#末端姿态和位置的拼接
    sum = np.multiply(d, xd_error)
    pos_error = desired_pos - x_pos#位置偏差
    ori_error = orientation_error(desired_ori, x_ori)#姿态偏差
    x_error = np.concatenate([pos_error, ori_error])#两者拼接

    sum += np.multiply(k, x_error)
    sum -= np.dot(np.dot(Md, Jd), qd)
    tau += np.dot(coef, sum)

    return tau

rebortsuit的阻抗控制：
1、支持的阻抗模型Supported modes: {'force', 'variable_kp', 'tracking', 'variable', 'fixed', 'variable_z'}
2、mujoco训练模型的存储路径：

save_model_path:  src/trained_rl_models/test
save_vecnormalize_path:  src/trained_rl_models/vec_normalize_test.pkl
load_model_path:  src/trained_rl_models/tracking
load_vecnormalize_path:  src/trained_rl_models/vec_normalize_tracking.pkl
Before Pipe

Creating window glfw
Creating window glfw