-
Notifications
You must be signed in to change notification settings - Fork 0
/
haptics_test.py
executable file
·58 lines (43 loc) · 1.23 KB
/
haptics_test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
#!/usr/bin/env python
import rospy
import dvrk
import numpy as np
import signal
import PyKDL
from geometry_msgs.msg import Vector3, Quaternion, Wrench
from sensor_msgs.msg import Joy
#rospy.init_node('haptic_feedback')
def trigger_callback(data):
global trigger
butt = data.buttons[0]
if butt > 0.5:
trigger = True
else:
trigger = False
return
def haptic_feedback(data):
global force_feedback
force_feedback[0] = data.force.x
force_feedback[1] = data.force.y
force_feedback[2] = -data.force.z
translation = PyKDL.Vector(0.0,0.0,0.0) #dummy to get the robot back in position control mode
force_feedback = [0,0,0]
m2 = dvrk.mtm('MTMR')
teleop_sub = rospy.Subscriber('/dvrk/footpedals/coag',Joy,trigger_callback)
force_sub = rospy.Subscriber('force_sensor',Wrench,haptic_feedback)
force_feedback = [0,0,0]
trigger = False
position_state_flag = True
m2.set_wrench_body_orientation_absolute(True)
while True:
if trigger == True:
#print force_feedback
m2.set_wrench_body_force(force_feedback)
if position_state_flag == True:
#print m2.get_robot_state()
position_state_flag = False
else:
if position_state_flag == False:
m2.dmove(translation)
#print m2.get_robot_state()
position_state_flag = True