From e343ee401f9faf1cc3cdebb3a34ae3be434b1e43 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Fri, 8 Oct 2021 15:21:20 +0200 Subject: [PATCH] Add normal vel only when min_z is violated --- src/active_grasp/controller.py | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index cf6041e..54536fd 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -120,21 +120,14 @@ class GraspController: self.cartesian_vel_pub.publish(to_twist_msg(cmd)) def compute_velocity_cmd(self, x_d, x): - # Velocity cmd towards the target - e_t = x_d.translation - x.translation - - # Velocity cmd towards the surface of the sphere r, theta, phi = cartesian_to_spherical(x.translation - self.view_sphere.center) - e_n = (self.view_sphere.center - x.translation) * (r - self.view_sphere.r) / r - - # Final cmd is a linear combination of both components - linear = 1.0 * e_t + 6.0 * e_n + 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) 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() - return np.r_[linear, angular] def execute_grasp(self, grasp):