1歉甚、關(guān)節(jié)空間阻抗控制器
#robot:機器人計算的KDL的類 sim:mujoco初始化的仿真環(huán)境 k: 剛度 d:阻尼 desired_pos:期望的位置 desired_ori:期望的姿態(tài) tau_last：傳入一個力矩
def torque_joint(robot, sim, k, d, desired_pos, desired_ori, tau_last):
q = np.array(sim.data.qpos[:])#當前關(guān)節(jié)的位置
qd = np.array(sim.data.qvel[:])#速度
q_target = ik(robot, q, desired_pos, desired_ori)#計算當前關(guān)節(jié)的目標位置
M = robot.mass_matrix()#機器人的質(zhì)量矩陣
ok = False
tau = tau_last
if len(q_target) > 0:
#robot的關(guān)節(jié)空間控制的計算公式（multiply等同于向量相乘）
tau = np.multiply(k, q_target - q) - np.multiply(d, qd)
tau = np.dot(M, tau)#乘上質(zhì)量矩陣會更穩(wěn)定一些
tau += robot.coriolis_gravity()#加上科氏力和重力矩
ok = True #標記用于判斷解算是否成功
return tau, ok
2辩尊、笛卡爾空間阻抗控制器
#robot:機器人計算的KDL的類 sim:mujoco初始化的仿真環(huán)境 k: 剛度 d:阻尼 desired_pos:期望的位置 desired_ori:期望的姿態(tài) tau_last：傳入一個力矩 eef_nam：對應(yīng)機器人末端位置的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))#目標質(zhì)量矩陣，在講解里邊
tau = sim.data.qfrc_bias[:]
#獲取末端的位置/姿態(tài)/速度/
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])#末端姿態(tài)和位置的拼接
sum = np.multiply(d, xd_error)
pos_error = desired_pos - x_pos#位置偏差
ori_error = orientation_error(desired_ori, x_ori)#姿態(tài)偏差
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訓(xùn)練模型的存儲路徑：
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
