diff --git a/ff_control/ff_control/wrench_ctrl.py b/ff_control/ff_control/wrench_ctrl.py index 68f0754..f58b3ee 100644 --- a/ff_control/ff_control/wrench_ctrl.py +++ b/ff_control/ff_control/wrench_ctrl.py @@ -96,7 +96,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 5fa7e4c..3c716c5 100644 --- a/ff_sim/ff_sim/simulator_node.py +++ b/ff_sim/ff_sim/simulator_node.py @@ -408,9 +408,7 @@ def f_dynamics_continuous_time(self, x, u): f[0:2] = v 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)) - ) + f[3:5] = np.matmul(R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0))) f[5] = thetaddot # add constant force due to table tilt