("manipulator_h_base_module_msgs::KinematicsPose");
85 | QObject::connect(&qnode, SIGNAL(updateCurrentKinematicsPose(manipulator_h_base_module_msgs::KinematicsPose)), this, SLOT(updateCurrKinematicsPoseSpinbox(manipulator_h_base_module_msgs::KinematicsPose)));
86 |
87 | /*********************
88 | ** Auto Start
89 | **********************/
90 | qnode.init();
91 | }
92 |
93 | MainWindow::~MainWindow() {}
94 |
95 | /*****************************************************************************
96 | ** Implementation [Slots]
97 | *****************************************************************************/
98 |
99 | void MainWindow::on_curr_joint_button_clicked( bool check )
100 | {
101 | qnode.getJointPose( joint_name );
102 | }
103 |
104 | void MainWindow::on_des_joint_button_clicked( bool check )
105 | {
106 | manipulator_h_base_module_msgs::JointPose msg;
107 |
108 | for ( int _id = 0; _id < joint_spinbox.size(); _id++ )
109 | {
110 | msg.name.push_back( joint_name[ _id ] );
111 | msg.value.push_back( ((QDoubleSpinBox *) joint_spinbox[ _id ])->value() * M_PI / 180.0 );
112 | }
113 |
114 | qnode.sendJointPoseMsg( msg );
115 | }
116 |
117 | void MainWindow::on_curr_pos_button_clicked( bool check )
118 | {
119 | std::string group_name = "arm";
120 |
121 | qnode.getKinematicsPose( group_name );
122 | }
123 |
124 | void MainWindow::on_des_pos_button_clicked( bool check )
125 | {
126 | manipulator_h_base_module_msgs::KinematicsPose msg;
127 |
128 | msg.name = "arm";
129 |
130 | msg.pose.position.x = ui.pos_x_spinbox->value();
131 | msg.pose.position.y = ui.pos_y_spinbox->value();
132 | msg.pose.position.z = ui.pos_z_spinbox->value();
133 |
134 | double roll = ui.ori_roll_spinbox->value() * M_PI / 180.0;
135 | double pitch = ui.ori_pitch_spinbox->value() * M_PI / 180.0;
136 | double yaw = ui.ori_yaw_spinbox->value() * M_PI / 180.0;
137 |
138 | Eigen::Quaterniond QR = rpy2quaternion( roll, pitch, yaw );
139 |
140 | msg.pose.orientation.x = QR.x();
141 | msg.pose.orientation.y = QR.y();
142 | msg.pose.orientation.z = QR.z();
143 | msg.pose.orientation.w = QR.w();
144 |
145 | qnode.sendKinematicsPoseMsg( msg );
146 | }
147 |
148 | void MainWindow::on_ini_pose_button_clicked( bool check )
149 | {
150 | std_msgs::String msg;
151 |
152 | msg.data ="ini_pose";
153 |
154 | qnode.sendIniPoseMsg( msg );
155 | }
156 |
157 | void MainWindow::on_set_mode_button_clicked( bool check )
158 | {
159 | std_msgs::String msg;
160 |
161 | msg.data ="set_mode";
162 |
163 | qnode.sendSetModeMsg( msg );
164 | }
165 |
166 | void MainWindow::updateCurrJointPoseSpinbox( manipulator_h_base_module_msgs::JointPose msg )
167 | {
168 | for ( int _name_index = 0; _name_index < msg.name.size(); _name_index++ )
169 | {
170 | for ( int _id = 0; _id < joint_name.size(); _id++ )
171 | {
172 | if ( msg.name[ _id ] == joint_name[ _name_index ] )
173 | {
174 | ((QDoubleSpinBox *) joint_spinbox[ _name_index ])->setValue( msg.value[ _id ] * 180.0 / M_PI );
175 | break;
176 | }
177 | }
178 | }
179 | }
180 |
181 | void MainWindow::updateCurrKinematicsPoseSpinbox( manipulator_h_base_module_msgs::KinematicsPose msg )
182 | {
183 | ui.pos_x_spinbox->setValue( msg.pose.position.x );
184 | ui.pos_y_spinbox->setValue( msg.pose.position.y );
185 | ui.pos_z_spinbox->setValue( msg.pose.position.z );
186 |
187 | Eigen::Quaterniond QR( msg.pose.orientation.w , msg.pose.orientation.x , msg.pose.orientation.y , msg.pose.orientation.z );
188 |
189 | Eigen::MatrixXd rpy = quaternion2rpy( QR );
190 |
191 | double roll = rpy.coeff( 0 , 0 ) * 180.0 / M_PI;
192 | double pitch = rpy.coeff( 1 , 0 ) * 180.0 / M_PI;
193 | double yaw = rpy.coeff( 2, 0 ) * 180.0 /M_PI;
194 |
195 | ui.ori_roll_spinbox->setValue( roll );
196 | ui.ori_pitch_spinbox->setValue( pitch );
197 | ui.ori_yaw_spinbox->setValue( yaw );
198 | }
199 |
200 | Eigen::MatrixXd MainWindow::rotationX( double angle )
201 | {
202 | Eigen::MatrixXd _rotation( 3 , 3 );
203 |
204 | _rotation << 1.0, 0.0, 0.0,
205 | 0.0, cos( angle ), -sin( angle ),
206 | 0.0, sin( angle ), cos( angle );
207 |
208 | return _rotation;
209 | }
210 |
211 | Eigen::MatrixXd MainWindow::rotationY( double angle )
212 | {
213 | Eigen::MatrixXd _rotation( 3 , 3 );
214 |
215 | _rotation << cos( angle ), 0.0, sin( angle ),
216 | 0.0, 1.0, 0.0,
217 | -sin( angle ), 0.0, cos( angle );
218 |
219 | return _rotation;
220 | }
221 |
222 | Eigen::MatrixXd MainWindow::rotationZ( double angle )
223 | {
224 | Eigen::MatrixXd _rotation(3,3);
225 |
226 | _rotation << cos( angle ), -sin( angle ), 0.0,
227 | sin( angle ), cos( angle ), 0.0,
228 | 0.0, 0.0, 1.0;
229 |
230 | return _rotation;
231 | }
232 |
233 | Eigen::MatrixXd MainWindow::rotation2rpy( Eigen::MatrixXd rotation )
234 | {
235 | Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 );
236 |
237 | _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) );
238 | _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) );
239 | _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) );
240 |
241 | return _rpy;
242 | }
243 |
244 | Eigen::MatrixXd MainWindow::rpy2rotation( double roll, double pitch, double yaw )
245 | {
246 | Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll );
247 |
248 | return _rotation;
249 | }
250 |
251 | Eigen::Quaterniond MainWindow::rpy2quaternion( double roll, double pitch, double yaw )
252 | {
253 | Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw );
254 |
255 | Eigen::Matrix3d _rotation3d;
256 | _rotation3d = _rotation.block( 0 , 0 , 3 , 3 );
257 |
258 | Eigen::Quaterniond _quaternion;
259 |
260 | _quaternion = _rotation3d;
261 |
262 | return _quaternion;
263 | }
264 |
265 | Eigen::Quaterniond MainWindow::rotation2quaternion( Eigen::MatrixXd rotation )
266 | {
267 | Eigen::Matrix3d _rotation3d;
268 |
269 | _rotation3d = rotation.block( 0 , 0 , 3 , 3 );
270 |
271 | Eigen::Quaterniond _quaternion;
272 | _quaternion = _rotation3d;
273 |
274 | return _quaternion;
275 | }
276 |
277 | Eigen::MatrixXd MainWindow::quaternion2rpy( Eigen::Quaterniond quaternion )
278 | {
279 | Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() );
280 |
281 | return _rpy;
282 | }
283 |
284 | Eigen::MatrixXd MainWindow::quaternion2rotation( Eigen::Quaterniond quaternion )
285 | {
286 | Eigen::MatrixXd _rotation = quaternion.toRotationMatrix();
287 |
288 | return _rotation;
289 | }
290 |
291 | /*****************************************************************************
292 | ** Implemenation [Slots][manually connected]
293 | *****************************************************************************/
294 |
295 | /**
296 | * This function is signalled by the underlying model. When the model changes,
297 | * this will drop the cursor down to the last line in the QListview to ensure
298 | * the user can always see the latest log message.
299 | */
300 | void MainWindow::updateLoggingView() {
301 | ui.view_logging->scrollToBottom();
302 | }
303 |
304 | /*****************************************************************************
305 | ** Implementation [Menu]
306 | *****************************************************************************/
307 |
308 | void MainWindow::on_actionAbout_triggered() {
309 | QMessageBox::about(this, tr("About ..."),tr("PACKAGE_NAME Test Program 0.10
Copyright Robotis
This package needs an about description.
"));
310 | }
311 |
312 | /*****************************************************************************
313 | ** Implementation [Configuration]
314 | *****************************************************************************/
315 |
316 | void MainWindow::closeEvent(QCloseEvent *event)
317 | {
318 | QMainWindow::closeEvent(event);
319 | }
320 |
321 | } // namespace manipulator_h_gui
322 |
323 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | Apache License
2 | Version 2.0, January 2004
3 | http://www.apache.org/licenses/
4 |
5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
6 |
7 | 1. Definitions.
8 |
9 | "License" shall mean the terms and conditions for use, reproduction,
10 | and distribution as defined by Sections 1 through 9 of this document.
11 |
12 | "Licensor" shall mean the copyright owner or entity authorized by
13 | the copyright owner that is granting the License.
14 |
15 | "Legal Entity" shall mean the union of the acting entity and all
16 | other entities that control, are controlled by, or are under common
17 | control with that entity. For the purposes of this definition,
18 | "control" means (i) the power, direct or indirect, to cause the
19 | direction or management of such entity, whether by contract or
20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the
21 | outstanding shares, or (iii) beneficial ownership of such entity.
22 |
23 | "You" (or "Your") shall mean an individual or Legal Entity
24 | exercising permissions granted by this License.
25 |
26 | "Source" form shall mean the preferred form for making modifications,
27 | including but not limited to software source code, documentation
28 | source, and configuration files.
29 |
30 | "Object" form shall mean any form resulting from mechanical
31 | transformation or translation of a Source form, including but
32 | not limited to compiled object code, generated documentation,
33 | and conversions to other media types.
34 |
35 | "Work" shall mean the work of authorship, whether in Source or
36 | Object form, made available under the License, as indicated by a
37 | copyright notice that is included in or attached to the work
38 | (an example is provided in the Appendix below).
39 |
40 | "Derivative Works" shall mean any work, whether in Source or Object
41 | form, that is based on (or derived from) the Work and for which the
42 | editorial revisions, annotations, elaborations, or other modifications
43 | represent, as a whole, an original work of authorship. For the purposes
44 | of this License, Derivative Works shall not include works that remain
45 | separable from, or merely link (or bind by name) to the interfaces of,
46 | the Work and Derivative Works thereof.
47 |
48 | "Contribution" shall mean any work of authorship, including
49 | the original version of the Work and any modifications or additions
50 | to that Work or Derivative Works thereof, that is intentionally
51 | submitted to Licensor for inclusion in the Work by the copyright owner
52 | or by an individual or Legal Entity authorized to submit on behalf of
53 | the copyright owner. For the purposes of this definition, "submitted"
54 | means any form of electronic, verbal, or written communication sent
55 | to the Licensor or its representatives, including but not limited to
56 | communication on electronic mailing lists, source code control systems,
57 | and issue tracking systems that are managed by, or on behalf of, the
58 | Licensor for the purpose of discussing and improving the Work, but
59 | excluding communication that is conspicuously marked or otherwise
60 | designated in writing by the copyright owner as "Not a Contribution."
61 |
62 | "Contributor" shall mean Licensor and any individual or Legal Entity
63 | on behalf of whom a Contribution has been received by Licensor and
64 | subsequently incorporated within the Work.
65 |
66 | 2. Grant of Copyright License. Subject to the terms and conditions of
67 | this License, each Contributor hereby grants to You a perpetual,
68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
69 | copyright license to reproduce, prepare Derivative Works of,
70 | publicly display, publicly perform, sublicense, and distribute the
71 | Work and such Derivative Works in Source or Object form.
72 |
73 | 3. Grant of Patent License. Subject to the terms and conditions of
74 | this License, each Contributor hereby grants to You a perpetual,
75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable
76 | (except as stated in this section) patent license to make, have made,
77 | use, offer to sell, sell, import, and otherwise transfer the Work,
78 | where such license applies only to those patent claims licensable
79 | by such Contributor that are necessarily infringed by their
80 | Contribution(s) alone or by combination of their Contribution(s)
81 | with the Work to which such Contribution(s) was submitted. If You
82 | institute patent litigation against any entity (including a
83 | cross-claim or counterclaim in a lawsuit) alleging that the Work
84 | or a Contribution incorporated within the Work constitutes direct
85 | or contributory patent infringement, then any patent licenses
86 | granted to You under this License for that Work shall terminate
87 | as of the date such litigation is filed.
88 |
89 | 4. Redistribution. You may reproduce and distribute copies of the
90 | Work or Derivative Works thereof in any medium, with or without
91 | modifications, and in Source or Object form, provided that You
92 | meet the following conditions:
93 |
94 | (a) You must give any other recipients of the Work or
95 | Derivative Works a copy of this License; and
96 |
97 | (b) You must cause any modified files to carry prominent notices
98 | stating that You changed the files; and
99 |
100 | (c) You must retain, in the Source form of any Derivative Works
101 | that You distribute, all copyright, patent, trademark, and
102 | attribution notices from the Source form of the Work,
103 | excluding those notices that do not pertain to any part of
104 | the Derivative Works; and
105 |
106 | (d) If the Work includes a "NOTICE" text file as part of its
107 | distribution, then any Derivative Works that You distribute must
108 | include a readable copy of the attribution notices contained
109 | within such NOTICE file, excluding those notices that do not
110 | pertain to any part of the Derivative Works, in at least one
111 | of the following places: within a NOTICE text file distributed
112 | as part of the Derivative Works; within the Source form or
113 | documentation, if provided along with the Derivative Works; or,
114 | within a display generated by the Derivative Works, if and
115 | wherever such third-party notices normally appear. The contents
116 | of the NOTICE file are for informational purposes only and
117 | do not modify the License. You may add Your own attribution
118 | notices within Derivative Works that You distribute, alongside
119 | or as an addendum to the NOTICE text from the Work, provided
120 | that such additional attribution notices cannot be construed
121 | as modifying the License.
122 |
123 | You may add Your own copyright statement to Your modifications and
124 | may provide additional or different license terms and conditions
125 | for use, reproduction, or distribution of Your modifications, or
126 | for any such Derivative Works as a whole, provided Your use,
127 | reproduction, and distribution of the Work otherwise complies with
128 | the conditions stated in this License.
129 |
130 | 5. Submission of Contributions. Unless You explicitly state otherwise,
131 | any Contribution intentionally submitted for inclusion in the Work
132 | by You to the Licensor shall be under the terms and conditions of
133 | this License, without any additional terms or conditions.
134 | Notwithstanding the above, nothing herein shall supersede or modify
135 | the terms of any separate license agreement you may have executed
136 | with Licensor regarding such Contributions.
137 |
138 | 6. Trademarks. This License does not grant permission to use the trade
139 | names, trademarks, service marks, or product names of the Licensor,
140 | except as required for reasonable and customary use in describing the
141 | origin of the Work and reproducing the content of the NOTICE file.
142 |
143 | 7. Disclaimer of Warranty. Unless required by applicable law or
144 | agreed to in writing, Licensor provides the Work (and each
145 | Contributor provides its Contributions) on an "AS IS" BASIS,
146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 | implied, including, without limitation, any warranties or conditions
148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 | PARTICULAR PURPOSE. You are solely responsible for determining the
150 | appropriateness of using or redistributing the Work and assume any
151 | risks associated with Your exercise of permissions under this License.
152 |
153 | 8. Limitation of Liability. In no event and under no legal theory,
154 | whether in tort (including negligence), contract, or otherwise,
155 | unless required by applicable law (such as deliberate and grossly
156 | negligent acts) or agreed to in writing, shall any Contributor be
157 | liable to You for damages, including any direct, indirect, special,
158 | incidental, or consequential damages of any character arising as a
159 | result of this License or out of the use or inability to use the
160 | Work (including but not limited to damages for loss of goodwill,
161 | work stoppage, computer failure or malfunction, or any and all
162 | other commercial damages or losses), even if such Contributor
163 | has been advised of the possibility of such damages.
164 |
165 | 9. Accepting Warranty or Additional Liability. While redistributing
166 | the Work or Derivative Works thereof, You may choose to offer,
167 | and charge a fee for, acceptance of support, warranty, indemnity,
168 | or other liability obligations and/or rights consistent with this
169 | License. However, in accepting such obligations, You may act only
170 | on Your own behalf and on Your sole responsibility, not on behalf
171 | of any other Contributor, and only if You agree to indemnify,
172 | defend, and hold each Contributor harmless for any liability
173 | incurred by, or claims asserted against, such Contributor by reason
174 | of your accepting any such warranty or additional liability.
175 |
176 | END OF TERMS AND CONDITIONS
177 |
178 | APPENDIX: How to apply the Apache License to your work.
179 |
180 | To apply the Apache License to your work, attach the following
181 | boilerplate notice, with the fields enclosed by brackets "[]"
182 | replaced with your own identifying information. (Don't include
183 | the brackets!) The text should be enclosed in the appropriate
184 | comment syntax for the file format. We also recommend that a
185 | file or class name and description of purpose be included on the
186 | same "printed page" as the copyright notice for easier
187 | identification within third-party archives.
188 |
189 | Copyright [yyyy] [name of copyright owner]
190 |
191 | Licensed under the Apache License, Version 2.0 (the "License");
192 | you may not use this file except in compliance with the License.
193 | You may obtain a copy of the License at
194 |
195 | http://www.apache.org/licenses/LICENSE-2.0
196 |
197 | Unless required by applicable law or agreed to in writing, software
198 | distributed under the License is distributed on an "AS IS" BASIS,
199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 | See the License for the specific language governing permissions and
201 | limitations under the License.
202 |
--------------------------------------------------------------------------------
/manipulator_h_description/urdf/manipulator_h.xacro:
--------------------------------------------------------------------------------
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 | transmission_interface/SimpleTransmission
64 |
65 | EffortJointInterface
66 |
67 |
68 | EffortJointInterface
69 | 1
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 | transmission_interface/SimpleTransmission
111 |
112 | EffortJointInterface
113 |
114 |
115 | EffortJointInterface
116 | 1
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 | transmission_interface/SimpleTransmission
158 |
159 | EffortJointInterface
160 |
161 |
162 | EffortJointInterface
163 | 1
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 | transmission_interface/SimpleTransmission
205 |
206 | EffortJointInterface
207 |
208 |
209 | EffortJointInterface
210 | 1
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 |
232 |
233 |
234 |
235 |
236 |
237 |
238 |
239 |
240 |
241 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 | transmission_interface/SimpleTransmission
252 |
253 | EffortJointInterface
254 |
255 |
256 | EffortJointInterface
257 | 1
258 |
259 |
260 |
261 |
262 |
263 |
264 |
265 |
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
293 |
294 |
295 |
296 |
297 |
298 | transmission_interface/SimpleTransmission
299 |
300 | EffortJointInterface
301 |
302 |
303 | EffortJointInterface
304 | 1
305 |
306 |
307 |
308 |
309 |
310 |
311 |
312 |
313 |
314 |
315 |
316 |
317 |
318 |
319 |
320 |
321 |
322 |
323 |
324 |
325 |
326 |
327 |
328 |
329 |
330 |
331 |
332 |
333 |
334 |
--------------------------------------------------------------------------------
/manipulator_h_kinematics_dynamics/src/manipulator_h_kinematics_dynamics.cpp:
--------------------------------------------------------------------------------
1 | /*******************************************************************************
2 | * Copyright 2018 ROBOTIS CO., LTD.
3 | *
4 | * Licensed under the Apache License, Version 2.0 (the "License");
5 | * you may not use this file except in compliance with the License.
6 | * You may obtain a copy of the License at
7 | *
8 | * http://www.apache.org/licenses/LICENSE-2.0
9 | *
10 | * Unless required by applicable law or agreed to in writing, software
11 | * distributed under the License is distributed on an "AS IS" BASIS,
12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | * See the License for the specific language governing permissions and
14 | * limitations under the License.
15 | *******************************************************************************/
16 |
17 | /*
18 | * Link.cpp
19 | *
20 | * Created on: Jan 11, 2016
21 | * Author: SCH
22 | */
23 |
24 | #include
25 | #include "manipulator_h_kinematics_dynamics/manipulator_h_kinematics_dynamics.h"
26 |
27 | namespace robotis_manipulator_h
28 | {
29 |
30 | ManipulatorKinematicsDynamics::ManipulatorKinematicsDynamics()
31 | {
32 | }
33 | ManipulatorKinematicsDynamics::~ManipulatorKinematicsDynamics()
34 | {
35 | }
36 |
37 | ManipulatorKinematicsDynamics::ManipulatorKinematicsDynamics(TreeSelect tree)
38 | {
39 | for (int id = 0; id <= ALL_JOINT_ID; id++)
40 | manipulator_link_data_[id] = new LinkData();
41 |
42 | if (tree == ARM)
43 | {
44 | manipulator_link_data_[0]->name_ = "base";
45 | manipulator_link_data_[0]->parent_ = -1;
46 | manipulator_link_data_[0]->sibling_ = -1;
47 | manipulator_link_data_[0]->child_ = 1;
48 | manipulator_link_data_[0]->mass_ = 0.0;
49 | manipulator_link_data_[0]->relative_position_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
50 | manipulator_link_data_[0]->joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
51 | manipulator_link_data_[0]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
52 | manipulator_link_data_[0]->joint_limit_max_ = 100.0;
53 | manipulator_link_data_[0]->joint_limit_min_ = -100.0;
54 | manipulator_link_data_[0]->inertia_ = robotis_framework::getInertiaXYZ(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
55 |
56 | manipulator_link_data_[1]->name_ = "joint1";
57 | manipulator_link_data_[1]->parent_ = 0;
58 | manipulator_link_data_[1]->sibling_ = -1;
59 | manipulator_link_data_[1]->child_ = 2;
60 | manipulator_link_data_[1]->mass_ = 0.85644;
61 | manipulator_link_data_[1]->relative_position_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.126);
62 | manipulator_link_data_[1]->joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 1.0);
63 | manipulator_link_data_[1]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
64 | manipulator_link_data_[1]->joint_limit_max_ = 0.5 * M_PI;
65 | manipulator_link_data_[1]->joint_limit_min_ = -0.5 * M_PI;
66 | manipulator_link_data_[1]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
67 |
68 | manipulator_link_data_[2]->name_ = "joint2";
69 | manipulator_link_data_[2]->parent_ = 1;
70 | manipulator_link_data_[2]->sibling_ = -1;
71 | manipulator_link_data_[2]->child_ = 3;
72 | manipulator_link_data_[2]->mass_ = 0.94658;
73 | manipulator_link_data_[2]->relative_position_ = robotis_framework::getTransitionXYZ(0.0, 0.06900, 0.033);
74 | manipulator_link_data_[2]->joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 1.0, 0.0);
75 | manipulator_link_data_[2]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
76 | manipulator_link_data_[2]->joint_limit_max_ = 0.5 * M_PI;
77 | manipulator_link_data_[2]->joint_limit_min_ = -0.5 * M_PI;
78 | manipulator_link_data_[2]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
79 |
80 | manipulator_link_data_[3]->name_ = "joint3";
81 | manipulator_link_data_[3]->parent_ = 2;
82 | manipulator_link_data_[3]->sibling_ = -1;
83 | manipulator_link_data_[3]->child_ = 4;
84 | manipulator_link_data_[3]->mass_ = 1.30260;
85 | manipulator_link_data_[3]->relative_position_ = robotis_framework::getTransitionXYZ(0.03000, -0.01150, 0.26400);
86 | manipulator_link_data_[3]->joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 1.0, 0.0);
87 | manipulator_link_data_[3]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
88 | manipulator_link_data_[3]->joint_limit_max_ = 0.5 * M_PI;
89 | manipulator_link_data_[3]->joint_limit_min_ = -0.5 * M_PI;
90 | manipulator_link_data_[3]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
91 |
92 | manipulator_link_data_[4]->name_ = "joint4";
93 | manipulator_link_data_[4]->parent_ = 3;
94 | manipulator_link_data_[4]->sibling_ = -1;
95 | manipulator_link_data_[4]->child_ = 5;
96 | manipulator_link_data_[4]->mass_ = 1.236;
97 | manipulator_link_data_[4]->relative_position_ = robotis_framework::getTransitionXYZ(0.19500, -0.05750, 0.03000);
98 | manipulator_link_data_[4]->joint_axis_ = robotis_framework::getTransitionXYZ(1.0, 0.0, 0.0);
99 | manipulator_link_data_[4]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
100 | manipulator_link_data_[4]->joint_limit_max_ = 0.5 * M_PI;
101 | manipulator_link_data_[4]->joint_limit_min_ = -0.5 * M_PI;
102 | manipulator_link_data_[4]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
103 |
104 | manipulator_link_data_[5]->name_ = "joint5";
105 | manipulator_link_data_[5]->parent_ = 4;
106 | manipulator_link_data_[5]->sibling_ = -1;
107 | manipulator_link_data_[5]->child_ = 6;
108 | manipulator_link_data_[5]->mass_ = 0.491;
109 | manipulator_link_data_[5]->relative_position_ = robotis_framework::getTransitionXYZ(0.06300, 0.04500, 0.00000);
110 | manipulator_link_data_[5]->joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 1.0, 0.0);
111 | manipulator_link_data_[5]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
112 | manipulator_link_data_[5]->joint_limit_max_ = 0.5 * M_PI;
113 | manipulator_link_data_[5]->joint_limit_min_ = -0.5 * M_PI;
114 | manipulator_link_data_[5]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
115 |
116 | manipulator_link_data_[6]->name_ = "joint6";
117 | manipulator_link_data_[6]->parent_ = 5;
118 | manipulator_link_data_[6]->sibling_ = -1;
119 | manipulator_link_data_[6]->child_ = 7;
120 | manipulator_link_data_[6]->mass_ = 0.454;
121 | manipulator_link_data_[6]->relative_position_ = robotis_framework::getTransitionXYZ(0.12300, -0.04500, 0.00000);
122 | manipulator_link_data_[6]->joint_axis_ = robotis_framework::getTransitionXYZ(1.0, 0.0, 0.0);
123 | manipulator_link_data_[6]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
124 | manipulator_link_data_[6]->joint_limit_max_ = 0.5 * M_PI;
125 | manipulator_link_data_[6]->joint_limit_min_ = -0.5 * M_PI;
126 | manipulator_link_data_[6]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
127 |
128 | manipulator_link_data_[7]->name_ = "end";
129 | manipulator_link_data_[7]->parent_ = 6;
130 | manipulator_link_data_[7]->sibling_ = -1;
131 | manipulator_link_data_[7]->child_ = -1;
132 | manipulator_link_data_[7]->mass_ = 0.0;
133 | manipulator_link_data_[7]->relative_position_ = robotis_framework::getTransitionXYZ(0.0115, 0.0, 0.0);
134 | manipulator_link_data_[7]->joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
135 | manipulator_link_data_[7]->center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0);
136 | manipulator_link_data_[7]->joint_limit_max_ = 100.0;
137 | manipulator_link_data_[7]->joint_limit_min_ = -100.0;
138 | manipulator_link_data_[7]->inertia_ = robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0);
139 | }
140 | }
141 |
142 | std::vector ManipulatorKinematicsDynamics::findRoute(int to)
143 | {
144 | int id = manipulator_link_data_[to]->parent_;
145 |
146 | std::vector idx;
147 |
148 | if (id == 0)
149 | {
150 | idx.push_back(0);
151 | idx.push_back(to);
152 | }
153 | else
154 | {
155 | idx = findRoute(id);
156 | idx.push_back(to);
157 | }
158 |
159 | return idx;
160 | }
161 |
162 | std::vector ManipulatorKinematicsDynamics::findRoute(int from, int to)
163 | {
164 | int id = manipulator_link_data_[to]->parent_;
165 |
166 | std::vector idx;
167 |
168 | if (id == from)
169 | {
170 | idx.push_back(from);
171 | idx.push_back(to);
172 | }
173 | else if (id != 0)
174 | {
175 | idx = findRoute(from, id);
176 | idx.push_back(to);
177 | }
178 |
179 | return idx;
180 | }
181 |
182 | double ManipulatorKinematicsDynamics::totalMass(int joint_ID)
183 | {
184 | double mass;
185 |
186 | if (joint_ID == -1)
187 | mass = 0.0;
188 | else
189 | mass = manipulator_link_data_[joint_ID]->mass_ + totalMass(manipulator_link_data_[joint_ID]->sibling_)
190 | + totalMass(manipulator_link_data_[joint_ID]->child_);
191 |
192 | return mass;
193 | }
194 |
195 | Eigen::MatrixXd ManipulatorKinematicsDynamics::calcMC(int joint_ID)
196 | {
197 | Eigen::MatrixXd mc(3, 1);
198 |
199 | if (joint_ID == -1)
200 | mc = Eigen::MatrixXd::Zero(3, 1);
201 | else
202 | {
203 | mc = manipulator_link_data_[joint_ID]->mass_ * (manipulator_link_data_[joint_ID]->orientation_
204 | * manipulator_link_data_[joint_ID]->center_of_mass_ + manipulator_link_data_[joint_ID]->position_);
205 | mc = mc + calcMC(manipulator_link_data_[joint_ID]->sibling_) + calcMC(manipulator_link_data_[joint_ID]->child_);
206 | }
207 |
208 | return mc;
209 | }
210 |
211 | Eigen::MatrixXd ManipulatorKinematicsDynamics::calcCOM(Eigen::MatrixXd MC)
212 | {
213 | double mass;
214 | Eigen::MatrixXd COM(3, 1);
215 |
216 | mass = totalMass(0);
217 |
218 | COM = MC / mass;
219 |
220 | return COM;
221 | }
222 |
223 | void ManipulatorKinematicsDynamics::forwardKinematics(int joint_ID)
224 | {
225 | if (joint_ID == -1)
226 | return;
227 |
228 | manipulator_link_data_[0]->position_ = Eigen::MatrixXd::Zero(3, 1);
229 | manipulator_link_data_[0]->orientation_ = robotis_framework::calcRodrigues(
230 | robotis_framework::calcHatto(manipulator_link_data_[0]->joint_axis_),
231 | manipulator_link_data_[0]->joint_angle_
232 | );
233 |
234 | if (joint_ID != 0)
235 | {
236 | int parent = manipulator_link_data_[joint_ID]->parent_;
237 |
238 | manipulator_link_data_[joint_ID]->position_ = manipulator_link_data_[parent]->orientation_
239 | * manipulator_link_data_[joint_ID]->relative_position_
240 | + manipulator_link_data_[parent]->position_;
241 | manipulator_link_data_[joint_ID]->orientation_ = manipulator_link_data_[parent]->orientation_
242 | * robotis_framework::calcRodrigues(robotis_framework::calcHatto(manipulator_link_data_[joint_ID]->joint_axis_),
243 | manipulator_link_data_[joint_ID]->joint_angle_);
244 |
245 | manipulator_link_data_[joint_ID]->transformation_.block<3, 1>(0, 3) = manipulator_link_data_[joint_ID]->position_;
246 | manipulator_link_data_[joint_ID]->transformation_.block<3, 3>(0, 0) = manipulator_link_data_[joint_ID]->orientation_;
247 | }
248 |
249 | forwardKinematics(manipulator_link_data_[joint_ID]->sibling_);
250 | forwardKinematics(manipulator_link_data_[joint_ID]->child_);
251 | }
252 |
253 | Eigen::MatrixXd ManipulatorKinematicsDynamics::calcJacobian(std::vector idx)
254 | {
255 | int idx_size = idx.size();
256 | int end = idx_size - 1;
257 |
258 | Eigen::MatrixXd tar_position = manipulator_link_data_[idx[end]]->position_;
259 | Eigen::MatrixXd Jacobian = Eigen::MatrixXd::Zero(6, idx_size);
260 |
261 | for (int index = 0; index < idx_size; index++)
262 | {
263 | int id = idx[index];
264 |
265 | Eigen::MatrixXd tar_orientation = manipulator_link_data_[id]->orientation_ * manipulator_link_data_[id]->joint_axis_;
266 |
267 | Jacobian.block(0, index, 3, 1) = robotis_framework::calcCross(tar_orientation,
268 | tar_position - manipulator_link_data_[id]->position_);
269 | Jacobian.block(3, index, 3, 1) = tar_orientation;
270 | }
271 |
272 | return Jacobian;
273 | }
274 |
275 | Eigen::MatrixXd ManipulatorKinematicsDynamics::calcJacobianCOM(std::vector idx)
276 | {
277 | int idx_size = idx.size();
278 | int end = idx_size - 1;
279 |
280 | Eigen::MatrixXd target_position = manipulator_link_data_[idx[end]]->position_;
281 | Eigen::MatrixXd jacobianCOM = Eigen::MatrixXd::Zero(6, idx_size);
282 |
283 | for (int index = 0; index < idx_size; index++)
284 | {
285 | int id = idx[index];
286 | double mass = totalMass(id);
287 |
288 | Eigen::MatrixXd og = calcMC(id) / mass - manipulator_link_data_[id]->position_;
289 | Eigen::MatrixXd target_orientation = manipulator_link_data_[id]->orientation_ * manipulator_link_data_[id]->joint_axis_;
290 |
291 | jacobianCOM.block(0, index, 3, 1) = robotis_framework::calcCross(target_orientation, og);
292 | jacobianCOM.block(3, index, 3, 1) = target_orientation;
293 | }
294 |
295 | return jacobianCOM;
296 | }
297 |
298 | Eigen::MatrixXd ManipulatorKinematicsDynamics::calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position,
299 | Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation)
300 | {
301 | Eigen::MatrixXd pos_err = tar_position - curr_position;
302 | Eigen::MatrixXd ori_err1 = curr_orientation.inverse() * tar_orientation;
303 | Eigen::MatrixXd ori_err2 = curr_orientation * robotis_framework::convertRotToOmega(ori_err1);
304 |
305 | Eigen::MatrixXd err = Eigen::MatrixXd::Zero(6, 1);
306 | err.block(0, 0, 3, 1) = pos_err;
307 | err.block(3, 0, 3, 1) = ori_err2;
308 |
309 | return err;
310 | }
311 |
312 | bool ManipulatorKinematicsDynamics::inverseKinematics(int to, Eigen::MatrixXd tar_position,
313 | Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
314 | {
315 | bool ik_success = false;
316 | bool limit_success = false;
317 |
318 | forwardKinematics(0);
319 |
320 | std::vector idx = findRoute(to);
321 |
322 | for (int iter = 0; iter < max_iter; iter++)
323 | {
324 | Eigen::MatrixXd jacobian = calcJacobian(idx);
325 |
326 | Eigen::MatrixXd curr_position = manipulator_link_data_[to]->position_;
327 | Eigen::MatrixXd curr_orientation = manipulator_link_data_[to]->orientation_;
328 |
329 | Eigen::MatrixXd err = calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
330 |
331 | if (err.norm() < ik_err)
332 | {
333 | ik_success = true;
334 | break;
335 | }
336 | else
337 | ik_success = false;
338 |
339 | Eigen::MatrixXd jacobian2 = jacobian * jacobian.transpose();
340 | Eigen::MatrixXd jacobian3 = jacobian.transpose() * jacobian2.inverse();
341 |
342 | Eigen::MatrixXd _delta_angle = jacobian3 * err;
343 |
344 | for (int id = 0; id < idx.size(); id++)
345 | {
346 | int joint_num = idx[id];
347 | manipulator_link_data_[joint_num]->joint_angle_ += _delta_angle.coeff(id);
348 | }
349 |
350 | forwardKinematics(0);
351 | }
352 |
353 | for (int id = 0; id < idx.size(); id++)
354 | {
355 | int joint_num = idx[id];
356 |
357 | if (manipulator_link_data_[joint_num]->joint_angle_ >= manipulator_link_data_[joint_num]->joint_limit_max_)
358 | {
359 | limit_success = false;
360 | break;
361 | }
362 | else if (manipulator_link_data_[joint_num]->joint_angle_ <= manipulator_link_data_[joint_num]->joint_limit_min_)
363 | {
364 | limit_success = false;
365 | break;
366 | }
367 | else
368 | limit_success = true;
369 | }
370 |
371 | if (ik_success == true && limit_success == true)
372 | return true;
373 | else
374 | return false;
375 | }
376 |
377 | bool ManipulatorKinematicsDynamics::inverseKinematics(int from, int to, Eigen::MatrixXd tar_position,
378 | Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
379 | {
380 | bool ik_success = false;
381 | bool limit_success = false;
382 |
383 | forwardKinematics(0);
384 |
385 | std::vector idx = findRoute(from, to);
386 |
387 | for (int iter = 0; iter < max_iter; iter++)
388 | {
389 | Eigen::MatrixXd jacobian = calcJacobian(idx);
390 |
391 | Eigen::MatrixXd curr_position = manipulator_link_data_[to]->position_;
392 | Eigen::MatrixXd curr_orientation = manipulator_link_data_[to]->orientation_;
393 |
394 | Eigen::MatrixXd err = calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
395 |
396 | if (err.norm() < ik_err)
397 | {
398 | ik_success = true;
399 | break;
400 | }
401 | else
402 | {
403 | ik_success = false;
404 | }
405 |
406 | Eigen::MatrixXd jacobian2 = jacobian * jacobian.transpose();
407 | Eigen::MatrixXd jacobian3 = jacobian.transpose() * jacobian2.inverse();
408 |
409 | Eigen::MatrixXd delta_angle = jacobian3 * err;
410 |
411 | for (int id = 0; id < idx.size(); id++)
412 | {
413 | int joint_num = idx[id];
414 | manipulator_link_data_[joint_num]->joint_angle_ += delta_angle.coeff(id);
415 | }
416 |
417 | forwardKinematics(0);
418 | }
419 |
420 | for (int id = 0; id < idx.size(); id++)
421 | {
422 | int joint_num = idx[id];
423 |
424 | if (manipulator_link_data_[joint_num]->joint_angle_ >= manipulator_link_data_[joint_num]->joint_limit_max_)
425 | {
426 | limit_success = false;
427 | break;
428 | }
429 | else if (manipulator_link_data_[joint_num]->joint_angle_ <= manipulator_link_data_[joint_num]->joint_limit_min_)
430 | {
431 | limit_success = false;
432 | break;
433 | }
434 | else
435 | limit_success = true;
436 | }
437 |
438 | if (ik_success == true && limit_success == true)
439 | return true;
440 | else
441 | return false;
442 | }
443 |
444 | }
445 |
--------------------------------------------------------------------------------
/manipulator_h_base_module/src/base_module.cpp:
--------------------------------------------------------------------------------
1 | /*******************************************************************************
2 | * Copyright 2018 ROBOTIS CO., LTD.
3 | *
4 | * Licensed under the Apache License, Version 2.0 (the "License");
5 | * you may not use this file except in compliance with the License.
6 | * You may obtain a copy of the License at
7 | *
8 | * http://www.apache.org/licenses/LICENSE-2.0
9 | *
10 | * Unless required by applicable law or agreed to in writing, software
11 | * distributed under the License is distributed on an "AS IS" BASIS,
12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 | * See the License for the specific language governing permissions and
14 | * limitations under the License.
15 | *******************************************************************************/
16 |
17 | /*
18 | * ThorManipulation.cpp
19 | *
20 | * Created on: 2016. 1. 18.
21 | * Author: Zerom
22 | */
23 |
24 | #include
25 |
26 | #include "manipulator_h_base_module/base_module.h"
27 |
28 | using namespace robotis_manipulator_h;
29 |
30 | BaseModule::BaseModule()
31 | : control_cycle_msec_(0)
32 | {
33 | enable_ = false;
34 | module_name_ = "base_module";
35 | control_mode_ = robotis_framework::PositionControl;
36 |
37 | tra_gene_thread_ = NULL;
38 |
39 | result_["joint1"] = new robotis_framework::DynamixelState();
40 | result_["joint2"] = new robotis_framework::DynamixelState();
41 | result_["joint3"] = new robotis_framework::DynamixelState();
42 | result_["joint4"] = new robotis_framework::DynamixelState();
43 | result_["joint5"] = new robotis_framework::DynamixelState();
44 | result_["joint6"] = new robotis_framework::DynamixelState();
45 |
46 | joint_name_to_id_["joint1"] = 1;
47 | joint_name_to_id_["joint2"] = 2;
48 | joint_name_to_id_["joint3"] = 3;
49 | joint_name_to_id_["joint4"] = 4;
50 | joint_name_to_id_["joint5"] = 5;
51 | joint_name_to_id_["joint6"] = 6;
52 |
53 | robotis_ = new RobotisState();
54 | joint_state_ = new BaseJointState();
55 | manipulator_ = new ManipulatorKinematicsDynamics(ARM);
56 | }
57 |
58 | BaseModule::~BaseModule()
59 | {
60 | queue_thread_.join();
61 | }
62 |
63 | void BaseModule::initialize(const int control_cycle_msec, robotis_framework::Robot *robot)
64 | {
65 | control_cycle_msec_ = control_cycle_msec;
66 | queue_thread_ = boost::thread(boost::bind(&BaseModule::queueThread, this));
67 | }
68 |
69 | void BaseModule::parseIniPoseData(const std::string &path)
70 | {
71 | YAML::Node doc;
72 | try
73 | {
74 | // load yaml
75 | doc = YAML::LoadFile(path.c_str());
76 | } catch (const std::exception& e)
77 | {
78 | ROS_ERROR("Fail to load yaml file.");
79 | return;
80 | }
81 |
82 | // parse movement time
83 | double _mov_time;
84 | _mov_time = doc["mov_time"].as();
85 |
86 | robotis_->mov_time_ = _mov_time;
87 |
88 | // parse target pose
89 | YAML::Node _tar_pose_node = doc["tar_pose"];
90 | for (YAML::iterator _it = _tar_pose_node.begin(); _it != _tar_pose_node.end(); ++_it)
91 | {
92 | int _id;
93 | double _value;
94 |
95 | _id = _it->first.as();
96 | _value = _it->second.as();
97 |
98 | robotis_->joint_ini_pose_.coeffRef(_id, 0) = _value * DEGREE2RADIAN;
99 | }
100 |
101 | robotis_->all_time_steps_ = int(robotis_->mov_time_ / robotis_->smp_time_) + 1;
102 | robotis_->calc_joint_tra_.resize(robotis_->all_time_steps_, MAX_JOINT_ID + 1);
103 | }
104 |
105 | void BaseModule::queueThread()
106 | {
107 | ros::NodeHandle ros_node;
108 | ros::CallbackQueue callback_queue;
109 |
110 | ros_node.setCallbackQueue(&callback_queue);
111 |
112 | /* publish topics */
113 | status_msg_pub_ = ros_node.advertise("/robotis/status", 1);
114 | set_ctrl_module_pub_ = ros_node.advertise("/robotis/enable_ctrl_module", 1);
115 |
116 | /* subscribe topics */
117 | ros::Subscriber ini_pose_msg_sub = ros_node.subscribe("/robotis/base/ini_pose_msg", 5,
118 | &BaseModule::initPoseMsgCallback, this);
119 | ros::Subscriber set_mode_msg_sub = ros_node.subscribe("/robotis/base/set_mode_msg", 5,
120 | &BaseModule::setModeMsgCallback, this);
121 |
122 | ros::Subscriber joint_pose_msg_sub = ros_node.subscribe("/robotis/base/joint_pose_msg", 5,
123 | &BaseModule::jointPoseMsgCallback, this);
124 | ros::Subscriber kinematics_pose_msg_sub = ros_node.subscribe("/robotis/base/kinematics_pose_msg", 5,
125 | &BaseModule::kinematicsPoseMsgCallback, this);
126 |
127 | ros::ServiceServer get_joint_pose_server = ros_node.advertiseService("/robotis/base/get_joint_pose",
128 | &BaseModule::getJointPoseCallback, this);
129 | ros::ServiceServer get_kinematics_pose_server = ros_node.advertiseService("/robotis/base/get_kinematics_pose",
130 | &BaseModule::getKinematicsPoseCallback, this);
131 |
132 | while (ros_node.ok())
133 | {
134 | callback_queue.callAvailable();
135 | usleep(1000);
136 | }
137 | }
138 |
139 | void BaseModule::initPoseMsgCallback(const std_msgs::String::ConstPtr& msg)
140 | {
141 | if (enable_ == false)
142 | return;
143 |
144 | if (robotis_->is_moving_ == false)
145 | {
146 | if (msg->data == "ini_pose")
147 | {
148 | // parse initial pose
149 | std::string ini_pose_path = ros::package::getPath("manipulator_h_base_module") + "/config/ini_pose.yaml";
150 | parseIniPoseData(ini_pose_path);
151 |
152 | tra_gene_thread_ = new boost::thread(boost::bind(&BaseModule::generateInitPoseTrajProcess, this));
153 | delete tra_gene_thread_;
154 | }
155 | }
156 | else
157 | {
158 | ROS_INFO("previous task is alive");
159 | }
160 |
161 | return;
162 | }
163 |
164 | void BaseModule::setModeMsgCallback(const std_msgs::String::ConstPtr& msg)
165 | {
166 | std_msgs::String str_msg;
167 | str_msg.data = "base_module";
168 |
169 | set_ctrl_module_pub_.publish(str_msg);
170 |
171 | return;
172 | }
173 |
174 | bool BaseModule::getJointPoseCallback(manipulator_h_base_module_msgs::GetJointPose::Request &req,
175 | manipulator_h_base_module_msgs::GetJointPose::Response &res)
176 | {
177 | if (enable_ == false)
178 | return false;
179 |
180 | for (int id = 1; id <= MAX_JOINT_ID; id++)
181 | {
182 | for (int name_index = 0; name_index < req.joint_name.size(); name_index++)
183 | {
184 | if (manipulator_->manipulator_link_data_[id]->name_ == req.joint_name[name_index])
185 | {
186 | res.joint_name.push_back(manipulator_->manipulator_link_data_[id]->name_);
187 | res.joint_value.push_back(joint_state_->goal_joint_state_[id].position_);
188 |
189 | break;
190 | }
191 | }
192 | }
193 |
194 | return true;
195 | }
196 |
197 | bool BaseModule::getKinematicsPoseCallback(manipulator_h_base_module_msgs::GetKinematicsPose::Request &req,
198 | manipulator_h_base_module_msgs::GetKinematicsPose::Response &res)
199 | {
200 | if (enable_ == false)
201 | return false;
202 |
203 | res.group_pose.position.x = manipulator_->manipulator_link_data_[END_LINK]->position_.coeff(0, 0);
204 | res.group_pose.position.y = manipulator_->manipulator_link_data_[END_LINK]->position_.coeff(1, 0);
205 | res.group_pose.position.z = manipulator_->manipulator_link_data_[END_LINK]->position_.coeff(2, 0);
206 |
207 | Eigen::Quaterniond quaternion = robotis_framework::convertRotationToQuaternion(manipulator_->manipulator_link_data_[END_LINK]->orientation_);
208 |
209 | res.group_pose.orientation.w = quaternion.w();
210 | res.group_pose.orientation.x = quaternion.x();
211 | res.group_pose.orientation.y = quaternion.y();
212 | res.group_pose.orientation.z = quaternion.z();
213 |
214 | return true;
215 | }
216 |
217 | void BaseModule::kinematicsPoseMsgCallback(const manipulator_h_base_module_msgs::KinematicsPose::ConstPtr& msg)
218 | {
219 | if (enable_ == false)
220 | return;
221 |
222 | robotis_->kinematics_pose_msg_ = *msg;
223 |
224 | robotis_->ik_id_start_ = 0;
225 | robotis_->ik_id_end_ = END_LINK;
226 |
227 | if (robotis_->is_moving_ == false)
228 | {
229 | tra_gene_thread_ = new boost::thread(boost::bind(&BaseModule::generateTaskTrajProcess, this));
230 | delete tra_gene_thread_;
231 | }
232 | else
233 | {
234 | ROS_INFO("previous task is alive");
235 | }
236 |
237 | return;
238 | }
239 |
240 | void BaseModule::jointPoseMsgCallback(const manipulator_h_base_module_msgs::JointPose::ConstPtr& msg)
241 | {
242 | if (enable_ == false)
243 | return;
244 |
245 | robotis_->joint_pose_msg_ = *msg;
246 |
247 | if (robotis_->is_moving_ == false)
248 | {
249 | tra_gene_thread_ = new boost::thread(boost::bind(&BaseModule::generateJointTrajProcess, this));
250 | delete tra_gene_thread_;
251 | }
252 | else
253 | {
254 | ROS_INFO("previous task is alive");
255 | }
256 |
257 | return;
258 | }
259 |
260 | void BaseModule::generateInitPoseTrajProcess()
261 | {
262 | if (enable_ == false)
263 | return;
264 |
265 | for (int id = 1; id <= MAX_JOINT_ID; id++)
266 | {
267 | double ini_value = joint_state_->goal_joint_state_[id].position_;
268 | double tar_value = robotis_->joint_ini_pose_.coeff(id, 0);
269 |
270 | Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
271 | robotis_->smp_time_, robotis_->mov_time_);
272 |
273 | robotis_->calc_joint_tra_.block(0, id, robotis_->all_time_steps_, 1) = tra;
274 | }
275 |
276 | robotis_->cnt_ = 0;
277 | robotis_->is_moving_ = true;
278 |
279 | ROS_INFO("[start] send trajectory");
280 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Start Trajectory");
281 | }
282 |
283 | void BaseModule::generateJointTrajProcess()
284 | {
285 | if (enable_ == false)
286 | return;
287 |
288 | /* set movement time */
289 | double tol = 35 * DEGREE2RADIAN; // rad per sec
290 | double mov_time = 2.0;
291 |
292 | double max_diff, abs_diff;
293 | max_diff = 0.0;
294 |
295 | for (int name_index = 0; name_index < robotis_->joint_pose_msg_.name.size(); name_index++)
296 | {
297 | double ini_value;
298 | double tar_value;
299 |
300 | for (int id = 1; id <= MAX_JOINT_ID; id++)
301 | {
302 | if (manipulator_->manipulator_link_data_[id]->name_ == robotis_->joint_pose_msg_.name[name_index])
303 | {
304 | ini_value = joint_state_->goal_joint_state_[id].position_;
305 | tar_value = robotis_->joint_pose_msg_.value[name_index];
306 |
307 | break;
308 | }
309 | }
310 |
311 | abs_diff = fabs(tar_value - ini_value);
312 |
313 | if (max_diff < abs_diff)
314 | max_diff = abs_diff;
315 | }
316 |
317 | robotis_->mov_time_ = max_diff / tol;
318 | int all_time_steps = int(floor((robotis_->mov_time_ / robotis_->smp_time_) + 1.0));
319 | robotis_->mov_time_ = double(all_time_steps - 1) * robotis_->smp_time_;
320 |
321 | if (robotis_->mov_time_ < mov_time)
322 | robotis_->mov_time_ = mov_time;
323 |
324 | robotis_->all_time_steps_ = int(robotis_->mov_time_ / robotis_->smp_time_) + 1;
325 |
326 | robotis_->calc_joint_tra_.resize(robotis_->all_time_steps_, MAX_JOINT_ID + 1);
327 |
328 | /* calculate joint trajectory */
329 | for (int id = 1; id <= MAX_JOINT_ID; id++)
330 | {
331 | double ini_value = joint_state_->goal_joint_state_[id].position_;
332 | double tar_value;
333 |
334 | for (int name_index = 0; name_index < robotis_->joint_pose_msg_.name.size(); name_index++)
335 | {
336 | if (manipulator_->manipulator_link_data_[id]->name_ == robotis_->joint_pose_msg_.name[name_index])
337 | {
338 | tar_value = robotis_->joint_pose_msg_.value[name_index];
339 | break;
340 | }
341 | }
342 |
343 | Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
344 | robotis_->smp_time_, robotis_->mov_time_);
345 |
346 | robotis_->calc_joint_tra_.block(0, id, robotis_->all_time_steps_, 1) = tra;
347 | }
348 |
349 | robotis_->cnt_ = 0;
350 | robotis_->is_moving_ = true;
351 |
352 | ROS_INFO("[start] send trajectory");
353 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Start Trajectory");
354 | }
355 |
356 | void BaseModule::generateTaskTrajProcess()
357 | {
358 | /* set movement time */
359 | double tol = 0.1; // m per sec
360 | double mov_time = 2.0;
361 |
362 | double diff = sqrt(
363 | pow(manipulator_->manipulator_link_data_[robotis_->ik_id_end_]->position_.coeff(0, 0)
364 | - robotis_->kinematics_pose_msg_.pose.position.x, 2)
365 | + pow(manipulator_->manipulator_link_data_[robotis_->ik_id_end_]->position_.coeff(1, 0)
366 | - robotis_->kinematics_pose_msg_.pose.position.y, 2)
367 | + pow(manipulator_->manipulator_link_data_[robotis_->ik_id_end_]->position_.coeff(2, 0)
368 | - robotis_->kinematics_pose_msg_.pose.position.z, 2)
369 | );
370 |
371 | robotis_->mov_time_ = diff / tol;
372 | int all_time_steps = int(floor((robotis_->mov_time_ / robotis_->smp_time_) + 1.0));
373 | robotis_->mov_time_ = double(all_time_steps - 1) * robotis_->smp_time_;
374 |
375 | if (robotis_->mov_time_ < mov_time)
376 | robotis_->mov_time_ = mov_time;
377 |
378 | robotis_->all_time_steps_ = int(robotis_->mov_time_ / robotis_->smp_time_) + 1;
379 |
380 | robotis_->calc_task_tra_.resize(robotis_->all_time_steps_, 3);
381 |
382 | /* calculate trajectory */
383 | for (int dim = 0; dim < 3; dim++)
384 | {
385 | double ini_value = manipulator_->manipulator_link_data_[robotis_->ik_id_end_]->position_.coeff(dim, 0);
386 | double tar_value;
387 |
388 | if (dim == 0)
389 | tar_value = robotis_->kinematics_pose_msg_.pose.position.x;
390 | else if (dim == 1)
391 | tar_value = robotis_->kinematics_pose_msg_.pose.position.y;
392 | else if (dim == 2)
393 | tar_value = robotis_->kinematics_pose_msg_.pose.position.z;
394 |
395 | Eigen::MatrixXd tra = robotis_framework::calcMinimumJerkTra(ini_value, 0.0, 0.0, tar_value, 0.0, 0.0,
396 | robotis_->smp_time_, robotis_->mov_time_);
397 |
398 | robotis_->calc_task_tra_.block(0, dim, robotis_->all_time_steps_, 1) = tra;
399 | }
400 |
401 | robotis_->cnt_ = 0;
402 | robotis_->is_moving_ = true;
403 | robotis_->ik_solve_ = true;
404 |
405 | ROS_INFO("[start] send trajectory");
406 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Start Trajectory");
407 | }
408 |
409 | void BaseModule::process(std::map dxls,
410 | std::map sensors)
411 | {
412 | if (enable_ == false)
413 | return;
414 |
415 | /*----- write curr position -----*/
416 |
417 | for (std::map::iterator state_iter = result_.begin();
418 | state_iter != result_.end(); state_iter++)
419 | {
420 | std::string joint_name = state_iter->first;
421 |
422 | robotis_framework::Dynamixel *dxl = NULL;
423 | std::map::iterator dxl_it = dxls.find(joint_name);
424 | if (dxl_it != dxls.end())
425 | dxl = dxl_it->second;
426 | else
427 | continue;
428 |
429 | double joint_curr_position = dxl->dxl_state_->present_position_;
430 | double joint_goal_position = dxl->dxl_state_->goal_position_;
431 |
432 | joint_state_->curr_joint_state_[joint_name_to_id_[joint_name]].position_ = joint_curr_position;
433 | joint_state_->goal_joint_state_[joint_name_to_id_[joint_name]].position_ = joint_goal_position;
434 | }
435 |
436 | /*----- forward kinematics -----*/
437 |
438 | for (int id = 1; id <= MAX_JOINT_ID; id++)
439 | manipulator_->manipulator_link_data_[id]->joint_angle_ = joint_state_->goal_joint_state_[id].position_;
440 |
441 | manipulator_->forwardKinematics(0);
442 |
443 | /* ----- send trajectory ----- */
444 |
445 | // ros::Time time = ros::Time::now();
446 | if (robotis_->is_moving_ == true)
447 | {
448 | if (robotis_->cnt_ == 0)
449 | robotis_->ik_start_rotation_ = manipulator_->manipulator_link_data_[robotis_->ik_id_end_]->orientation_;
450 |
451 | if (robotis_->ik_solve_ == true)
452 | {
453 | robotis_->setInverseKinematics(robotis_->cnt_, robotis_->ik_start_rotation_);
454 |
455 | int max_iter = 30;
456 | double ik_tol = 1e-3;
457 | bool ik_success = manipulator_->inverseKinematics(robotis_->ik_id_start_, robotis_->ik_id_end_,
458 | robotis_->ik_target_position_, robotis_->ik_target_rotation_, max_iter, ik_tol);
459 |
460 | if (ik_success == true)
461 | {
462 | for (int id = 1; id <= MAX_JOINT_ID; id++)
463 | joint_state_->goal_joint_state_[id].position_ = manipulator_->manipulator_link_data_[id]->joint_angle_;
464 | }
465 | else
466 | {
467 | ROS_INFO("[end] send trajectory (ik failed)");
468 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "End Trajectory (IK Failed)");
469 |
470 | robotis_->is_moving_ = false;
471 | robotis_->ik_solve_ = false;
472 | robotis_->cnt_ = 0;
473 | }
474 | }
475 | else
476 | {
477 | for (int id = 1; id <= MAX_JOINT_ID; id++)
478 | joint_state_->goal_joint_state_[id].position_ = robotis_->calc_joint_tra_(robotis_->cnt_, id);
479 | }
480 |
481 | robotis_->cnt_++;
482 | }
483 |
484 | /*----- set joint data -----*/
485 |
486 | for (std::map::iterator state_iter = result_.begin();
487 | state_iter != result_.end(); state_iter++)
488 | {
489 | std::string joint_name = state_iter->first;
490 | result_[joint_name]->goal_position_ = joint_state_->goal_joint_state_[joint_name_to_id_[joint_name]].position_;
491 | }
492 |
493 | /*---------- initialize count number ----------*/
494 |
495 | if (robotis_->cnt_ >= robotis_->all_time_steps_ && robotis_->is_moving_ == true)
496 | {
497 | ROS_INFO("[end] send trajectory");
498 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "End Trajectory");
499 |
500 | robotis_->is_moving_ = false;
501 | robotis_->ik_solve_ = false;
502 | robotis_->cnt_ = 0;
503 | }
504 | }
505 |
506 | void BaseModule::stop()
507 | {
508 | robotis_->is_moving_ = false;
509 | robotis_->ik_solve_ = false;
510 | robotis_->cnt_ = 0;
511 |
512 | return;
513 | }
514 |
515 | bool BaseModule::isRunning()
516 | {
517 | return robotis_->is_moving_;
518 | }
519 |
520 | void BaseModule::publishStatusMsg(unsigned int type, std::string msg)
521 | {
522 | robotis_controller_msgs::StatusMsg status;
523 | status.header.stamp = ros::Time::now();
524 | status.type = type;
525 | status.module_name = "Base";
526 | status.status_msg = msg;
527 |
528 | status_msg_pub_.publish(status);
529 | }
530 |
--------------------------------------------------------------------------------
/manipulator_h_gui/ui/main_window.ui:
--------------------------------------------------------------------------------
1 |
2 |
3 | MainWindowDesign
4 |
5 |
6 |
7 | 0
8 | 0
9 | 1047
10 | 612
11 |
12 |
13 |
14 | QRosApp
15 |
16 |
17 |
18 | :/images/icon.png:/images/icon.png
19 |
20 |
21 |
22 |
23 |
24 |
25 | -
26 |
27 |
28 |
29 | 100
30 | 0
31 |
32 |
33 |
34 |
35 |
36 |
37 | 0
38 |
39 |
40 |
41 | Ros Communications
42 |
43 |
44 |
-
45 |
46 |
47 |
48 | 0
49 | 0
50 |
51 |
52 |
53 | Logging
54 |
55 |
56 |
-
57 |
58 |
59 |
60 |
61 |
62 |
63 |
64 |
65 |
66 |
67 |
68 |
90 |
91 |
92 |
93 |
94 | 0
95 | 0
96 |
97 |
98 |
99 |
100 | 500
101 | 560
102 |
103 |
104 |
105 |
106 | 524287
107 | 10000
108 |
109 |
110 |
111 | Qt::RightDockWidgetArea
112 |
113 |
114 | Command Panel
115 |
116 |
117 | 2
118 |
119 |
120 |
121 | -
122 |
123 |
124 |
125 | 0
126 | 0
127 |
128 |
129 |
130 |
131 | 16777215
132 | 600
133 |
134 |
135 |
136 | QFrame::StyledPanel
137 |
138 |
139 | QFrame::Raised
140 |
141 |
142 |
-
143 |
144 |
145 | Task Space Control
146 |
147 |
148 |
-
149 |
150 |
151 | Get current pose
152 |
153 |
154 |
155 | -
156 |
157 |
158 | position x [m]
159 |
160 |
161 |
162 | -
163 |
164 |
165 | 3
166 |
167 |
168 | -10.000000000000000
169 |
170 |
171 | 10.000000000000000
172 |
173 |
174 | 0.010000000000000
175 |
176 |
177 |
178 | -
179 |
180 |
181 | position y [m]
182 |
183 |
184 |
185 | -
186 |
187 |
188 | 3
189 |
190 |
191 | -10.000000000000000
192 |
193 |
194 | 10.000000000000000
195 |
196 |
197 | 0.010000000000000
198 |
199 |
200 |
201 | -
202 |
203 |
204 | position z [m]
205 |
206 |
207 |
208 | -
209 |
210 |
211 | 3
212 |
213 |
214 | -10.000000000000000
215 |
216 |
217 | 10.000000000000000
218 |
219 |
220 | 0.010000000000000
221 |
222 |
223 |
224 | -
225 |
226 |
227 | orientation roll [deg]
228 |
229 |
230 |
231 | -
232 |
233 |
234 | 3
235 |
236 |
237 | -360.000000000000000
238 |
239 |
240 | 360.000000000000000
241 |
242 |
243 | 0.500000000000000
244 |
245 |
246 |
247 | -
248 |
249 |
250 | orientation pitch [deg]
251 |
252 |
253 |
254 | -
255 |
256 |
257 | 3
258 |
259 |
260 | -360.000000000000000
261 |
262 |
263 | 360.000000000000000
264 |
265 |
266 | 0.500000000000000
267 |
268 |
269 |
270 | -
271 |
272 |
273 | orientation yaw [deg]
274 |
275 |
276 |
277 | -
278 |
279 |
280 | 3
281 |
282 |
283 | -360.000000000000000
284 |
285 |
286 | 360.000000000000000
287 |
288 |
289 | 0.500000000000000
290 |
291 |
292 |
293 | -
294 |
295 |
296 | Send Des Pos.
297 |
298 |
299 |
300 |
301 |
302 |
303 | -
304 |
305 |
306 | Joint Space Control
307 |
308 |
309 |
-
310 |
311 |
312 | Get current joint values
313 |
314 |
315 |
316 | -
317 |
318 |
319 | Joint 1 [deg]
320 |
321 |
322 |
323 | -
324 |
325 |
326 | -180.000000000000000
327 |
328 |
329 | 180.000000000000000
330 |
331 |
332 | 0.500000000000000
333 |
334 |
335 |
336 | -
337 |
338 |
339 | Joint 2 [deg]
340 |
341 |
342 |
343 | -
344 |
345 |
346 | -180.000000000000000
347 |
348 |
349 | 180.000000000000000
350 |
351 |
352 | 0.500000000000000
353 |
354 |
355 |
356 | -
357 |
358 |
359 | Joint 3 [deg]
360 |
361 |
362 |
363 | -
364 |
365 |
366 | -180.000000000000000
367 |
368 |
369 | 180.000000000000000
370 |
371 |
372 | 0.500000000000000
373 |
374 |
375 |
376 | -
377 |
378 |
379 | Joint 4 [deg]
380 |
381 |
382 |
383 | -
384 |
385 |
386 | -180.000000000000000
387 |
388 |
389 | 180.000000000000000
390 |
391 |
392 | 0.500000000000000
393 |
394 |
395 |
396 | -
397 |
398 |
399 | Joint 5 [deg]
400 |
401 |
402 |
403 | -
404 |
405 |
406 | -180.000000000000000
407 |
408 |
409 | 180.000000000000000
410 |
411 |
412 | 0.500000000000000
413 |
414 |
415 |
416 | -
417 |
418 |
419 | Joint 6 [deg]
420 |
421 |
422 |
423 | -
424 |
425 |
426 | -360.000000000000000
427 |
428 |
429 | 360.000000000000000
430 |
431 |
432 | 0.500000000000000
433 |
434 |
435 |
436 | -
437 |
438 |
439 | Send Des Joint Val.
440 |
441 |
442 |
443 |
444 |
445 |
446 |
447 |
448 |
449 | -
450 |
451 |
452 | true
453 |
454 |
455 |
456 | 50
457 | 50
458 |
459 |
460 |
461 | QFrame::StyledPanel
462 |
463 |
464 | QFrame::Raised
465 |
466 |
467 |
-
468 |
469 |
470 | Set Mode
471 |
472 |
473 |
474 | -
475 |
476 |
477 | Go to Initial Pose
478 |
479 |
480 |
481 | -
482 |
483 |
484 | Quit
485 |
486 |
487 |
488 |
489 |
490 |
491 |
492 |
493 |
494 |
495 |
496 | &Quit
497 |
498 |
499 | Ctrl+Q
500 |
501 |
502 | Qt::ApplicationShortcut
503 |
504 |
505 |
506 |
507 | &Preferences
508 |
509 |
510 |
511 |
512 | &About
513 |
514 |
515 |
516 |
517 | About &Qt
518 |
519 |
520 |
521 |
522 |
523 |
524 |
525 |
526 | action_Quit
527 | triggered()
528 | MainWindowDesign
529 | close()
530 |
531 |
532 | -1
533 | -1
534 |
535 |
536 | 399
537 | 299
538 |
539 |
540 |
541 |
542 | quit_button
543 | clicked()
544 | MainWindowDesign
545 | close()
546 |
547 |
548 | 859
549 | 552
550 |
551 |
552 | 469
553 | 299
554 |
555 |
556 |
557 |
558 |
559 |
--------------------------------------------------------------------------------