hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add normal vel only when min_z is violated

Browse Source
This commit is contained in:
Michel Breyer 2021-10-08 15:21:20 +02:00
parent 8df7118ae2
commit e343ee401f
1 changed files with 3 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/active_grasp/controller.py
View File

@ -120,21 +120,14 @@ class GraspController:
self.cartesian_vel_pub.publish(to_twist_msg(cmd)) self.cartesian_vel_pub.publish(to_twist_msg(cmd))
def compute_velocity_cmd(self, x_d, x): 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) 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 e_t = x_d.translation - x.translation
e_n = (x.translation - self.view_sphere.center) * (self.view_sphere.r - r) / r
# Final cmd is a linear combination of both components linear = 1.0 * e_t + 6.0 * (r < self.view_sphere.r) * e_n
linear = 1.0 * e_t + 6.0 * e_n
scale = np.linalg.norm(linear) scale = np.linalg.norm(linear)
linear *= np.clip(scale, 0.0, self.linear_vel) / scale linear *= np.clip(scale, 0.0, self.linear_vel) / scale
angular = self.view_sphere.get_view(theta, phi).rotation * x.rotation.inv() angular = self.view_sphere.get_view(theta, phi).rotation * x.rotation.inv()
angular = 0.5 * angular.as_rotvec() angular = 0.5 * angular.as_rotvec()
return np.r_[linear, angular] return np.r_[linear, angular]
def execute_grasp(self, grasp): def execute_grasp(self, grasp):