def apply_joint_torques()

in polymetis/python/polysim/envs/bullet_manipulator.py [0:0]

• 22 lines of code
• 2 McCabe index (conditional complexity)

    def apply_joint_torques(self, torque: np.ndarray):
        """Applies a NumPy array of torques and returns the final applied torque
        (after gravity compensation, if used)."""
        assert isinstance(torque, np.ndarray)
        self.prev_torques_commanded = torque

        # Do not mutate original array!
        applied_torque = np.clip(torque, -self.torque_limits, self.torque_limits)
        self.prev_torques_applied = applied_torque.copy()

        if self.use_grav_comp:
            joint_cur_pos = self.get_current_joint_pos()
            grav_comp_torques = self.compute_inverse_dynamics(
                joint_pos=joint_cur_pos,
                joint_vel=[0] * self.n_dofs,
                joint_acc=[0] * self.n_dofs,
            )
            applied_torque += grav_comp_torques
        self.prev_torques_measured = applied_torque.copy()

        self.sim.setJointMotorControlArray(
            bodyIndex=self.robot_id,
            jointIndices=self.controlled_joints,
            controlMode=pybullet.TORQUE_CONTROL,
            forces=applied_torque,
        )

        self.sim.stepSimulation()

        return applied_torque