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

Merge branch 'devel' of github.com:mbreyer/active_grasp into devel

Browse Source
This commit is contained in:
Michel Breyer 2021-11-04 15:15:22 +01:00
commit c9b6ecbad1
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/active_grasp/controller.py
View File

@ -126,7 +126,7 @@ class GraspController:
e_t = x_d.translation - x.translation e_t = x_d.translation - x.translation
e_n = (x.translation - self.view_sphere.center) * (self.view_sphere.r - r) / r 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 linear = 1.0 * e_t + 6.0 * (r < self.view_sphere.r) * e_n
scale = np.linalg.norm(linear) scale = np.linalg.norm(linear) # TODO can be zero if x_d == x
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()