前文我们知道了如何导入xml到mujoco环境中，接下来我将展示我的笔记，以讲解如何控制机器人的末端到某一指定位置。
整体的思路为：
按数据流讲解代码：
1. 期望末端位姿
device_outputs = device.get_outputs()
action[:3] += device_outputs[0]
action[3:] = T.mat_2_quat(T.quat_2_mat(action[3:]).dot(device_outputs[1]))action[]是一个7维的列表，前三个存储了位置，后三个存储了旋转的四元数，在这里面，action=[ 8.35922622e-01 4.86367540e-03 3.69073482e-01 7.07106781e-01
6.83999407e-06 -7.07106781e-01 -9.27055488e-06]
2.逆运动学解算期望的7个关节角度
2.1得到当前的夹爪末端位置（用于做残差）
p_cur, r_cur = self.forward_kinematics(self.robot.get_arm_qpos(agent))def forward_kinematics(self, q, agent='arm0'):
def set_joint_qpos(qpos: np.ndarray, agent: str = 'arm0'):
""" Set joint position. """
assert qpos.shape[0] == self.robot.jnt_num
for j, per_arm_joint_names in enumerate(self.robot.arm_joint_names[agent]):
self.robot.kine_data.joint(per_arm_joint_names).qpos = qpos[j]#mark:这是深拷贝得到的用于解算的关节位置
set_joint_qpos(q, agent)#mark:设置kine_data的关节位置
mujoco.mj_fwdPosition(self.robot.robot_model, self.robot.kine_data)#mark:These are components of the simulation pipeline, called internally from mj_step, mj_forward and mj_inverse. It is unlikely that the user will need to call them.
base_pos = self.robot.kine_data.body(self.robot.base_link_name[agent]).xpos#mark:这个用法是获取关节位置
base_mat = self.robot.kine_data.body(self.robot.base_link_name[agent]).xmat.reshape(3, 3)
end_pos = self.robot.kine_data.body(self.robot.end_name[agent]).xpos
end_mat = self.robot.kine_data.body(self.robot.end_name[agent]).xmat.reshape(3, 3)
end = T.make_transform(end_pos, end_mat)
base = T.make_transform(base_pos, base_mat)
if self.reference == 'base':
ret = np.linalg.pinv(base) @ end#mark: 计算end相对于base的变换，看不懂
return ret[:3, -1], T.mat_2_quat(ret[:3, :3])
else:
return end_pos, T.mat_2_quat(end_mat)2.2 得到7个关节的期望位置
核心：调用了x, _ = minimize.least_squares(x, ik_target, self.robot.mani_joint_bounds[agent], jacobian=jac_target, verbose=0)
ret[agent] = self.ik(p_goal, r_goal, agent)#p_goal = act[:3]
#r_goal = self.robot.init_quat[agent] if len(act) == 3 else act[3:]
def ik(self, pos, quat, agent='arm0', q_init=None):
del q_init
x = self.robot.get_arm_qpos(agent)
x_prev = x.copy()
ik_target = lambda x: self._ik_res(x, pos=pos, quat=quat, reg_target=x_prev, radius=.1, reg=1e-2, agent=agent)
jac_target = lambda x, r: self._ik_jac(x, r, pos=pos, quat=quat, radius=.1, reg=1e-2, agent=agent)
#mark: 各参数的意义：
#x: 初始位置
#ik_target: 残差函数
#self.robot.mani_joint_bounds[agent]: 边界
#jacobian=jac_target: 雅克比矩阵函数
#verbose=0: 输出信息
x, _ = minimize.least_squares(x, ik_target, self.robot.mani_joint_bounds[agent], jacobian=jac_target, verbose=0)
return x
def _ik_res(self, x, pos=None, quat=None, radius=6, reg=1e-3, reg_target=None, agent='arm0'):
"""Residual for inverse kinematics.
Args:
x: joint angles.
pos: target position for the end effector.
quat: target orientation for the end effector.
radius: scaling of the 3D cross.
Returns:
The residual of the Inverse Kinematics task.
"""
res = []
for i in range(x.shape[1]):
p_cur, r_cur = self.forward_kinematics(x[:, i], agent)
# Position residual.
res_pos = p_cur - pos
# Orientation residual: quaternion difference.
res_quat = np.empty(3)
mujoco.mju_subQuat(res_quat, quat, r_cur)
res_quat *= radius#mark: 看不懂
# Regularization residual.
reg_target = self.robot.init_qpos[agent] if reg_target is None else reg_target
res_reg = reg * (x[:, i] - reg_target)
res_i = np.hstack((res_pos, res_quat, res_reg))
res.append(np.atleast_2d(res_i).T)
return np.hstack(res)
def _ik_jac(self, x, res, pos=None, quat=None, radius=.04, reg=1e-3, agent='arm0'):
"""Analytic Jacobian of inverse kinematics residual
Args:
x: joint angles.
pos: target position for the end effector.
quat: target orientation for the end effector.
radius: scaling of the 3D cross.
Returns:
The Jacobian of the Inverse Kinematics task.
"""
# least_squares() passes the value of the residual at x which is sometimes
# useful, but we don't need it here.
del res
# We can assume x has been copied into qpos
# and that mj_kinematics has been called by ik()
# Call mj_comPos (required for Jacobians).
mujoco.mj_comPos(self.robot.robot_model, self.robot.kine_data)
# Get end-effector site Jacobian.
jac_pos = np.empty((3, self.robot.robot_model.nv))
jac_quat = np.empty((3, self.robot.robot_model.nv))
mujoco.mj_jacBody(
self.robot.robot_model, self.robot.kine_data, jac_pos, jac_quat, self.robot.kine_data.body(self.robot.end_name[agent]).id
)
jac_pos = jac_pos[:, self.robot.arm_joint_indexes[agent]]
jac_quat = jac_quat[:, self.robot.arm_joint_indexes[agent]]
if self.reference == 'base':
base2world_mat = self.robot.get_base_xmat(agent)
jac_pos = base2world_mat.T @ jac_pos
jac_quat = base2world_mat.T @ jac_quat
# Get Deffector, the 3x3 mjd_subQuat Jacobian
effector_quat = self.robot.kine_data.body(self.robot.end_name[agent]).xquat
if self.reference == 'base':
effector_mat = base2world_mat.T @ T.quat_2_mat(effector_quat)
effector_quat = T.mat_2_quat(effector_mat)
Deffector = np.empty((3, 3))
mujoco.mjd_subQuat(quat, effector_quat, None, Deffector)
# Rotate into target frame, multiply by subQuat Jacobian, scale by radius.
target_mat = T.quat_2_mat(quat)
mat = radius * Deffector.T @ target_mat.T
jac_quat = mat @ jac_quat
# Regularization Jacobian.
jac_reg = reg * np.eye(len(self.robot.arm_joint_indexes[agent]))
return np.vstack((jac_pos, jac_quat, jac_reg))3.阻抗控制器对7个关节分别进行控制
3.1反馈当前7个关节的角度和速度
q_cur=self.robot.get_arm_qpos(agent),
v_cur=self.robot.get_arm_qvel(agent),3.2计算7个关节的力矩
def step_controller(self, action):
"""
:param action: joint position
:return: joint torque
"""
ret = dict()
if isinstance(action, np.ndarray):
action = {self.robot.agents[0]: action}
for agent, act in action.items():
torque = self.compute_jnt_torque(
q_des=act,
v_des=np.zeros(self.dofs),
q_cur=self.robot.get_arm_qpos(agent),
v_cur=self.robot.get_arm_qvel(agent),
agent=agent
)
ret[agent] = torque
return retdef compute_jnt_torque(
self,
q_des: np.ndarray,
v_des: np.ndarray,
q_cur: np.ndarray,
v_cur: np.ndarray,
agent: str = 'arm0'
) -> np.ndarray:
""" robot的关节空间控制的计算公式
Compute desired torque with robot dynamics modeling:
> M(q)qdd + C(q, qd)qd + G(q) + tau_F(qd) = tau_ctrl + tau_env
:param q_des: desired joint position
:param v_des: desired joint velocity
:param q_cur: current joint position
:param v_cur: current joint velocity
:return: desired joint torque
"""
M = self.robot.get_mass_matrix(agent)
compensation = self.robot.get_coriolis_gravity_compensation(agent)
acc_desire = self.K * (q_des - q_cur) + self.B * (v_des - v_cur)
tau = np.dot(M, acc_desire) + compensation
return tau3.3 输出力矩
self.set_actuator_ctrl(joint_inputs[agent], agent)
# joint_inputs = self.controller.step_controller(action)
def set_actuator_ctrl(self, torque: np.ndarray, agent: str = 'arm0'):
""" Set joint torque. """
assert torque.shape[0] == self.robot.jnt_num
for j, per_arm_actuator_names in enumerate(self.robot.arm_actuator_names[agent]):
self.mj_data.actuator(per_arm_actuator_names).ctrl = torque[j]4. 模型step
def step(self, action: Union[np.ndarray, Dict[str, np.ndarray]] = None) -> Any:
"""
mark:调用了mj_step
This method will be called with one-step in mujoco
:param action: Input action
:return: None
"""
if self.renderer.render_paused:
self.cur_timestep += 1
mujoco.mj_step(self.mj_model, self.mj_data, self._n_substeps)
if self.renderer.exit_flag:
self.close()render:
if isinstance(self.viewer, viewer.Handle):
if self.viewer.is_running():
self.viewer.sync()
else:
self.close()
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
