From c508c65038de9462642203e5617adbea93bf1511 Mon Sep 17 00:00:00 2001
From: Michel Breyer <michel.p.breyer@gmail.com>
Date: Fri, 3 Sep 2021 23:07:30 +0200
Subject: [PATCH] Improve plugin activation

---
 scripts/bt_sim_node.py | 42 +++++++++++++++++++++++-------------------
 1 file changed, 23 insertions(+), 19 deletions(-)

diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py
index 5dffab2..211a77f 100755
--- a/scripts/bt_sim_node.py
+++ b/scripts/bt_sim_node.py
@@ -42,24 +42,28 @@ class BtSimNode:
         return SeedResponse()
 
     def reset(self, req):
-        for plugin in self.plugins:
-            plugin.is_running = False
+        self.deactivate_plugins()
         rospy.sleep(1.0)  # TODO replace with a read-write lock
         bbox = self.sim.reset()
-        self.plugins[2].dx_d = np.zeros(6)
-        res = ResetResponse(to_bbox_msg(bbox))
-        for plugin in self.plugins:
-            plugin.is_running = True
-        return res
+        self.activate_plugins()
+        return ResetResponse(to_bbox_msg(bbox))
 
     def run(self):
         self.start_plugins()
+        self.activate_plugins()
         rospy.spin()
 
     def start_plugins(self):
         for plugin in self.plugins:
             plugin.thread.start()
-            plugin.is_running = True
+
+    def activate_plugins(self):
+        for plugin in self.plugins:
+            plugin.activate()
+
+    def deactivate_plugins(self):
+        for plugin in self.plugins:
+            plugin.deactivate()
 
 
 class Plugin:
@@ -70,6 +74,12 @@ class Plugin:
         self.thread = Thread(target=self.loop, daemon=True)
         self.is_running = False
 
+    def activate(self):
+        self.is_running = True
+
+    def deactivate(self):
+        self.is_running = False
+
     def loop(self):
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
@@ -115,27 +125,21 @@ class CartesianVelocityControllerPlugin(Plugin):
         super().__init__(rate)
         self.arm = arm
         self.model = model
-        self.max_linear_vel = 0.1
-        self.max_angular_vel = 1.57
-        self.dx_d = np.zeros(6)
         rospy.Subscriber("command", Twist, self.target_cb)
 
     def target_cb(self, msg):
         self.dx_d = from_twist_msg(msg)
 
+    def activate(self):
+        self.dx_d = np.zeros(6)
+        self.is_running = True
+
     def update(self):
         q, _ = self.arm.get_state()
-        dx = self.limit_rate(self.dx_d)
         J_pinv = np.linalg.pinv(self.model.jacobian(q))
-        cmd = np.dot(J_pinv, dx)
+        cmd = np.dot(J_pinv, self.dx_d)
         self.arm.set_desired_joint_velocities(cmd)
 
-    def limit_rate(self, dx):
-        linear, angular = dx[:3], dx[3:]
-        linear = np.clip(linear, -self.max_linear_vel, self.max_linear_vel)
-        angular = np.clip(angular, -self.max_angular_vel, self.max_angular_vel)
-        return np.r_[linear, angular]
-
 
 class MoveActionPlugin(Plugin):
     def __init__(self, gripper, rate=10):