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

Comment velocity controller

Browse Source
This commit is contained in:
Michel Breyer 2021-09-14 12:37:48 +02:00
parent 2835bb7c87
commit 86f96e28c8
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
active_grasp/controller.py
View File

@ -120,15 +120,21 @@ class GraspController:
self.cartesian_vel_pub.publish(to_twist_msg(cmd))
def compute_velocity_cmd(self, x_d, x):
# TODO verify that the frames are handled correctly
r, theta, phi = cartesian_to_spherical(x.translation - self.view_sphere.center)
# 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
linear = 1.0 * e_t + 10.0 * e_n
# Final cmd is a linear combination of both components
linear = 1.0 * e_t + 6.0 * 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):