added wait_until_at_setpoint

hello-robot · Aug 29, 2022 · d15e3fb · d15e3fb
1 parent c9c79d6
commit d15e3fb
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git a/body/stretch_body/base.py b/body/stretch_body/base.py
@@ -95,6 +95,10 @@ def wait_for_contact(self, timeout=5.0):
             time.sleep(0.01)
         return False
 
+    def wait_until_at_setpoint(self, timeout=15.0):
+        #Assume both are in motion. This will exit once both are at setpoints
+        self.left_wheel.wait_until_at_setpoint(timeout)
+        self.right_wheel.wait_until_at_setpoint(timeout)
 
     def contact_thresh_to_motor_current(self,is_translate,contact_thresh, contact_model):
         contact_model = self.params['contact_model'] if contact_model is None else contact_model

diff --git a/body/stretch_body/prismatic_joint.py b/body/stretch_body/prismatic_joint.py
@@ -448,6 +448,9 @@ def translate_m_to_motor_rad(self, x):#Override
         self.logger.warning('motor_rad_to_translate_m not implemented in %s' % self.name)
         pass
 
+    def wait_until_at_setpoint(self, timeout=15.0):
+        self.motor.wait_until_at_setpoint(timeout)
+
     def wait_for_contact(self, timeout=5.0):
         ts=time.time()
         while (time.time()-ts<timeout):