forked from Yusuke-Yamasaki-555/crane_x7_ros
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathpreset_reconfigure.py
executable file
·103 lines (90 loc) · 7.36 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
#!/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...")
self.joint_list = [
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_shoulder_fixed_part_pan_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_shoulder_revolute_part_tilt_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_upper_arm_revolute_part_twist_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_upper_arm_revolute_part_rotate_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_lower_arm_fixed_part_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_lower_arm_revolute_part_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_wrist_joint"}, \
{"control":"/crane_x7/crane_x7_control","joint":"crane_x7_gripper_finger_a_joint"}, \
]
### プリセット定義 - 0.Initial parameters ###
self.preset_init = []
for i in range(len(self.joint_list)):
self.preset_init.append( { "p_gain": 800, "i_gain": 0, "d_gain": 0 } )
### プリセット定義 - 1 脱力状態 ###
self.preset_1 = [
{ "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_wrist_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_gripper_finger_a_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 2 PゲインをInitial parametersの半分にする ###
self.preset_2 = [
{ "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_wrist_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_gripper_finger_a_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 3 ティーチングサンプル用のPIDゲイン ###
self.preset_3 = [
{ "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_wrist_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_gripper_finger_a_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
]
### プリセット定義 - 4 未設定。モノを掴んだ時や、アームの形状を変えた時に設定してみてください ###
self.preset_4 = [
{ "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_wrist_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
{ "name":"crane_x7_gripper_finger_a_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_4 )
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()