dvrk_python psm_effort_test: added sine wave of joint effort

jhu-dvrk · May 1, 2018 · baea375 · baea375
1 parent 4de4091
commit baea375
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/dvrk_python/scripts/dvrk_psm_effort_test.py b/dvrk_python/scripts/dvrk_psm_effort_test.py
@@ -98,6 +98,18 @@ def joint_effort(self):
         effort_joint[0] = 0.5
         self.arm.set_effort_joint(effort_joint)
 
+        print rospy.get_caller_id(), '   -> arm will now apply sine wave forces on first two joints'
+        raw_input("    Press Enter to continue...")
+        duration = 10  # seconds
+        rate = 200 # aiming for 200 Hz
+        samples = duration * rate
+        # create a new goal starting with current position
+        for i in xrange(samples):
+            effort_joint[0] = 1.0 *  math.sin(i * math.radians(360.0) / samples)
+            effort_joint[1] = 1.0 *  math.sin(i * math.radians(360.0) / samples)
+            rospy.sleep(1.0 / rate)
+            self.arm.set_effort_joint(effort_joint)
+
         print rospy.get_caller_id(), '   -> arm will now go limp'
         raw_input("    Press Enter to continue...")
         effort_joint.fill(0.0)