From baea3759275e498850f79b4c6ad049182425b1ac Mon Sep 17 00:00:00 2001 From: Anton Deguet Date: Tue, 1 May 2018 10:50:24 -0400 Subject: [PATCH] dvrk_python psm_effort_test: added sine wave of joint effort --- dvrk_python/scripts/dvrk_psm_effort_test.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/dvrk_python/scripts/dvrk_psm_effort_test.py b/dvrk_python/scripts/dvrk_psm_effort_test.py index 3d932fcf..d272be91 100755 --- 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)