-
Notifications
You must be signed in to change notification settings - Fork 0
/
logger.py
executable file
·100 lines (75 loc) · 2.33 KB
/
logger.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
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
#!/usr/bin/env python
import rospy
import dvrk
import numpy as np
import signal
def signal_handler(signal,frame):
global interrupted
interrupted = True
def get_cartesian(pose):
position = pose.p
x = position.x()
y = position.y()
z = position.z()
output = np.array([x,y,z])
return output
signal.signal(signal.SIGINT,signal_handler)
interrupted = False
print "initializing logger"
filename = raw_input("Please key in filename :")
filename = filename+'.csv'
p1 = dvrk.psm('PSM1')
p2 = dvrk.psm('PSM2')
rate = rospy.Rate(30)
time_start = rospy.get_time()
pose1 = p1.get_current_position()
pose2 = p2.get_current_position()
twist1 = p1.get_current_twist_body()
twist2 = p2.get_current_twist_body()
wrench1 = p1.get_current_wrench_body()
wrench2 = p2.get_current_wrench_body()
#jaw_pos1 = p1.get_current_jaw_position()
#jaw_pos2 = p2.get_current_jaw_position()
#jaw_vel1 = p1.get_current_jaw_velocity()
#jaw_vel2 = p2.get_current_jaw_velocity()
#jaw_torq1 = p1.get_current_jaw_effort()
#jaw_torq2 = p2.get_current_jaw_effort()
time = rospy.get_time()-time_start
trial_num = 1
# initialize our np array
pos1 = get_cartesian(pose1)
pos2 = get_cartesian(pose2)
data = np.hstack((trial_num,time,pos1,pos2,twist1,twist2,wrench1,wrench2))
print "logging data..."
while trial_num<16:
print ' trial '+str(trial_num)+' start...'
time_start = rospy.get_time()
while True:
time = rospy.get_time()-time_start
pose1 = p1.get_current_position()
pose2 = p2.get_current_position()
twist1 = p1.get_current_twist_body()
twist2 = p2.get_current_twist_body()
wrench1 = p1.get_current_wrench_body()
wrench2 = p2.get_current_wrench_body()
#jaw_pos1 = p1.get_current_jaw_position()
#jaw_pos2 = p2.get_current_jaw_position()
#jaw_vel1 = p1.get_current_jaw_velocity()
#jaw_vel2 = p2.get_current_jaw_velocity()
#jaw_torq1 = p1.get_current_jaw_effort()
#jaw_torq2 = p2.get_current_jaw_effort()
pos1 = get_cartesian(pose1)
pos2 = get_cartesian(pose2)
new_data = np.hstack((trial_num,time,pos1,pos2,twist1,twist2,wrench1,wrench2))
print new_data
data = np.vstack((data,new_data))
if interrupted:
trial_num += 1
break
rate.sleep()
interrupted = False
print ' trial '+str(trial_num-1)+' completed'
#scipy.io.savemat('test.mat',dat = data)
print '\n'
print 'saving '+filename+'...'
np.savetxt(filename,data,delimiter=',',fmt='%.4f')