From 3b44940e36edf18efc8e3c69d8f7a7ab9892dab2 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Thu, 4 Nov 2021 15:21:36 +0100 Subject: [PATCH] Prevent division by zero --- src/active_grasp/controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index db592be..29f4f08 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -126,7 +126,7 @@ class GraspController: e_t = x_d.translation - x.translation e_n = (x.translation - self.view_sphere.center) * (self.view_sphere.r - r) / r linear = 1.0 * e_t + 6.0 * (r < self.view_sphere.r) * e_n - scale = np.linalg.norm(linear) # TODO can be zero if x_d == x + scale = np.linalg.norm(linear) + 1e-6 linear *= np.clip(scale, 0.0, self.linear_vel) / scale angular = self.view_sphere.get_view(theta, phi).rotation * x.rotation.inv() angular = 0.5 * angular.as_rotvec()