diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index 55c4143..60024da 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -93,7 +93,7 @@ def set_world_wrench(self, wrench_world: Wrench2D, theta: float) -> None: def clip_wrench(self, wrench: Wrench2D) -> Wrench2D: wrench_clipped = Wrench2D() - force = np.sqrt(wrench.fx**2 + wrench.fy**2) + force = np.sqrt(wrench.fx ** 2 + wrench.fy ** 2) force_scale = max(force / self.p.actuators["F_body_max"], 1.0) torque_scale = max(abs(wrench.tz) / self.p.actuators["M_body_max"], 1.0) diff --git a/ff_sim/ff_sim/simulator_node.py b/ff_sim/ff_sim/simulator_node.py index 7503257..fa58a11 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -375,7 +375,7 @@ def f_dynamics_continuous_time(self, x, u): f[2] = thetadot thetaddot = (M - F[1] * p0[0] + F[0] * p0[1]) / J f[3:5] = np.matmul( - R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0)) + R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot ** 2 * p0)) ) f[5] = thetaddot