-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathpreset_reconfigure.py
executable file
·180 lines (175 loc) · 14.1 KB
/
preset_reconfigure.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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
#!/usr/bin/env python
# coding: utf-8
import roslib
import rospy
import dynamic_reconfigure.client
from std_msgs.msg import UInt8
class PRESET_RECONFIGURE:
def __init__( self ):
rospy.loginfo("Wait reconfig server...")
### Dynamic Reconfigure 接続先一覧 ###
self.joint_list = [
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint1"}, \
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint2"}, \
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint3"}, \
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint4"}, \
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint5"}, \
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint6"}, \
{"control":"/sciurus17/controller1/joints","joint":"r_arm_joint7"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint1"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint2"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint3"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint4"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint5"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint6"}, \
{"control":"/sciurus17/controller2/joints","joint":"l_arm_joint7"}, \
{"control":"/sciurus17/controller3/joints","joint":"neck_pitch_joint"},\
{"control":"/sciurus17/controller3/joints","joint":"neck_yaw_joint"}, \
{"control":"/sciurus17/controller3/joints","joint":"waist_yaw_joint"}, \
]
### プリセット定義 - 初期値 ###
self.preset_init = []
for i in range(17):
self.preset_init.append( { "p_gain": 800, "i_gain": 0, "d_gain": 0 } )
### プリセット定義 - 1.Free ###
self.preset_1 = [ { "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_pitch_joint","p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_yaw_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"waist_yaw_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 2 ###
self.preset_2 = [ { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"r_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"r_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"r_arm_joint4", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"l_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"l_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"l_arm_joint4", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_pitch_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"waist_yaw_joint", "p_gain": 1500, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 3 ###
self.preset_3 = [ { "name":"r_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"r_arm_joint2", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"r_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint4", "p_gain": 800, "i_gain": 100, "d_gain": 0 },\
{ "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint1", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"l_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint3", "p_gain": 800, "i_gain": 200, "d_gain": 0 },\
{ "name":"l_arm_joint4", "p_gain": 800, "i_gain": 100, "d_gain": 0 },\
{ "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_pitch_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"waist_yaw_joint", "p_gain": 1500, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 右手脱力 ###
self.preset_free_right_arm = [
{ "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint1", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint4", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 左手脱力 ###
self.preset_free_left_arm = [
{ "name":"r_arm_joint1", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint2", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint3", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint4", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint5", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint6", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint7", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 両手脱力 ###
self.preset_free_two_arms = [
{ "name":"r_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"r_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint1", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint2", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint3", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint4", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint5", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint6", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"l_arm_joint7", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_pitch_joint","p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"neck_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"waist_yaw_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
]
### プリセットデータリスト ###
self.preset_list = []
self.preset_list.append( self.preset_init )
self.preset_list.append( self.preset_1 )
self.preset_list.append( self.preset_2 )
self.preset_list.append( self.preset_3 )
self.preset_list.append( self.preset_free_right_arm)
self.preset_list.append( self.preset_free_left_arm)
self.preset_list.append( self.preset_free_two_arms)
self.reconfigure = []
for joint in self.joint_list:
self.reconfigure.append( {"client":dynamic_reconfigure.client.Client( joint["control"]+"/"+joint["joint"],timeout=10 ), \
"joint:":joint["joint"]} )
rospy.loginfo("Wait sub...")
self.subscribe = rospy.Subscriber("preset_gain_no", UInt8, self.preset_no_callback)
rospy.loginfo("Init finished.")
def preset_no_callback(self, no):
joint_no = 0
for conf in self.reconfigure:
conf["client"].update_configuration( {"position_p_gain":self.preset_list[no.data][joint_no]["p_gain"],"position_i_gain":self.preset_list[no.data][joint_no]["i_gain"],"position_d_gain":self.preset_list[no.data][joint_no]["d_gain"]} )
joint_no = joint_no + 1
if __name__ == '__main__':
rospy.init_node('preset_reconfigure')
pr = PRESET_RECONFIGURE()
rospy.spin()