preset_reconfigure.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # coding: utf-8
3 
4 import roslib
5 import rospy
6 import dynamic_reconfigure.client
7 from std_msgs.msg import UInt8
8 
9 class PRESET_RECONFIGURE:
10  def __init__( self ):
11  rospy.loginfo("Wait reconfig server...")
12  self.joint_list = [
13  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_shoulder_fixed_part_pan_joint"}, \
14  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_shoulder_revolute_part_tilt_joint"}, \
15  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_upper_arm_revolute_part_twist_joint"}, \
16  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_upper_arm_revolute_part_rotate_joint"}, \
17  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_lower_arm_fixed_part_joint"}, \
18  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_lower_arm_revolute_part_joint"}, \
19  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_wrist_joint"}, \
20  {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_gripper_finger_a_joint"}, \
21  ]
22 
23  ### プリセット定義 - 0.Initial parameters ###
24  self.preset_init = []
25  for i in range(len(self.joint_list)):
26  self.preset_init.append( { "p_gain": 800, "i_gain": 0, "d_gain": 0 } )
27 
28  ### プリセット定義 - 1 脱力状態 ###
29  self.preset_1 = [
30  { "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
31  { "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
32  { "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
33  { "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
34  { "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
35  { "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
36  { "name":"crane_x7_wrist_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
37  { "name":"crane_x7_gripper_finger_a_joint", "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
38  ]
39 
40  ### プリセット定義 - 2 PゲインをInitial parametersの半分にする ###
41  self.preset_2 = [
42  { "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
43  { "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
44  { "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
45  { "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
46  { "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
47  { "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
48  { "name":"crane_x7_wrist_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
49  { "name":"crane_x7_gripper_finger_a_joint", "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
50  ]
51 
52  ### プリセット定義 - 3 ティーチングサンプル用のPIDゲイン ###
53  self.preset_3 = [
54  { "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
55  { "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
56  { "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
57  { "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
58  { "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
59  { "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
60  { "name":"crane_x7_wrist_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
61  { "name":"crane_x7_gripper_finger_a_joint", "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
62  ]
63 
64  ### プリセット定義 - 4 未設定。モノを掴んだ時や、アームの形状を変えた時に設定してみてください ###
65  self.preset_4 = [
66  { "name":"crane_x7_shoulder_fixed_part_pan_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
67  { "name":"crane_x7_shoulder_revolute_part_tilt_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
68  { "name":"crane_x7_upper_arm_revolute_part_twist_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
69  { "name":"crane_x7_upper_arm_revolute_part_rotate_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
70  { "name":"crane_x7_lower_arm_fixed_part_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
71  { "name":"crane_x7_lower_arm_revolute_part_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
72  { "name":"crane_x7_wrist_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
73  { "name":"crane_x7_gripper_finger_a_joint", "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
74  ]
75 
76  ### プリセットデータリスト ###
77  self.preset_list = []
78  self.preset_list.append( self.preset_init )
79  self.preset_list.append( self.preset_1 )
80  self.preset_list.append( self.preset_2 )
81  self.preset_list.append( self.preset_3 )
82  self.preset_list.append( self.preset_4 )
83 
84  self.reconfigure = []
85  for joint in self.joint_list:
86  self.reconfigure.append( {"client":dynamic_reconfigure.client.Client( joint["control"]+"/"+joint["joint"],timeout=10 ), \
87  "joint:":joint["joint"]} )
88  rospy.loginfo("Wait sub...")
89  self.subscribe = rospy.Subscriber("preset_gain_no", UInt8, self.preset_no_callback)
90  rospy.loginfo("Init finished.")
91 
92 
93  def preset_no_callback(self, no):
94  joint_no = 0
95  for conf in self.reconfigure:
96  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"]} )
97  joint_no = joint_no + 1
98 
99 
100 if __name__ == '__main__':
101  rospy.init_node('preset_reconfigure')
102  pr = PRESET_RECONFIGURE()
103  rospy.spin()
preset_reconfigure.PRESET_RECONFIGURE.preset_3
preset_3
プリセット定義 - 3 ティーチングサンプル用のPIDゲイン ###
Definition: preset_reconfigure.py:53
preset_reconfigure.PRESET_RECONFIGURE
Definition: preset_reconfigure.py:9
preset_reconfigure.PRESET_RECONFIGURE.preset_2
preset_2
プリセット定義 - 2 PゲインをInitial parametersの半分にする ###
Definition: preset_reconfigure.py:41
preset_reconfigure.PRESET_RECONFIGURE.__init__
def __init__(self)
Definition: preset_reconfigure.py:10
preset_reconfigure.PRESET_RECONFIGURE.preset_1
preset_1
プリセット定義 - 1 脱力状態 ###
Definition: preset_reconfigure.py:29
preset_reconfigure.PRESET_RECONFIGURE.preset_init
preset_init
プリセット定義 - 0.Initial parameters ###
Definition: preset_reconfigure.py:24
preset_reconfigure.PRESET_RECONFIGURE.subscribe
subscribe
Definition: preset_reconfigure.py:89
preset_reconfigure.PRESET_RECONFIGURE.joint_list
joint_list
Definition: preset_reconfigure.py:12
preset_reconfigure.PRESET_RECONFIGURE.preset_list
preset_list
プリセットデータリスト ###
Definition: preset_reconfigure.py:77
preset_reconfigure.PRESET_RECONFIGURE.reconfigure
reconfigure
Definition: preset_reconfigure.py:84
preset_reconfigure.PRESET_RECONFIGURE.preset_no_callback
def preset_no_callback(self, no)
Definition: preset_reconfigure.py:93
preset_reconfigure.PRESET_RECONFIGURE.preset_4
preset_4
プリセット定義 - 4 未設定。モノを掴んだ時や、アームの形状を変えた時に設定してみてください ### ...
Definition: preset_reconfigure.py:65

crane_x7_control
Author(s): Hiroyuki Nomura , Geoffrey Biggs
autogenerated on Mon Mar 1 2021 03:18:36