(self)
| 130 | self.goal_pos = set_goal_position(scaled[:3], self.ee_pos, set_pos=None) |
| 131 | |
| 132 | def run_controller(self): |
| 133 | self._update() |
| 134 | desired_pos = np.array(self.goal_pos) |
| 135 | ori_err = orientation_error(np.array(self.goal_ori), self.ee_ori_mat) |
| 136 | |
| 137 | pos_err = desired_pos - self.ee_pos |
| 138 | desired_force = pos_err * self.kp[0:3] + (-self.ee_pos_vel) * self.kd[0:3] |
| 139 | desired_torque = ori_err * self.kp[3:6] + (-self.ee_ori_vel) * self.kd[3:6] |
| 140 | |
| 141 | lam_full, lam_pos, lam_ori, nullspace = opspace_matrices(self.mass_matrix, self.J_full, self.J_pos, self.J_ori) |
| 142 | # uncouple_pos_ori = True |
| 143 | decoupled = np.concatenate([lam_pos @ desired_force, lam_ori @ desired_torque]) |
| 144 | torques = self.J_full.T @ decoupled + self.torque_compensation |
| 145 | torques = torques + nullspace_torques( |
| 146 | self.mass_matrix, nullspace, self.initial_joint, self.joint_pos, self.joint_vel |
| 147 | ) |
| 148 | if self.torque_limits is not None: |
| 149 | torques = np.clip(torques, self.torque_limits[0], self.torque_limits[1]) |
| 150 | return torques |
| 151 | |
| 152 | def apply(self, torques): |
| 153 | self.data.ctrl[self.act_index] = torques |
no test coverage detected