-
Notifications
You must be signed in to change notification settings - Fork 13
/
Copy pathsetup_gazebo.py
127 lines (108 loc) · 4.45 KB
/
setup_gazebo.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
import rospkg
def add_text(file_path, text_to_add):
with open(file_path, 'r+') as file:
content = file.read()
if text_to_add not in content:
file.seek(0, 2) # Move the file pointer to the end
file.write(text_to_add)
def search_and_paste(file_path, search_word, paste_word):
with open(file_path, 'r+') as file:
lines = file.readlines()
file.seek(0) # Reset file pointer to the beginning
for line in lines:
file.write(line) # Write the original line to the file
if search_word in line:
if not(paste_word + '\n' in lines):
file.write(paste_word + '\n')
def replace_line(file_path, search_line, replace_line):
with open(file_path, 'r') as file:
lines = file.readlines()
with open(file_path, 'w') as file:
for line in lines:
if line.strip() == search_line:
file.write(replace_line + '\n')
else:
file.write(line)
# Print the path to the franka_gazebo
ros_pack = rospkg.RosPack()
franka_gazebo = ros_pack.get_path('franka_gazebo')
franka_hfc = ros_pack.get_path('franka_human_friendly_controllers')
new_text_cartesian='''
cartesian_variable_impedance_controller:
type: franka_human_friendly_controllers/CartesianVariableImpedanceController
arm_id: $(arg arm_id)
joint_names:
- $(arg arm_id)_joint1
- $(arg arm_id)_joint2
- $(arg arm_id)_joint3
- $(arg arm_id)_joint4
- $(arg arm_id)_joint5
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
'''
new_text_joint='''
joint_variable_impedance_controller:
type: franka_human_friendly_controllers/JointVariableImpedanceController
arm_id: $(arg arm_id)
joint_names:
- $(arg arm_id)_joint1
- $(arg arm_id)_joint2
- $(arg arm_id)_joint3
- $(arg arm_id)_joint4
- $(arg arm_id)_joint5
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
'''
# # Perform the replacement
# new_text = new_text.replace("franka_advanced_controllers", input_string)
file_path=franka_gazebo + '/config/sim_controllers.yaml'
print("Change files")
print(file_path)
add_text(file_path, new_text_cartesian)
add_text(file_path, new_text_joint)
# MODIFY THE PACKAGE
file_path = franka_gazebo + '/package.xml'
print("Change files")
print(file_path)
existing_depend = '<depend>franka_example_controllers</depend>'
new_depend = ' <depend>franka_human_friendly_controllers</depend>'
# new_depend = new_depend.replace("franka_advanced_controllers", input_string)
search_and_paste(file_path, existing_depend, new_depend)
# MODIFY CMAKE
file_path = franka_gazebo + '/CMakeLists.txt'
print("Change files in directory")
print(file_path)
existing_depend = 'franka_example_controllers'
new_depend = ' franka_human_friendly_controllers'
# new_depend = new_depend.replace("franka_advanced_controllers", input_string)
search_and_paste(file_path, existing_depend, new_depend)
# Modify the controller to not have the joint repulsion that generated obscillations in sumulation
file_path = franka_hfc + '/src/cartesian_variable_impedance_controller.cpp'
print("Change files in directory")
print(file_path)
search_line= 'tau_d << tau_task + tau_nullspace + coriolis+ tau_joint_limit+ tau_joint_limit_ns+ tau_default_joint_damping + tau_default_last_joint_damping + tau_vibration;'
new_line='tau_d << tau_task + tau_nullspace + coriolis;'
replace_line(file_path, search_line, new_line)
# Modify the joint impedance control also
file_path = franka_hfc + '/src/joint_variable_impedance_controller.cpp'
print("Change files in directory")
print(file_path)
search_line= 'tau_d << tau_joint + coriolis + tau_joint_limit;'
new_line='tau_d << tau_joint + coriolis;'
replace_line(file_path, search_line, new_line)
# MODIFY THE CLOCK
file_path = franka_gazebo + '/launch/panda.launch'
search_line = '<arg name="use_sim_time" value="true"/>'
new_line = ' <arg name="use_sim_time" value="false"/>'
print("Change files in directory")
print(file_path)
replace_line(file_path, search_line, new_line)
# add the rqt in the launch file
file_path = franka_gazebo + '/launch/panda.launch'
print("Change files")
print(file_path)
existing_depend='</include>'
new_depend='<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />'
search_and_paste(file_path, existing_depend, new_depend)
new_depend='<rosparam file="$(find franka_description)/robots/panda/joint_limits.yaml" command="load"/>'
search_and_paste(file_path, existing_depend, new_depend)