rob_ilc_lin_force.xml 11.8 KB
Newer Older
victor's avatar
victor committed
1
2
3
4
5
6
<?xml version="1.0" encoding="UTF-8"?>

<pitasc>

    <models>
        <include package="pitasc_library" file="models/pitasc.xml"/>
Lorenz Halt's avatar
Lorenz Halt committed
7
        <!-- <include package="pitasc_library" file="models/skills.xml"/> -->
victor's avatar
victor committed
8
9
        <include package="ilc" file="models/ilc_controllers.xml"/>
        <include package="ilc" file="models/ilc_scripts.xml"/>
Lorenz Halt's avatar
Lorenz Halt committed
10
11
12
13

        <include package="pitasc_library" file="universal_robots/ur.xml"/>
      <!--  <include package="ilc" file="models/skills.xml"/> -->

victor's avatar
victor committed
14
15
16
17
18
19
20
21
22
23
24
25
26
    </models>

    <!-- Create a project -->
    <clone prototype="project">

        <member id="configuration">
            <!-- Use the default configuration with recommended settings -->
            <clone id="configuration" prototype="default_configuration"/>
        </member>

        <member id="environment">
            <!-- Add a UR5 -->
            <clone prototype="robot_ur5">
Lorenz Halt's avatar
Lorenz Halt committed
27
                <member id="robot_driver.max_velocity">20</member>
victor's avatar
victor committed
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
                <member id="robot_driver.max_acceleration">10</member>
                <member id="components">
                    <clone prototype="force_sensor">
                        <member id="wrench_topic">wrench</member>
                    </clone>
                </member>
            </clone>
        </member>

        <member id="applications">

            <!-- Use a skill sequence as a container -->
            <clone prototype="skill_sequence">

                <!-- Use the UR5 for this skill (and for its subskill) -->
                <member id="robot" reference_id="environment.robot_ur5"/>
Lorenz Halt's avatar
Lorenz Halt committed
44
45

                <!-- Add subskill(s) -->
victor's avatar
victor committed
46
                <member id="skills">
Lorenz Halt's avatar
Lorenz Halt committed
47
48
49
50
51

                    <!-- Move to target1 (lin) -->
                    <!-- <clone prototype="skill_lin_ilc"> -->
                   <clone prototype="skill_ptp">
                        <member id="joint_names">elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint</member>
Lorenz Halt's avatar
Lorenz Halt committed
52
                        <member id="target_joint_state">-1.8836291472064417, -1.6066244284259241, -1.5089896360980433, -1.2221058050738733, 1.5608270168304443, -0.7423265616046351</member>
Lorenz Halt's avatar
Lorenz Halt committed
53
54
55
56
                    </clone>

                    <clone prototype="skill_lin">
                        <member id="tool_frame">ee_link</member>
Lorenz Halt's avatar
Lorenz Halt committed
57
                        <member id="target_frame">5_above_contact</member>
victor's avatar
victor committed
58

Lorenz Halt's avatar
Lorenz Halt committed
59
60
                        <member id="linear_controller_gain">0.5</member>
                        <member id="max_linear_velocity">10</member>
victor's avatar
victor committed
61
62
63
64
                    </clone>

                    <clone id="ilc_skill" prototype="skill_lin">
                        <member id="tool_frame">ee_link</member>
Lorenz Halt's avatar
Lorenz Halt committed
65
                        <member id="target_frame">contact</member>
Lorenz Halt's avatar
Lorenz Halt committed
66
67
68

                        <member id="linear_controller_gain">0.5</member>
                         <member id="max_linear_velocity">10</member>
victor's avatar
victor committed
69
70
71


                        <member id="tasks.tracking.setpoint_generators">
Lorenz Halt's avatar
Lorenz Halt committed
72
                            <clone id="setpoint_cos" prototype="constant_setpoint">
victor's avatar
victor committed
73
74
75
76
77
78
79
80
81
82
83
                                <member id="data_source">
                                    <reference reference_id="collections.target_to_tool.chains[0]"/>
                                </member>
                                <member id="desired" reference_id="target_offsets"/>
                                <member id="coordinates" reference_id="coordinates"/>
                                <member id="prefix" reference_id="prefix"/>
                            </clone>
                        </member>

                        <member id="tasks.tracking.controllers">
                          <clone id="ilc_controller" prototype="p_controller_ilc">
Lorenz Halt's avatar
Lorenz Halt committed
84
                              <member id="filename_in">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
victor's avatar
victor committed
85
86
87
88
                              <member id="data_source">
                                  <reference reference_id="collections.target_to_tool.chains[0]"/>
                              </member>
                              <member id="setpoint_generator">
Lorenz Halt's avatar
Lorenz Halt committed
89
                                  <reference reference_id="setpoint_generators.setpoint_cos"/>
victor's avatar
victor committed
90
91
92
93
94
95
96
                              </member>
                              <member id="coordinates">x, y, z</member>
                              <member id="prefix" reference_id="prefix"/>
                              <member id="limit" reference_id="max_linear_velocity"/>
                              <member id="gain" reference_id="linear_controller_gain"/>
                          </clone>

Lorenz Halt's avatar
Lorenz Halt committed
97

victor's avatar
victor committed
98
99
100
101
102
103
104
                          <clone prototype="tanh_controller">
                              <member id="max_output" reference_id="max_angular_velocity"/>
                              <member id="P_lin" reference_id="angular_controller_gain"/>
                              <member id="data_source">
                                  <reference reference_id="collections.target_to_tool.chains[0]"/>
                              </member>
                              <member id="setpoint_generator">
Lorenz Halt's avatar
Lorenz Halt committed
105
                                  <reference reference_id="setpoint_generators.setpoint_cos"/>
victor's avatar
victor committed
106
107
108
109
110
111
112
113
114
                              </member>
                              <member id="coordinates">a, b, c</member>
                              <member id="prefix" reference_id="prefix"/>
                          </clone>
                        </member>

                        <member id="scripts">
                            <clone prototype="script_error_logger">
                                <member id="controllers">
Lorenz Halt's avatar
Lorenz Halt committed
115

victor's avatar
victor committed
116
117
                                    <reference reference_id="tasks.tracking.controllers.ilc_controller"/>
                                </member>
Lorenz Halt's avatar
Lorenz Halt committed
118
                                <member id="file_name">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member>
victor's avatar
victor committed
119
120
121
122
                            </clone>
                        </member>
                    </clone>

Lorenz Halt's avatar
Lorenz Halt committed
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
                    <clone prototype="skill_idle_duration">
                        <member id ="duration"> 1 </member>
                    </clone>

                    <clone id="ilc_push_settle" prototype="skill_push_settle">
                        <member id="tool_frame">ee_link</member>
                        <member id="force.force_frame">kms40</member>
                        <member id="force.axes">z</member>
                        <member id="force.target_forces">10.0</member>
                        <member id="force.compliance">0.0005</member>
                        <member id="force.corner_frequency">7</member>

                        <member id="force.tasks.apply_force.controllers">
                             <clone id="ilc_controller" prototype="first_order_impedance_controller_ilc">
                                    <member id="filename_in">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member>
                                     <member id="data_source">
                                              <reference reference_id="collections.force_chain.chains[0]"/>
                                     </member>
                                     <member id="setpoint_generator">
                                             <reference reference_id="setpoint_generators.setpoint"/>
                                     </member>
                                     <member id="coordinates">x, y, z</member>
                                     <member id="prefix" reference_id="prefix"/>
                             </clone>
                        </member>

                        <member id="scripts">
                            <clone prototype="script_error_logger">
                                <member id="controllers">

                                        <reference reference_id="force.tasks.apply_force.controllers.first_order_impedance_controller"/>
                                </member>
                                <member id="file_name">/home/ipa325/Documents/ilc_Logs/force_logger_0.csv</member>
                                <member id="filter">force_chain/x,force_chain/y,force_chain/z</member>
                            </clone>
                        </member>

                    </clone>

                    <clone prototype="skill_idle_duration">
                        <member id ="duration"> 1 </member>
                    </clone>
Lorenz Halt's avatar
Lorenz Halt committed
165

victor's avatar
victor committed
166
167
                    <clone prototype="skill_lin">
                        <member id="tool_frame">ee_link</member>
Lorenz Halt's avatar
Lorenz Halt committed
168
169
170
171
                        <member id="target_frame">5_above_contact</member>

                        <member id="linear_controller_gain">0.5</member>
                         <member id="max_linear_velocity">10</member>
victor's avatar
victor committed
172
173
174

                        <member id="scripts">
                            <clone prototype="script_trigger_ilc">
Lorenz Halt's avatar
Lorenz Halt committed
175
                                <member id="namespace">ilc_rosproxy</member>
victor's avatar
victor committed
176
                                <member id="service_name">ilc_service</member>
Lorenz Halt's avatar
Lorenz Halt committed
177
178
179
                                <member id="file_in">/home/ipa325/Documents/ilc_Logs/logger_0.csv</member>
                                <member id="file_out">/home/ipa325/Documents/ilc_Logs/lin_ff_2.txt</member>
                                <member id="file_traj">/home/ipa325/Documents/ilc_Logs/trajectory.txt</member>
victor's avatar
victor committed
180
                                <member id="traj_type">cos</member>
Lorenz Halt's avatar
Lorenz Halt committed
181
                                <member id="L">0.6</member>
victor's avatar
victor committed
182
                                <member id="cutoff_freq">0.5</member>
Lorenz Halt's avatar
Lorenz Halt committed
183
184
185
186
187
188
189
190
191
192
                                <member id="time_shift">25</member>
                            </clone>
                            <clone prototype="script_retime_if_ilc">
                                <member id="namespace">ilc_rosproxy</member>
                                <member id="service_name">ilc_retime_if</member>
                                <member id="on_start">false</member>
                                <member id="retime_factor">0.9</member>
                                <member id="convergence_threshold">0.03</member>
                                <member id="reuse_ff">True</member>
                                <member id="reuse_scheme">1</member>
victor's avatar
victor committed
193
                            </clone>
Lorenz Halt's avatar
Lorenz Halt committed
194

Lorenz Halt's avatar
Lorenz Halt committed
195
196
197
198
199
200
201
202
203
204
                            <clone prototype="script_trigger_ilc">
                                <member id="namespace">ilc_rosproxy2</member>
                                <member id="service_name">ilc_service</member>
                                <member id="file_in">/home/ipa325/Documents/ilc_Logs/force_logger_0.csv</member>
                                <member id="file_out">/home/ipa325/Documents/ilc_Logs/force_ff_2.txt</member>
                                <member id="traj_type">cos</member>
                                <member id="L">0.00005</member>
                                <member id="cutoff_freq">0.9</member>
                                <member id="time_shift">25</member>
                            </clone>
Lorenz Halt's avatar
Lorenz Halt committed
205
                            <clone prototype="script_retime_if_ilc">
Lorenz Halt's avatar
Lorenz Halt committed
206
                                <member id="namespace">ilc_rosproxy2</member>
Lorenz Halt's avatar
Lorenz Halt committed
207
208
209
                                <member id="service_name">ilc_retime_if</member>
                                <member id="on_start">false</member>
                                <member id="retime_factor">0.9</member>
Lorenz Halt's avatar
Lorenz Halt committed
210
                                <member id="convergence_threshold">15</member>
Lorenz Halt's avatar
Lorenz Halt committed
211
212
213
214
                                <member id="reuse_ff">True</member>
                                <member id="reuse_scheme">1</member>
                            </clone>
                    </member>
victor's avatar
victor committed
215
216
217
218
219
220
221
222
223

                        <member id="transitions">
                            <clone prototype="transition">
                                <member id="event">succeeded</member>
                                <member id="target">skill_ptp</member>
                            </clone>
                        </member>

                    </clone>
Lorenz Halt's avatar
Lorenz Halt committed
224
225
226



victor's avatar
victor committed
227
228
229
230
231
              </member>
            </clone>
          </member>
        </clone>
</pitasc>