├── .gitignore ├── .travis.yml ├── .uuv_ci_config ├── install_gazebo7.sh ├── install_gazebo9.sh ├── ros_indigo.sh ├── ros_indigo_dependencies.txt ├── ros_kinetic.sh ├── ros_lunar.sh ├── run_tests.sh └── uuv_dependencies.sh ├── CONTRIBUTING.md ├── LICENSE ├── NOTICE ├── README.md ├── oberon4 ├── oberon4 │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml ├── oberon4_control │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ ├── joint_control.launch │ │ └── joint_effort_controllers.launch │ └── package.xml └── oberon4_description │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── meshes │ ├── Arm.dae │ ├── Azimuth.dae │ ├── Base.dae │ ├── GripperBase.dae │ ├── Jaw.dae │ ├── Jaw.stl │ └── WristStator.dae │ ├── package.xml │ ├── params │ ├── gripper_config.yaml │ └── robot_config.yaml │ ├── robots │ └── oberon4_default.xacro │ └── urdf │ ├── serial_arm.gazebo.xacro │ └── serial_arm.xacro ├── oberon7 ├── oberon7 │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml ├── oberon7_control │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── launch │ │ ├── gripper_controllers.launch │ │ ├── joint_control.launch │ │ ├── joint_effort_controllers.launch │ │ └── jt_cartesian_controller.launch │ └── package.xml └── oberon7_description │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── meshes │ ├── parallel_gripper │ │ └── visual │ │ │ ├── BaseGripper.dae │ │ │ ├── Finger.dae │ │ │ └── FingerTip.dae │ └── serial_arm │ │ ├── collision │ │ └── Base.stl │ │ └── visual │ │ ├── Base.dae │ │ ├── ElbowLink.dae │ │ ├── Forearm.dae │ │ ├── ShoulderLink.dae │ │ ├── UpperArm.dae │ │ └── Wrist.dae │ ├── models │ └── oberon7 │ │ ├── gen_sdf.sh │ │ ├── model.config │ │ ├── model.sdf │ │ └── model.urdf │ ├── package.xml │ ├── params │ ├── parallel_gripper_config.yaml │ └── robot_config.yaml │ ├── robots │ ├── oberon7_default.xacro │ └── oberon7_standalone.xacro │ └── urdf │ ├── parallel_gripper.xacro │ ├── parameters │ ├── parallel_gripper.xacro │ └── serial_arm.xacro │ ├── serial_arm.gazebo.xacro │ └── serial_arm.xacro ├── uuv_manipulators_control ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── scripts │ ├── gripper_controller.py │ ├── joint_position_controller.py │ ├── jt_cartesian_controller.py │ └── set_joint_config.py ├── setup.py └── src │ └── uuv_manipulators_control │ ├── __init__.py │ └── cartesian_controller.py ├── uuv_manipulators_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── urdf │ └── common.xacro ├── uuv_manipulators_kinematics ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── scripts │ └── kinematics_service.py ├── setup.py ├── src │ ├── uuv_kinematics_utils │ │ ├── __init__.py │ │ └── endeffector_state.py │ ├── uuv_manipulator_interfaces │ │ ├── __init__.py │ │ ├── arm.py │ │ ├── gripper.py │ │ └── kin_chain.py │ └── uuv_manipulator_nodes │ │ ├── __init__.py │ │ └── endpoint_state.py └── test │ ├── test_arm_interface.py │ └── test_kinematic_interfaces.test └── uuv_manipulators_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg ├── ArmConfigCommand.msg ├── EndPointState.msg ├── EndeffectorCommand.msg └── EndeffectorState.msg ├── package.xml └── srv └── SolveIK.srv /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | .clang_complete 4 | .idea 5 | docs 6 | .vscode 7 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | env: 7 | global: 8 | - LIBGL_ALWAYS_SOFTWARE=1 9 | - VERBOSE_OUTPUT='false' 10 | - CATKIN_CONFIG='--no-install' 11 | - CATKIN_PARALLEL_JOBS='-p1' 12 | - ROS_PARALLEL_JOBS='-j1' 13 | - AFTER_SCRIPT='sh .uuv_ci_config/run_tests.sh' 14 | matrix: 15 | include: 16 | - env: USE_DEB=true ROS_DISTRO=indigo ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh' ROSDEP_SKIP_KEYS="gazebo gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control gazebo_ros_pkgs" 17 | - env: USE_DEB=true ROS_DISTRO=kinetic ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh' 18 | - env: USE_DEB=true ROS_DISTRO=kinetic ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh && sh .uuv_ci_config/install_gazebo9.sh' ROSDEP_SKIP_KEYS="gazebo gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control gazebo_ros_pkgs" 19 | - env: USE_DEB=true ROS_DISTRO=lunar ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh' 20 | allow_failures: 21 | - env: USE_DEB=true ROS_DISTRO=indigo ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh' ROSDEP_SKIP_KEYS="gazebo gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control gazebo_ros_pkgs" 22 | - env: USE_DEB=true ROS_DISTRO=kinetic ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh && sh .uuv_ci_config/install_gazebo9.sh' ROSDEP_SKIP_KEYS="gazebo gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control gazebo_ros_pkgs" 23 | - env: USE_DEB=true ROS_DISTRO=lunar ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu NOT_TEST_BUILD=true NOT_TEST_INSTALL=true BEFORE_SCRIPT='sh .uuv_ci_config/ros_$ROS_DISTRO.sh' 24 | install: 25 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 26 | script: 27 | - source .ci_config/travis.sh 28 | notifications: 29 | webhooks: 30 | urls: 31 | - https://webhooks.gitter.im/e/a8c9c6426a721fbb8341 32 | on_success: change 33 | on_failure: always 34 | on_start: never 35 | -------------------------------------------------------------------------------- /.uuv_ci_config/install_gazebo7.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 18 | 19 | wget http://packages.osrfoundation.org/gazebo.key -O /tmp/gazebo.key 20 | 21 | apt-key add /tmp/gazebo.key 22 | 23 | apt update 24 | 25 | apt -qq install --no-install-recommends --allow-unauthenticated -y gazebo7 libgazebo7-dev ros-indigo-gazebo7-* 26 | 27 | source /usr/share/gazebo-7/setup.sh 28 | -------------------------------------------------------------------------------- /.uuv_ci_config/install_gazebo9.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' 18 | 19 | wget http://packages.osrfoundation.org/gazebo.key -O /tmp/gazebo.key 20 | 21 | apt-key add /tmp/gazebo.key 22 | 23 | apt update 24 | 25 | apt -qq install --no-install-recommends --allow-unauthenticated -y gazebo9 libgazebo9-dev ros-kinetic-gazebo9-* 26 | 27 | # source /usr/share/gazebo-9/setup.sh 28 | -------------------------------------------------------------------------------- /.uuv_ci_config/ros_indigo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | sh .uuv_ci_config/install_gazebo7.sh 18 | 19 | xargs -a .uuv_ci_config/ros_indigo_dependencies.txt apt -qq install --no-install-recommends --allow-unauthenticated -y 20 | 21 | source /usr/share/gazebo-7/setup.sh 22 | 23 | sh .uuv_ci_config/uuv_dependencies.sh 24 | -------------------------------------------------------------------------------- /.uuv_ci_config/ros_indigo_dependencies.txt: -------------------------------------------------------------------------------- 1 | protobuf-compiler 2 | protobuf-c-compiler 3 | ros-indigo-gazebo7-msgs 4 | ros-indigo-gazebo7-plugins 5 | ros-indigo-gazebo7-ros 6 | ros-indigo-gazebo7-ros-control 7 | ros-indigo-gazebo7-ros-pkgs 8 | ros-indigo-effort-controllers 9 | ros-indigo-image-pipeline 10 | ros-indigo-image-common 11 | ros-indigo-perception 12 | ros-indigo-perception-pcl 13 | ros-indigo-robot-state-publisher 14 | ros-indigo-ros-base 15 | ros-indigo-viz 16 | python-wstool 17 | python-catkin-tools 18 | python-catkin-lint 19 | python-tk 20 | ros-indigo-hector-localization 21 | ros-indigo-joy 22 | ros-indigo-joy-teleop 23 | libopencv-dev 24 | -------------------------------------------------------------------------------- /.uuv_ci_config/ros_kinetic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | sh .uuv_ci_config/uuv_dependencies.sh 18 | -------------------------------------------------------------------------------- /.uuv_ci_config/ros_lunar.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | # Clone repositories that do not have a ros-lunar package 18 | mkdir -p $CATKIN_WORKSPACE/src 19 | git clone https://github.com/tu-darmstadt-ros-pkg/hector_localization $CATKIN_WORKSPACE/src/hector_localization 20 | git clone https://github.com/ros-teleop/teleop_tools $CATKIN_WORKSPACE/src/teleop_tools 21 | 22 | sh .uuv_ci_config/uuv_dependencies.sh 23 | -------------------------------------------------------------------------------- /.uuv_ci_config/run_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | cd $CATKIN_WORKSPACE 18 | /bin/bash -c "source devel/setup.bash" 19 | 20 | cd $CATKIN_WORKSPACE/src/uuv_manipulators/uuv_manipulators_control 21 | catkin run_tests --no-deps --this 22 | 23 | cd $CATKIN_WORKSPACE/src/uuv_manipulators/uuv_manipulators_kinematics 24 | catkin run_tests --no-deps --this 25 | 26 | catkin_test_results 27 | -------------------------------------------------------------------------------- /.uuv_ci_config/uuv_dependencies.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | # DEP PACKAGES 18 | apt -qq install --no-install-recommends --allow-unauthenticated -y \ 19 | build-essential python-catkin-tools python-pip dvi2ps dvipng binutils \ 20 | mesa-utils module-init-tools x-window-system ros-$ROS_DISTRO-python-orocos-kdl 21 | 22 | # SETUP OTHER DEPENDENCIES FOR UUV SIMULATOR 23 | mkdir -p $CATKIN_WORKSPACE/src 24 | git clone https://github.com/uuvsimulator/uuv_simulator.git $CATKIN_WORKSPACE/src/uuv_simulator 25 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing 2 | 3 | Want to contribute? Great! You can do so through the standard GitHub pull 4 | request model. For large contributions we do encourage you to file a ticket in 5 | the GitHub issues tracking system prior to any code development to coordinate 6 | with the UUV Simulator development team early in the process. Coordinating up 7 | front helps to avoid frustration later on. 8 | 9 | Your contribution must be licensed under the Apache-2.0 license, the license 10 | used by this project. 11 | 12 | ## Add / retain copyright notices 13 | 14 | Include a copyright notice and license in each new file to be contributed, 15 | consistent with the style used by this project. If your contribution contains 16 | code under the copyright of a third party, document its origin, license, and 17 | copyright holders. 18 | 19 | ## Sign your work 20 | 21 | This project tracks patch provenance and licensing using a modified Developer 22 | Certificate of Origin (DCO; from [OSDL][DCO]) and Signed-off-by tags initially 23 | developed by the Linux kernel project. 24 | 25 | ``` 26 | UUV Simulator Developer's Certificate of Origin. Version 1.0 27 | 28 | By making a contribution to this project, I certify that: 29 | 30 | (a) The contribution was created in whole or in part by me and I 31 | have the right to submit it under the "Apache License, Version 2.0" 32 | ("Apache-2.0"); or 33 | 34 | (b) The contribution is based upon previous work that is covered by 35 | an appropriate open source license and I have the right under 36 | that license to submit that work with modifications, whether 37 | created in whole or in part by me, under the Apache-2.0 license; 38 | or 39 | 40 | (c) The contribution was provided directly to me by some other 41 | person who certified (a) or (b) and I have not modified it. 42 | 43 | (d) I understand and agree that this project and the contribution 44 | are public and that a record of the contribution (including all 45 | metadata and personal information I submit with it, including my 46 | sign-off) is maintained indefinitely and may be redistributed 47 | consistent with this project and the requirements of the Apache-2.0 48 | license or any open source license(s) involved, where they are 49 | relevant. 50 | 51 | (e) I am granting the contribution to this project under the terms of 52 | Apache-2.0. 53 | 54 | http://www.apache.org/licenses/LICENSE-2.0 55 | ``` 56 | 57 | With the sign-off in a commit message you certify that you authored the patch 58 | or otherwise have the right to submit it under an open source license. The 59 | procedure is simple: To certify above UUV Simulator Developer's Certificate of 60 | Origin 1.0 for your contribution just append a line 61 | 62 | Signed-off-by: Random J Developer 63 | 64 | to every commit message using your real name or your pseudonym and a valid 65 | email address. 66 | 67 | If you have set your `user.name` and `user.email` git configs you can 68 | automatically sign the commit by running the git-commit command with the `-s` 69 | option. There may be multiple sign-offs if more than one developer was 70 | involved in authoring the contribution. 71 | 72 | For a more detailed description of this procedure, please see 73 | [SubmittingPatches][] which was extracted from the Linux kernel project, and 74 | which is stored in an external repository. 75 | 76 | ### Individual vs. Corporate Contributors 77 | 78 | Often employers or academic institution have ownership over code that is 79 | written in certain circumstances, so please do due diligence to ensure that 80 | you have the right to submit the code. 81 | 82 | If you are a developer who is authorized to contribute to UUV Simulator on 83 | behalf of your employer, then please use your corporate email address in the 84 | Signed-off-by tag. Otherwise please use a personal email address. 85 | 86 | ## Maintain Copyright holder / Contributor list 87 | 88 | Each contributor is responsible for identifying themselves in the 89 | [NOTICE](NOTICE) file, the project's list of copyright holders and authors. 90 | Please add the respective information corresponding to the Signed-off-by tag 91 | as part of your first pull request. 92 | 93 | If you are a developer who is authorized to contribute to UUV Simulator on 94 | behalf of your employer, then add your company / organization to the list of 95 | copyright holders in the [NOTICE](NOTICE) file. As author of a corporate 96 | contribution you can also add your name and corporate email address as in the 97 | Signed-off-by tag. 98 | 99 | If your contribution is covered by this project's DCO's clause "(c) The 100 | contribution was provided directly to me by some other person who certified 101 | (a) or (b) and I have not modified it", please add the appropriate copyright 102 | holder(s) to the [NOTICE](NOTICE) file as part of your contribution. 103 | 104 | 105 | [DCO]: http://web.archive.org/web/20070306195036/http://osdlab.org/newsroom/press_releases/2004/2004_05_24_dco.html 106 | 107 | [SubmittingPatches]: https://github.com/wking/signed-off-by/blob/7d71be37194df05c349157a2161c7534feaf86a4/Documentation/SubmittingPatches 108 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright 2016 UUV Simulator Authors 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | # This is the official list of UUV Simulator copyright holders and authors. 2 | # 3 | # Often employers or academic institutions have ownership over code that is 4 | # written in certain circumstances, so please do due diligence to ensure that 5 | # you have the right to submit the code. 6 | # 7 | # When adding J Random Contributor's name to this file, either J's name on its 8 | # own or J's name associated with J's organization's name should be added, 9 | # depending on whether J's employer (or academic institution) has ownership 10 | # over code that is written for this project. 11 | # 12 | # How to add names to this file: 13 | # Individual's name . 14 | # 15 | # If Individual's organization is copyright holder of her contributions add the 16 | # organization's name, optionally also the contributor's name: 17 | # 18 | # Organization's name 19 | # Individual's name 20 | # 21 | # Please keep the list sorted. 22 | 23 | Robert Bosch GmbH 24 | Luiz Ricardo Douat 25 | Musa Morena Marcusso Manhães 26 | Sebastian Scherer 27 | Thomas Winkler 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Underwater Manipulators 2 | 3 | [![Build Status](https://travis-ci.org/uuvsimulator/uuv_manipulators.svg?branch=master)](https://travis-ci.org/uuvsimulator/uuv_manipulators) 4 | 5 | This repository contains the robot description and necessary nodes to simulate underwater 6 | manipulators integrated to underwater vehicles. This repository is complementary 7 | to the [Unmanned Underwater Vehicle Simulator (UUV Simulator)](https://github.com/uuvsimulator/uuv_simulator), 8 | an open-source project extending the simulation capabilities of the robotics 9 | simulator Gazebo to underwater vehicles and environments. For installation and 10 | usage instructions, please refer to the [documentation pages](https://uuvsimulator.github.io/). 11 | 12 | [![SWARMs Manipulation Demonstration](https://img.youtube.com/vi/vKMR8-7WRF4/0.jpg)](https://www.youtube.com/watch?v=vKMR8-7WRF4 "SWARMs - Semi-autonomous manipulation of simulated ROV") 13 | 14 | ## Purpose of the project 15 | 16 | This software is a research prototype, originally developed for the EU ECSEL 17 | Project 662107 [SWARMs](http://swarms.eu/). 18 | 19 | The software is not ready for production use. However, the license conditions of the 20 | applicable Open Source licenses allow you to adapt the software to your needs. 21 | Before using it in a safety relevant setting, make sure that the software 22 | fulfills your requirements and adjust it according to any applicable safety 23 | standards (e.g. ISO 26262). 24 | 25 | ## Requirements 26 | 27 | To simulate underwater manipulators, please refer to the [UUV Simulator](https://github.com/uuvsimulator/uuv_simulator) 28 | repository and follow the installation instructions of the package. Then you can clone 29 | this package in the `src` folder of you catkin workspace 30 | 31 | ``` 32 | cd ~/catkin_ws/src 33 | git clone https://github.com/uuvsimulator/uuv_manipulators.git 34 | ``` 35 | 36 | and then build your catkin workspace 37 | 38 | ```bash 39 | cd ~/catkin_ws 40 | catkin_make # or , if you are using catkin_tools 41 | ``` 42 | 43 | ## License 44 | 45 | The `uuv_manipulators` package is open-sourced under the Apache-2.0 license. See the 46 | [LICENSE](LICENSE) file for details. 47 | -------------------------------------------------------------------------------- /oberon4/oberon4/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package oberon4 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * Contributors: Musa Morena Marcusso Manhaes 43 | 44 | 0.5.4 (2018-07-04) 45 | ------------------ 46 | 47 | 0.5.3 (2018-07-04) 48 | ------------------ 49 | * ADD CHANGELOG files 50 | Signed-off-by: Musa Morena Marcusso Manhaes 51 | * Contributors: Musa Morena Marcusso Manhaes 52 | 53 | 0.5.1 (2018-07-03) 54 | ------------------ 55 | * CHANGE Bump version to 0.5.2 56 | Signed-off-by: Musa Morena Marcusso Manhaes 57 | * ADD execution dependencies to metapackages 58 | Signed-off-by: Gabriel Arjones 59 | * ADD format 2 to metapackage's manifests 60 | Signed-off-by: Gabriel Arjones 61 | * CHANGE Version 62 | * CHANGE Package versions 63 | Signed-off-by: Musa Morena Marcusso Manhaes 64 | * FIX Typos and package version 65 | Signed-off-by: Musa Morena Marcusso Manhaes 66 | * Adding new Oberon 4 arm (robot description and configuration parameters). 67 | Signed-off-by: Musa Morena Marcusso Manhães 68 | * Contributors: Gabriel Arjones, Musa Morena Marcusso Manhaes, Musa Morena Marcusso Manhães 69 | -------------------------------------------------------------------------------- /oberon4/oberon4/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oberon4) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /oberon4/oberon4/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oberon4 4 | 0.6.1 5 | The oberon4 package 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | oberon4_control 16 | oberon4_description 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /oberon4/oberon4_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package oberon4_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * UPDATE Catkin packages format to 2 43 | Signed-off-by: Musa Morena Marcusso Manhaes 44 | * Contributors: Musa Morena Marcusso Manhaes 45 | 46 | 0.5.4 (2018-07-04) 47 | ------------------ 48 | * UPDATE Catkin packages format to 2 49 | Signed-off-by: Musa Morena Marcusso Manhaes 50 | * Contributors: Musa Morena Marcusso Manhaes 51 | 52 | 0.5.3 (2018-07-04) 53 | ------------------ 54 | * ADD CHANGELOG files 55 | Signed-off-by: Musa Morena Marcusso Manhaes 56 | * Contributors: Musa Morena Marcusso Manhaes 57 | 58 | 0.5.1 (2018-07-03) 59 | ------------------ 60 | * CHANGE Bump version to 0.5.2 61 | Signed-off-by: Musa Morena Marcusso Manhaes 62 | * CHANGE Version 63 | * CHANGE Package versions 64 | Signed-off-by: Musa Morena Marcusso Manhaes 65 | * FIX Typos and package version 66 | Signed-off-by: Musa Morena Marcusso Manhaes 67 | * FIX Package dependencies for rosdep 68 | Signed-off-by: Musa Morena Marcusso Manhaes 69 | * ADD Kinematics service node call in the oberon4_control launch file. 70 | Signed-off-by: Musa Morena Marcusso Manhães 71 | * Modifying launch files and adding new demos with a joystick mapping for the Logitech Extreme 3D Pro. 72 | Signed-off-by: Musa Morena Marcusso Manhães 73 | * Corrections to improve stability. 74 | Signed-off-by: Musa Morena Marcusso Manhães 75 | * Adjusting gripper joint dynamics. 76 | Signed-off-by: Musa Morena Marcusso Manhães 77 | * Adapting demos for new vehicle teleop. 78 | Signed-off-by: Musa Morena Marcusso Manhães 79 | * Adding joint control launch files for the Oberon 4 arm 80 | Signed-off-by: Musa Morena Marcusso Manhães 81 | * Contributors: Musa Morena Marcusso Manhaes, Musa Morena Marcusso Manhães 82 | -------------------------------------------------------------------------------- /oberon4/oberon4_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oberon4_control) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | find_package(catkin) 7 | 8 | catkin_package() 9 | 10 | install(DIRECTORY launch 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 12 | PATTERN "*~" EXCLUDE) 13 | -------------------------------------------------------------------------------- /oberon4/oberon4_control/launch/joint_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 29 | 30 | 31 | exclusion_buttons: $(arg exclusion_button) 32 | deadman_button: $(arg deadman_button) 33 | home_button: $(arg home_button) 34 | controller_config: 35 | azimuth: 36 | joint_input_axis: $(arg axis_azimuth) 37 | axis_gain: 0.008 38 | topic: /$(arg uuv_name)/$(arg arm_name)/azimuth/controller/command 39 | controller: 40 | p: 800 41 | i: 20 42 | d: 50 43 | shoulder: 44 | joint_input_axis: $(arg axis_shoulder) 45 | axis_gain: 0.008 46 | topic: /$(arg uuv_name)/$(arg arm_name)/shoulder/controller/command 47 | controller: 48 | p: 800 49 | i: 20 50 | d: 50 51 | wrist_joint: 52 | joint_input_axis: $(arg axis_wrist) 53 | axis_gain: 0.05 54 | topic: /$(arg uuv_name)/$(arg arm_name)/wrist_joint/controller/command 55 | controller: 56 | p: 400 57 | i: 2 58 | d: 5 59 | 60 | 61 | 62 | 66 | 67 | 68 | 69 | exclusion_buttons: $(arg exclusion_button) 70 | deadman_button: $(arg deadman_button) 71 | open_button: $(arg gripper_open_button) 72 | close_button: $(arg gripper_close_button) 73 | kp: 40 74 | ki: 5 75 | kd: 1 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /oberon4/oberon4_control/launch/joint_effort_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | $(arg uuv_name): 7 | $(arg arm_name): 8 | azimuth: 9 | controller: 10 | type: effort_controllers/JointEffortController 11 | joint: $(arg arm_name)/azimuth 12 | pid: {p: 500.0, i: 0.0, d: 20.0} 13 | shoulder: 14 | controller: 15 | type: effort_controllers/JointEffortController 16 | joint: $(arg arm_name)/shoulder 17 | pid: {p: 500.0, i: 7.0, d: 20.0} 18 | wrist_joint: 19 | controller: 20 | type: effort_controllers/JointEffortController 21 | joint: $(arg arm_name)/wrist_joint 22 | pid: {p: 500.0, i: 1.0, d: 10.0} 23 | jaw_1_joint: 24 | controller: 25 | type: effort_controllers/JointEffortController 26 | joint: $(arg arm_name)/jaw_1_joint 27 | pid: {p: 100.0, i: 0, d: 1.0} 28 | jaw_2_joint: 29 | controller: 30 | type: effort_controllers/JointEffortController 31 | joint: $(arg arm_name)/jaw_2_joint 32 | pid: {p: 100.0, i: 0, d: 1.0} 33 | 34 | 35 | 36 | 37 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /oberon4/oberon4_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oberon4_control 4 | 0.6.1 5 | The oberon4_control package 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | uuv_manipulators_kinematics 16 | uuv_manipulators_control 17 | controller_manager 18 | ros_controllers 19 | ros_control 20 | gazebo_ros_control 21 | effort_controllers 22 | 23 | 24 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package oberon4_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * UPDATE Catkin packages format to 2 43 | Signed-off-by: Musa Morena Marcusso Manhaes 44 | * Contributors: Musa Morena Marcusso Manhaes 45 | 46 | 0.5.4 (2018-07-04) 47 | ------------------ 48 | * UPDATE Catkin packages format to 2 49 | Signed-off-by: Musa Morena Marcusso Manhaes 50 | * Contributors: Musa Morena Marcusso Manhaes 51 | 52 | 0.5.3 (2018-07-04) 53 | ------------------ 54 | * ADD CHANGELOG files 55 | Signed-off-by: Musa Morena Marcusso Manhaes 56 | * Contributors: Musa Morena Marcusso Manhaes 57 | 58 | 0.5.1 (2018-07-03) 59 | ------------------ 60 | * CHANGE Bump version to 0.5.2 61 | Signed-off-by: Musa Morena Marcusso Manhaes 62 | * CHANGE Version 63 | * CHANGE Package versions 64 | Signed-off-by: Musa Morena Marcusso Manhaes 65 | * FIX Typos and package version 66 | Signed-off-by: Musa Morena Marcusso Manhaes 67 | * ADD Macro for the description of the hydrodynamic models for the manipulator models. 68 | Signed-off-by: Musa Morena Marcusso Manhães 69 | * replace collision meshes with primitives 70 | modify joint parameters as suggested here: 71 | http://answers.gazebosim.org/question/13700/object-slips-out-of-the-gripper-after-jitters/ 72 | Signed-off-by: Sebastian Scherer 73 | * Corrections to improve stability. 74 | Signed-off-by: Musa Morena Marcusso Manhães 75 | * Adjusting gripper joint dynamics. 76 | Signed-off-by: Musa Morena Marcusso Manhães 77 | * Adapting demos for new vehicle teleop. 78 | Signed-off-by: Musa Morena Marcusso Manhães 79 | * Adding new Oberon 4 arm (robot description and configuration parameters). 80 | Signed-off-by: Musa Morena Marcusso Manhães 81 | * Contributors: Musa Morena Marcusso Manhaes, Musa Morena Marcusso Manhães, Sebastian Scherer 82 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oberon4_description) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | find_package(catkin) 7 | 8 | catkin_package() 9 | 10 | install(DIRECTORY meshes params robots urdf 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 12 | PATTERN "*~" EXCLUDE) 13 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/meshes/Jaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uuvsimulator/uuv_manipulators/1c73752f80ee0c4517bff8cb31316c12692775a0/oberon4/oberon4_description/meshes/Jaw.stl -------------------------------------------------------------------------------- /oberon4/oberon4_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oberon4_description 4 | 0.6.1 5 | The oberon4_description package 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | 16 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/params/gripper_config.yaml: -------------------------------------------------------------------------------- 1 | gripper: 2 | type: jaw 3 | base: gripper_base 4 | links: [jaw_1, jaw_2] 5 | control_joint: jaw_1_joint 6 | mimic_joint: jaw_2_joint 7 | mimic_joint_gain: 1.0 8 | closed_limit: upper 9 | full_open_limit: lower 10 | groups: 11 | left_finger: 12 | links: [jaw_1] 13 | ee: jaw_1 14 | right_finger: 15 | links: [jaw_2] 16 | ee: jaw_2 17 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/params/robot_config.yaml: -------------------------------------------------------------------------------- 1 | # Main parameters for all Oberon 4P manipulator instances 2 | # To emulate the conditions for a real controller, the link parameters are 3 | # loaded in this parameter file instead of being read from Gazebo topics. 4 | joint_names: [azimuth, shoulder, wrist_joint, jaw_1_joint, jaw_2_joint] 5 | base_link: base 6 | tip_link: wrist 7 | default_configs: 8 | home: 9 | azimuth: 0 10 | shoulder: 1.5707963 11 | wrist_joint: 0 12 | jaw_1_joint: 0 13 | jaw_2_joint: 0 14 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/robots/oberon4_default.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /oberon4/oberon4_description/urdf/serial_arm.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | false 23 | 24 | 25 | 26 | false 27 | 28 | 29 | 30 | false 31 | 32 | 33 | 34 | false 35 | 36 | 37 | 38 | false 39 | 40 | 41 | 42 | false 43 | 44 | 45 | 46 | false 47 | 48 | 49 | 50 | 1 51 | 52 | 53 | 54 | 1 55 | 56 | 57 | 58 | 1 59 | 60 | 61 | 62 | 1 63 | 64 | 65 | 66 | 1 67 | 68 | 69 | 70 | 1 71 | 72 | 73 | 74 | 1 75 | 76 | 77 | 78 | 83 | 84 | 85 | ${base_volume} 86 | 87 | ${base_y} 88 | ${base_x} 89 | ${base_z} 90 | 91 | 92 | fossen 93 | 94 | 95 | 96 | ${azimuth_volume} 97 | 98 | ${azimuth_y} 99 | ${azimuth_x} 100 | ${azimuth_z} 101 | 102 | 103 | fossen 104 | 105 | 106 | 107 | ${arm_volume} 108 | 109 | ${arm_y} 110 | ${arm_x} 111 | ${arm_z} 112 | 113 | 114 | fossen 115 | 116 | 117 | 118 | ${wrist_volume} 119 | 120 | ${wrist_y} 121 | ${wrist_x} 122 | ${wrist_z} 123 | 124 | 125 | fossen 126 | 127 | 128 | 129 | ${gripper_base_volume} 130 | 131 | ${gripper_base_y} 132 | ${gripper_base_x} 133 | ${gripper_base_z} 134 | 135 | 136 | fossen 137 | 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /oberon7/oberon7/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package oberon7 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * Contributors: Musa Morena Marcusso Manhaes 43 | 44 | 0.5.4 (2018-07-04) 45 | ------------------ 46 | 47 | 0.5.3 (2018-07-04) 48 | ------------------ 49 | * ADD CHANGELOG files 50 | Signed-off-by: Musa Morena Marcusso Manhaes 51 | * Contributors: Musa Morena Marcusso Manhaes 52 | 53 | 0.5.1 (2018-07-03) 54 | ------------------ 55 | * CHANGE Bump version to 0.5.2 56 | Signed-off-by: Musa Morena Marcusso Manhaes 57 | * ADD execution dependencies to metapackages 58 | Signed-off-by: Gabriel Arjones 59 | * ADD format 2 to metapackage's manifests 60 | Signed-off-by: Gabriel Arjones 61 | * ADD metapackage to package's manifest 62 | Signed-off-by: Gabriel Arjones 63 | * CHANGE Version 64 | * CHANGE Package versions 65 | Signed-off-by: Musa Morena Marcusso Manhaes 66 | * FIX Typos and package version 67 | Signed-off-by: Musa Morena Marcusso Manhaes 68 | * CHANGE Update packages information. 69 | Signed-off-by: Musa Morena Marcusso Manhaes 70 | * ADD New Oberon 7 metapackage 71 | Signed-off-by: Musa Morena Marcusso Manhaes 72 | * Contributors: Gabriel Arjones, Musa Morena Marcusso Manhaes 73 | -------------------------------------------------------------------------------- /oberon7/oberon7/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oberon7) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /oberon7/oberon7/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oberon7 4 | 0.6.1 5 | Meta-package for the Oberon 7, a mock-up robotic manipulator inspired on the Schilling Robotics Orion 7P 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | oberon7_control 16 | oberon7_description 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package oberon7_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | * FIX Order of CHANGELOG file 35 | Signed-off-by: Musa Morena Marcusso Manhaes 36 | * Contributors: Musa Morena Marcusso Manhaes 37 | 38 | 0.5.6 (2018-07-06) 39 | ------------------ 40 | 41 | 0.5.5 (2018-07-05) 42 | ------------------ 43 | * RM Merge messages from the change log 44 | Signed-off-by: Musa Morena Marcusso Manhaes 45 | * UPDATE Catkin packages format to 2 46 | Signed-off-by: Musa Morena Marcusso Manhaes 47 | * Contributors: Musa Morena Marcusso Manhaes 48 | 49 | 0.5.4 (2018-07-04) 50 | ------------------ 51 | * UPDATE Catkin packages format to 2 52 | Signed-off-by: Musa Morena Marcusso Manhaes 53 | * Contributors: Musa Morena Marcusso Manhaes 54 | 55 | 0.5.3 (2018-07-04) 56 | ------------------ 57 | * ADD CHANGELOG files 58 | Signed-off-by: Musa Morena Marcusso Manhaes 59 | * Contributors: Musa Morena Marcusso Manhaes 60 | 61 | 0.5.1 (2018-07-03) 62 | ------------------ 63 | * CHANGE Bump version to 0.5.2 64 | Signed-off-by: Musa Morena Marcusso Manhaes 65 | * CHANGE Version 66 | * CHANGE Package versions 67 | Signed-off-by: Musa Morena Marcusso Manhaes 68 | * FIX Typos and package version 69 | Signed-off-by: Musa Morena Marcusso Manhaes 70 | * FIX Joint dynamics parameters for Oberon7 71 | Signed-off-by: Musa Morena Marcusso Manhaes 72 | * FIX Package dependencies for rosdep 73 | Signed-off-by: Musa Morena Marcusso Manhaes 74 | * FIX Joystick input gains for the input axis 75 | Signed-off-by: Musa Morena Marcusso Manhaes 76 | * CHANGE Update packages information. 77 | Signed-off-by: Musa Morena Marcusso Manhaes 78 | * CHANGE JT controller gains 79 | Signed-off-by: Musa Morena Marcusso Manhaes 80 | * ADD Oberon 7 control package 81 | Signed-off-by: Musa Morena Marcusso Manhaes 82 | * ADD Launch files for joint and Cartesian controllers. 83 | Signed-off-by: Musa Morena Marcusso Manhaes 84 | * Contributors: Musa Morena Marcusso Manhaes 85 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oberon7_control) 3 | 4 | find_package(catkin) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | PATTERN "*~" EXCLUDE) 11 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/launch/gripper_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | $(arg uuv_name): 12 | $(arg arm_name): 13 | finger_left_joint: 14 | controller: 15 | type: effort_controllers/JointEffortController 16 | joint: $(arg arm_name)/finger_left_joint 17 | pid: {p: 50.0, i: 0, d: 0} 18 | finger_right_joint: 19 | controller: 20 | type: effort_controllers/JointEffortController 21 | joint: $(arg arm_name)/finger_right_joint 22 | pid: {p: 50.0, i: 0, d: 0} 23 | 24 | 25 | 26 | 27 | 35 | 36 | 37 | 38 | 42 | 43 | 44 | 45 | exclusion_buttons: $(arg exclusion_button) 46 | deadman_button: $(arg deadman_button) 47 | open_button: $(arg open_button) 48 | close_button: $(arg close_button) 49 | kp: 100 50 | ki: 4 51 | kd: 1 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/launch/joint_control.launch: -------------------------------------------------------------------------------- 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 | 40 | 41 | 42 | 44 | 45 | 46 | exclusion_buttons: $(arg exclusion_button) 47 | deadman_button: $(arg deadman_button) 48 | home_button: $(arg home_button) 49 | controller_config: 50 | azimuth: 51 | joint_input_axis: $(arg axis_azimuth) 52 | axis_gain: 0.01 53 | topic: /$(arg uuv_name)/$(arg arm_name)/azimuth/controller/command 54 | controller: 55 | p: 1000 56 | i: 20 57 | d: 50 58 | shoulder: 59 | joint_input_axis: $(arg axis_shoulder) 60 | axis_gain: 0.01 61 | topic: /$(arg uuv_name)/$(arg arm_name)/shoulder/controller/command 62 | controller: 63 | p: 1000 64 | i: 20 65 | d: 50 66 | elbow: 67 | joint_input_axis: $(arg axis_elbow) 68 | axis_gain: 0.01 69 | topic: /$(arg uuv_name)/$(arg arm_name)/elbow/controller/command 70 | controller: 71 | p: 1000 72 | i: 20 73 | d: 50 74 | roll: 75 | joint_input_axis: $(arg axis_roll) 76 | axis_gain: 0.01 77 | topic: /$(arg uuv_name)/$(arg arm_name)/roll/controller/command 78 | controller: 79 | p: 300 80 | i: 10 81 | d: 20 82 | pitch: 83 | joint_input_axis: $(arg axis_pitch) 84 | axis_gain: 0.1 85 | topic: /$(arg uuv_name)/$(arg arm_name)/pitch/controller/command 86 | controller: 87 | p: 300 88 | i: 10 89 | d: 0 90 | wrist: 91 | joint_input_axis: $(arg axis_yaw) 92 | axis_gain: 0.01 93 | topic: /$(arg uuv_name)/$(arg arm_name)/wrist/controller/command 94 | controller: 95 | p: 200 96 | i: 10 97 | d: 0 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/launch/joint_effort_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | $(arg uuv_name): 7 | $(arg arm_name): 8 | azimuth: 9 | controller: 10 | type: effort_controllers/JointEffortController 11 | joint: $(arg arm_name)/azimuth 12 | pid: {p: 2000, i: 100, d: 50} 13 | shoulder: 14 | controller: 15 | type: effort_controllers/JointEffortController 16 | joint: $(arg arm_name)/shoulder 17 | pid: {p: 2000, i: 100, d: 50} 18 | elbow: 19 | controller: 20 | type: effort_controllers/JointEffortController 21 | joint: $(arg arm_name)/elbow 22 | pid: {p: 2000, i: 100, d: 50} 23 | roll: 24 | controller: 25 | type: effort_controllers/JointEffortController 26 | joint: $(arg arm_name)/roll 27 | pid: {p: 1000, i: 50, d: 10} 28 | pitch: 29 | controller: 30 | type: effort_controllers/JointEffortController 31 | joint: $(arg arm_name)/pitch 32 | pid: {p: 1000, i: 50, d: 10} 33 | wrist: 34 | controller: 35 | type: effort_controllers/JointEffortController 36 | joint: $(arg arm_name)/wrist 37 | pid: {p: 500, i: 30, d: 10} 38 | 39 | 40 | 41 | 42 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/launch/jt_cartesian_controller.launch: -------------------------------------------------------------------------------- 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 | type: twist 47 | deadman_button: $(arg deadman_button) 48 | exclusion_buttons: [$(arg exclusion_button)] 49 | mapping: 50 | x: 51 | axis: $(arg axis_x) 52 | gain: $(arg gain_x) 53 | y: 54 | axis: $(arg axis_y) 55 | gain: $(arg gain_y) 56 | z: 57 | axis: $(arg axis_z) 58 | gain: $(arg gain_z) 59 | roll: 60 | axis: $(arg axis_roll) 61 | gain: $(arg gain_roll) 62 | pitch: 63 | axis: $(arg axis_pitch) 64 | gain: $(arg gain_pitch) 65 | yaw: 66 | axis: $(arg axis_yaw) 67 | gain: $(arg gain_yaw) 68 | 69 | 70 | 71 | 72 | 74 | 75 | 76 | 78 | 79 | 80 | publish_rate: 50 81 | Kp: 82 | - 8000.0 83 | - 8000.0 84 | - 8000.0 85 | - 300.0 86 | - 300.0 87 | - 300.0 88 | Kd: 89 | - 100.0 90 | - 100.0 91 | - 100.0 92 | - 2.0 93 | - 2.0 94 | - 2.0 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | -------------------------------------------------------------------------------- /oberon7/oberon7_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oberon7_control 4 | 0.6.1 5 | Package with configuration and launch files to control the Oberon 7 manipulator. 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | uuv_manipulators_kinematics 16 | uuv_manipulators_control 17 | controller_manager 18 | uuv_teleop 19 | ros_controllers 20 | ros_control 21 | gazebo_ros_control 22 | effort_controllers 23 | 24 | 25 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package oberon7_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.6 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.5 (2018-07-05) 36 | ------------------ 37 | * RM Merge messages from the change log 38 | Signed-off-by: Musa Morena Marcusso Manhaes 39 | * UPDATE Catkin packages format to 2 40 | Signed-off-by: Musa Morena Marcusso Manhaes 41 | * Contributors: Musa Morena Marcusso Manhaes 42 | 43 | 0.5.4 (2018-07-04) 44 | ------------------ 45 | * UPDATE Catkin packages format to 2 46 | Signed-off-by: Musa Morena Marcusso Manhaes 47 | * Contributors: Musa Morena Marcusso Manhaes 48 | 49 | 0.5.7 (2018-07-06) 50 | ------------------ 51 | * FIX Order of CHANGELOG file 52 | Signed-off-by: Musa Morena Marcusso Manhaes 53 | * Contributors: Musa Morena Marcusso Manhaes 54 | 55 | 0.5.3 (2018-07-04) 56 | ------------------ 57 | * ADD CHANGELOG files 58 | Signed-off-by: Musa Morena Marcusso Manhaes 59 | * Contributors: Musa Morena Marcusso Manhaes 60 | 61 | 0.5.1 (2018-07-03) 62 | ------------------ 63 | * CHANGE Bump version to 0.5.2 64 | Signed-off-by: Musa Morena Marcusso Manhaes 65 | * CHANGE Version 66 | * ADD New marker tags for BOP Panel 67 | FIX Oberon7 serial_arm parameters 68 | Signed-off-by: Luiz Ricardo Douat 69 | * CHANGE Package versions 70 | Signed-off-by: Musa Morena Marcusso Manhaes 71 | * FIX Typos and package version 72 | Signed-off-by: Musa Morena Marcusso Manhaes 73 | * FIX Joint dynamics parameters for Oberon7 74 | Signed-off-by: Musa Morena Marcusso Manhaes 75 | * CHANGE Oberon7 joint effort limits 76 | Signed-off-by: Musa Morena Marcusso Manhaes 77 | * ADD Cylinder hyd. parameters to Oberon7 links 78 | Signed-off-by: Musa Morena Marcusso Manhaes 79 | * Removed 'launch' from CMakeList 80 | Signed-off-by: Luiz Ricardo Douat 81 | * CHANGE Update packages information. 82 | Signed-off-by: Musa Morena Marcusso Manhaes 83 | * CHANGE Simplification of the collision geometries. 84 | Signed-off-by: Musa Morena Marcusso Manhaes 85 | * FIX Orientation of the end-effector frame. 86 | Signed-off-by: Musa Morena Marcusso Manhaes 87 | * CHANGE Gazebo SDF description 88 | Signed-off-by: Musa Morena Marcusso Manhaes 89 | * CHANGE Base gripper visual geometry. 90 | Signed-off-by: Musa Morena Marcusso Manhaes 91 | * ADD Simplified collision geometry for the base 92 | Signed-off-by: Musa Morena Marcusso Manhaes 93 | * ADD Robot parameters and Gazebo SDF model converter 94 | Signed-off-by: Musa Morena Marcusso Manhaes 95 | * ADD URDF files 96 | Signed-off-by: Musa Morena Marcusso Manhaes 97 | * ADD Meshes for the Oberon 7 arm and gripper 98 | Signed-off-by: Musa Morena Marcusso Manhaes 99 | * ADD New description for the Oberon 7 arm 100 | Signed-off-by: Musa Morena Marcusso Manhaes 101 | * Contributors: Luiz Ricardo Douat, Musa Morena Marcusso Manhaes 102 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(oberon7_description) 3 | 4 | find_package(catkin REQUIRED) 5 | find_package(gazebo REQUIRED) # this is only required to get gazebo's directories 6 | 7 | catkin_package( 8 | # INCLUDE_DIRS include 9 | # LIBRARIES uuv_descriptions 10 | # CATKIN_DEPENDS other_catkin_pkg 11 | # DEPENDS system_lib 12 | ) 13 | 14 | install(DIRECTORY meshes params robots urdf 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 16 | PATTERN "*~" EXCLUDE) 17 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/meshes/serial_arm/collision/Base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uuvsimulator/uuv_manipulators/1c73752f80ee0c4517bff8cb31316c12692775a0/oberon7/oberon7_description/meshes/serial_arm/collision/Base.stl -------------------------------------------------------------------------------- /oberon7/oberon7_description/models/oberon7/gen_sdf.sh: -------------------------------------------------------------------------------- 1 | xacro --inorder $(rospack find oberon7_description)/robots/oberon7_standalone.xacro > $(rospack find oberon7_description)/models/oberon7/model.urdf 2 | gz sdf -p $(rospack find oberon7_description)/models/oberon7/model.urdf > $(rospack find oberon7_description)/models/oberon7/model.sdf 3 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/models/oberon7/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Oberon 7 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Musa Morena Marcusso Manhaes 9 | Musa.Marcusso@de.bosch.com 10 | 11 | 12 | 13 | Oberon 7 robotic manipulator 14 | 15 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | oberon7_description 4 | 0.6.1 5 | Robot and parameter description for the mock-up model of the Schilling Robotics Orion 7P 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | gazebo_ros 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/params/parallel_gripper_config.yaml: -------------------------------------------------------------------------------- 1 | gripper: 2 | type: parallel 3 | base: end_effector 4 | links: [finger_left, finger_tip_left, finger_right, finger_tip_right] 5 | control_joint: finger_left_joint 6 | mimic_joint: finger_right_joint 7 | mimic_joint_gain: -1 8 | closed_limit: lower 9 | full_open_limit: upper 10 | groups: 11 | left_finger: 12 | links: [finger_left, finger_tip_left] 13 | ee: finger_tip_left 14 | right_finger: 15 | links: [finger_right, finger_tip_right] 16 | ee: finger_tip_right 17 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/params/robot_config.yaml: -------------------------------------------------------------------------------- 1 | # Main parameters for all Oberon 7P manipulator instances 2 | # To emulate the conditions for a real controller, the link parameters are 3 | # loaded in this parameter file instead of being read from Gazebo topics. 4 | joint_names: [azimuth, shoulder, elbow, roll, pitch, wrist] 5 | base_link: base 6 | tip_link: end_effector 7 | default_configs: 8 | home: 9 | azimuth: 0 10 | shoulder: 1.57 11 | elbow: -1.57 12 | pitch: 1.57 13 | roll: 0 14 | wrist: 0 15 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/robots/oberon7_default.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/robots/oberon7_standalone.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/urdf/parallel_gripper.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 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 | 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 | 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 | 128 | 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 | 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 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/urdf/parameters/parallel_gripper.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/urdf/parameters/serial_arm.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 198 | 199 | 200 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/urdf/serial_arm.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | false 22 | 1 23 | 24 | 25 | 26 | false 27 | 1 28 | 29 | 30 | 31 | false 32 | 1 33 | 34 | 35 | 36 | false 37 | 1 38 | 39 | 40 | 41 | false 42 | 1 43 | 44 | 45 | 46 | false 47 | 1 48 | 49 | 50 | 51 | false 52 | 1000000.0 53 | 1.0 54 | 1.0 55 | 1.0 56 | 0.001 57 | 1 58 | 1 59 | 60 | 61 | 62 | false 63 | 1000000.0 64 | 1.0 65 | 1.0 66 | 1.0 67 | 0.001 68 | 1 69 | 1 70 | 71 | 72 | 73 | true 74 | 1000000.0 75 | 1.0 76 | 1.0 77 | 1.0 78 | 0.001 79 | 3 80 | 1 81 | 82 | 83 | 84 | true 85 | 1000000.0 86 | 1.0 87 | 1.0 88 | 1.0 89 | 0.001 90 | 3 91 | 1 92 | 93 | 94 | 95 | 100 | 101 | 102 | ${total_volume * base_volume / total_volume_bbox} 103 | 104 | ${base_width} 105 | ${base_length} 106 | ${base_height} 107 | 108 | 109 | cylinder 110 | i 111 | 112 | 113 | 114 | 115 | ${total_volume * shoulder_link_volume / total_volume_bbox} 116 | 117 | ${shoulder_link_width} 118 | ${shoulder_link_length} 119 | ${shoulder_link_height} 120 | 121 | 122 | cylinder 123 | i 124 | 125 | 126 | 127 | 128 | ${total_volume * upper_arm_volume / total_volume_bbox} 129 | 130 | ${upper_arm_width} 131 | ${upper_arm_length} 132 | ${upper_arm_height} 133 | 134 | 135 | cylinder 136 | i 137 | 138 | 139 | 140 | 141 | ${total_volume * elbow_link_volume / total_volume_bbox} 142 | 143 | ${elbow_link_width} 144 | ${elbow_link_length} 145 | ${elbow_link_height} 146 | 147 | 148 | cylinder 149 | i 150 | 151 | 152 | 153 | 154 | ${total_volume * forearm_volume / total_volume_bbox} 155 | 156 | ${forearm_width} 157 | ${forearm_length} 158 | ${forearm_height} 159 | 160 | 161 | cylinder 162 | i 163 | 164 | 165 | 166 | 167 | ${total_volume * wrist_volume / total_volume_bbox} 168 | 169 | ${wrist_width} 170 | ${wrist_length} 171 | ${wrist_height} 172 | 173 | 174 | cylinder 175 | i 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /oberon7/oberon7_description/urdf/serial_arm.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 34 | 35 | 36 | 37 | 38 | 39 | transmission_interface/SimpleTransmission 40 | 41 | EffortJointInterface 42 | 43 | 44 | EffortJointInterface 45 | 1 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 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 | 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 | 159 | 160 | 161 | 162 | 163 | 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 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | -------------------------------------------------------------------------------- /uuv_manipulators_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package uuv_manipulators_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * UPDATE Catkin packages format to 2 43 | Signed-off-by: Musa Morena Marcusso Manhaes 44 | * Contributors: Musa Morena Marcusso Manhaes 45 | 46 | 0.5.4 (2018-07-04) 47 | ------------------ 48 | * UPDATE Catkin packages format to 2 49 | Signed-off-by: Musa Morena Marcusso Manhaes 50 | * Contributors: Musa Morena Marcusso Manhaes 51 | 52 | 0.5.3 (2018-07-04) 53 | ------------------ 54 | * ADD CHANGELOG files 55 | Signed-off-by: Musa Morena Marcusso Manhaes 56 | * Contributors: Musa Morena Marcusso Manhaes 57 | 58 | 0.5.1 (2018-07-03) 59 | ------------------ 60 | * CHANGE Bump version to 0.5.2 61 | Signed-off-by: Musa Morena Marcusso Manhaes 62 | * CHANGE Version 63 | * FIX Use FIDUALS-ARUCO instead of RCARS-APRILTAGS 64 | Signed-off-by: Luiz Ricardo Douat 65 | * CHANGE Package versions 66 | Signed-off-by: Musa Morena Marcusso Manhaes 67 | * FIX Typos and package version 68 | Signed-off-by: Musa Morena Marcusso Manhaes 69 | * FIX Package dependencies for rosdep 70 | Signed-off-by: Musa Morena Marcusso Manhaes 71 | * FIX Test for ROS parameter of publish rate. 72 | Signed-off-by: Musa Morena Marcusso Manhaes 73 | * FIX Computation of next pose goal in Cartesian controller. 74 | Signed-off-by: Musa Morena Marcusso Manhaes 75 | * FIX Index of the input command vector. 76 | Signed-off-by: Musa Morena Marcusso Manhães 77 | * ADD Exception handler for the case the joystick has been initialized with the wrong mapping. 78 | Signed-off-by: Musa Morena Marcusso Manhães 79 | * ADD Publish the current pose reference. 80 | Signed-off-by: Musa Morena Marcusso Manhães 81 | * RM Joystick subscriber 82 | ADD Velocity reference topic as controller input 83 | ADD Generation of reference topic and visual markers 84 | Signed-off-by: Musa Morena Marcusso Manhães 85 | * FIX CMakeLists files 86 | Signed-off-by: Musa Morena Marcusso Manhães 87 | * CHANGE Exception handler in joystick parsing 88 | callback function 89 | Signed-off-by: Musa Morena Marcusso Manhães 90 | * Modifying launch files and adding new demos with a joystick mapping for the Logitech Extreme 3D Pro. 91 | Signed-off-by: Musa Morena Marcusso Manhães 92 | * Corrections to improve stability. 93 | Signed-off-by: Musa Morena Marcusso Manhães 94 | * Correcting the exception handler. 95 | Signed-off-by: Musa Morena Marcusso Manhães 96 | * Adding new joint control node. 97 | Signed-off-by: Musa Morena Marcusso Manhães 98 | * Modifying the controller nodes to parse the joystick input. Adding exclusion buttons to allow controlling a single arm at time. 99 | Signed-off-by: Musa Morena Marcusso Manhães 100 | * initial commit 101 | Signed-off-by: Sebastian Scherer (CR/AEI) 102 | * Contributors: Luiz Ricardo Douat, Musa Morena Marcusso Manhaes, Musa Morena Marcusso Manhães, Sebastian Scherer (CR/AEI) 103 | -------------------------------------------------------------------------------- /uuv_manipulators_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uuv_manipulators_control) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | uuv_manipulators_kinematics 11 | uuv_manipulators_msgs 12 | uuv_control_cascaded_pid 13 | rospy 14 | geometry_msgs 15 | std_msgs 16 | visualization_msgs 17 | sensor_msgs 18 | tf 19 | ) 20 | 21 | find_package(orocos_kdl REQUIRED) 22 | 23 | catkin_python_setup() 24 | 25 | catkin_package( 26 | CATKIN_DEPENDS 27 | uuv_manipulators_kinematics 28 | ) 29 | 30 | catkin_install_python(PROGRAMS 31 | scripts/set_joint_config.py 32 | scripts/jt_cartesian_controller.py 33 | scripts/gripper_controller.py 34 | scripts/joint_position_controller.py 35 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 36 | -------------------------------------------------------------------------------- /uuv_manipulators_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uuv_manipulators_control 4 | 0.6.1 5 | Cartesian and gripper controllers. 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | uuv_manipulators_kinematics 16 | uuv_manipulators_msgs 17 | uuv_control_cascaded_pid 18 | rospy 19 | geometry_msgs 20 | std_msgs 21 | visualization_msgs 22 | sensor_msgs 23 | python-numpy 24 | tf 25 | orocos_kdl 26 | 27 | 28 | -------------------------------------------------------------------------------- /uuv_manipulators_control/scripts/gripper_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | import os 17 | import rospy 18 | from uuv_manipulator_interfaces import GripperInterface 19 | from uuv_manipulators_msgs.msg import EndeffectorCommand 20 | from sensor_msgs.msg import Joy 21 | from PID import PIDRegulator 22 | 23 | 24 | class GripperController: 25 | 26 | def __init__(self): 27 | # Timeout for input inactivity 28 | self._timeout = 0.01 29 | 30 | # Initializing the gripper interface for the current namespace 31 | self._gripper = GripperInterface() 32 | 33 | # Initial values for the gripper joystick buttons 34 | # Default - B button from XBox 360 controller 35 | self._open_button = 1 36 | if rospy.has_param('~open_button'): 37 | self._open_button = rospy.get_param('~open_button') 38 | 39 | # Default - X button from XBox 360 controller 40 | self._close_button = 2 41 | if rospy.has_param('~close_button'): 42 | self._close_button = rospy.get_param('~close_button') 43 | 44 | # Default - RB button from XBox 360 controller 45 | self._deadman_button = 5 46 | if rospy.has_param('~deadman_button'): 47 | self._deadman_button = rospy.get_param('~deadman_button') 48 | 49 | if rospy.has_param('~exclusion_buttons'): 50 | self._exclusion_buttons = rospy.get_param('~exclusion_buttons') 51 | if type(self._exclusion_buttons) in [float, int]: 52 | self._exclusion_buttons = [int(self._exclusion_buttons)] 53 | elif type(self._exclusion_buttons) == list: 54 | for n in self._exclusion_buttons: 55 | if type(n) != float: 56 | raise rospy.ROSException('Exclusion buttons must be an' 57 | ' integer index to the joystick button') 58 | else: 59 | self._exclusion_buttons = list() 60 | 61 | self._joy_gain = 0.1 62 | if rospy.has_param('~joy_gain'): 63 | self._joy_gain = rospy.get_param('~joy_gain') 64 | 65 | # Retrieve the publish rate 66 | self._publish_rate = 100 67 | if rospy.has_param('~publish_rate'): 68 | self._publish_rate = rospy.get_param('~publish_rate') 69 | 70 | if self._publish_rate <= 0: 71 | raise rospy.ROSException('Invalid negative publish rate') 72 | 73 | self._pos_goal = self._gripper.closed_pos 74 | 75 | self._ratio_goal = 0.0 76 | 77 | if rospy.has_param('~kp'): 78 | self._kp = rospy.get_param('~kp') 79 | else: 80 | self._kp = 100.0 81 | 82 | if rospy.has_param('~ki'): 83 | self._ki = rospy.get_param('~ki') 84 | else: 85 | self._ki = 0.0 86 | 87 | if rospy.has_param('~kd'): 88 | self._kd = rospy.get_param('~kd') 89 | else: 90 | self._kd = 0.0 91 | 92 | self._controllers = dict() 93 | self._controllers[self._gripper.control_joint] = PIDRegulator(self._kp, self._ki, self._kd, 100) 94 | self._controllers[self._gripper.mimic_joint] = PIDRegulator(self._kp, self._ki, self._kd, 100) 95 | 96 | if self._gripper.control_joint is None: 97 | raise rospy.ROSException('Gripper cannot be controlled') 98 | 99 | self._limits = self._gripper.control_joint_limits 100 | 101 | self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) 102 | 103 | rate = rospy.Rate(self._publish_rate) 104 | 105 | while not rospy.is_shutdown(): 106 | self._update() 107 | rate.sleep() 108 | 109 | def _update(self): 110 | # Compute the necessary effort for the requested joint position 111 | self._pos_goal = self._ratio_goal * (self._gripper.fully_open_pos - self._gripper.closed_pos) + \ 112 | self._gripper.closed_pos 113 | 114 | # Set the torque for the control joint 115 | error = self._pos_goal - self._gripper.control_joint_position 116 | torque = self._controllers[self._gripper.control_joint].regulate(error, rospy.get_time()) 117 | self._gripper.set_controller_command(self._gripper.control_joint, torque) 118 | 119 | # Set the torque for the mimic joint 120 | error = self._pos_goal - self._gripper.mimic_joint_position 121 | torque = self._controllers[self._gripper.mimic_joint].regulate(error, rospy.get_time()) 122 | self._gripper.set_controller_command(self._gripper.mimic_joint, torque) 123 | 124 | if self._gripper.is_parallel: 125 | pass 126 | 127 | def _joy_callback(self, joy): 128 | try: 129 | if self._deadman_button > -1: 130 | if not joy.buttons[self._deadman_button]: 131 | return 132 | for n in self._exclusion_buttons: 133 | if joy.buttons[n] == 1: 134 | return 135 | 136 | self._ratio_goal += self._joy_gain * (joy.buttons[self._open_button] - joy.buttons[self._close_button]) 137 | if self._ratio_goal < 0: 138 | self._ratio_goal = 0.0 139 | if self._ratio_goal > 1: 140 | self._ratio_goal = 1.0 141 | except Exception, e: 142 | print 'Error occurred while parsing joystick input, check if the joy_id corresponds to the joystick ' \ 143 | 'being used. message=%s' % str(e) 144 | 145 | 146 | if __name__ == '__main__': 147 | # Start the node 148 | node_name = os.path.splitext(os.path.basename(__file__))[0] + '_gripper' 149 | rospy.init_node(node_name) 150 | rospy.loginfo('Starting [%s] node' % node_name) 151 | 152 | jt_controller = GripperController() 153 | 154 | rospy.spin() 155 | rospy.loginfo('Shutting down [%s] node' % node_name) 156 | -------------------------------------------------------------------------------- /uuv_manipulators_control/scripts/joint_position_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | import os 17 | import rospy 18 | from copy import deepcopy 19 | from uuv_manipulator_interfaces import ArmInterface 20 | from PID import PIDRegulator 21 | from sensor_msgs.msg import Joy 22 | from std_msgs.msg import Float64 23 | 24 | 25 | class JointPositionController: 26 | def __init__(self): 27 | # Initializing arm interface 28 | self._arm_interface = ArmInterface() 29 | 30 | # PID controllers 31 | self._controllers = dict() 32 | # Current reference for each joint 33 | self._reference_pos = dict() 34 | # Output command topics 35 | self._command_topics = dict() 36 | # Axes mapping 37 | self._axes = dict() 38 | # Axes gain values 39 | self._axes_gain = dict() 40 | 41 | # Default for the RB button of the XBox 360 controller 42 | self._deadman_button = 5 43 | if rospy.has_param('~deadman_button'): 44 | self._deadman_button = int(rospy.get_param('~deadman_button')) 45 | 46 | # If these buttons are pressed, the arm will not move 47 | if rospy.has_param('~exclusion_buttons'): 48 | self._exclusion_buttons = rospy.get_param('~exclusion_buttons') 49 | if type(self._exclusion_buttons) in [float, int]: 50 | self._exclusion_buttons = [int(self._exclusion_buttons)] 51 | elif type(self._exclusion_buttons) == list: 52 | for n in self._exclusion_buttons: 53 | if type(n) not in [float, int]: 54 | raise rospy.ROSException('Exclusion buttons must be an' 55 | ' integer index to the joystick button') 56 | else: 57 | self._exclusion_buttons = list() 58 | 59 | # Default for the start button of the XBox 360 controller 60 | self._home_button = 7 61 | if rospy.has_param('~home_button'): 62 | self._home_button = int(rospy.get_param('~home_button')) 63 | 64 | # Last joystick update timestamp 65 | self._last_joy_update = rospy.get_time() 66 | # Joystick topic subscriber 67 | self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) 68 | 69 | # Reading the controller configuration 70 | controller_config = rospy.get_param('~controller_config') 71 | for joint in self._arm_interface.joint_names: 72 | for tag in controller_config: 73 | if tag in joint: 74 | try: 75 | # Read the controller parameters 76 | self._controllers[joint] = PIDRegulator(controller_config[tag]['controller']['p'], 77 | controller_config[tag]['controller']['i'], 78 | controller_config[tag]['controller']['d'], 79 | 1000) 80 | self._command_topics[joint] = rospy.Publisher( 81 | controller_config[tag]['topic'], 82 | Float64, 83 | queue_size=1) 84 | self._axes[joint] = controller_config[tag]['joint_input_axis'] 85 | self._axes_gain[joint] = controller_config[tag]['axis_gain'] 86 | 87 | # Setting the starting reference to the home position 88 | # in the robot parameters file 89 | self._reference_pos[joint] = deepcopy(self._arm_interface.home[joint]) 90 | except: 91 | raise rospy.ROSException('Error while trying to setup controller for joint <%s>' % joint) 92 | 93 | rate = rospy.Rate(50) 94 | 95 | while not rospy.is_shutdown(): 96 | pos = self._arm_interface.joint_angles 97 | for joint in pos: 98 | u = self._controllers[joint].regulate(self._reference_pos[joint] - pos[joint], rospy.get_time()) 99 | self._command_topics[joint].publish(Float64(u)) 100 | rate.sleep() 101 | 102 | def _check_joint_limits(self, value, joint): 103 | """Check the joint position and maintain it within joint limits""" 104 | output = value 105 | if self._arm_interface.joint_limits[joint] is not None: 106 | if output < self._arm_interface.joint_limits[joint]['lower']: 107 | output = self._arm_interface.joint_limits[joint]['lower'] 108 | if output > self._arm_interface.joint_limits[joint]['upper']: 109 | output = self._arm_interface.joint_limits[joint]['upper'] 110 | return output 111 | 112 | def _joy_callback(self, joy): 113 | try: 114 | # If deadman button is not pressed, do nothing 115 | if not joy.buttons[self._deadman_button] and self._deadman_button != -1: 116 | return 117 | # If any exclusion buttons are pressed, do nothing 118 | for n in self._exclusion_buttons: 119 | if joy.buttons[n] == 1: 120 | return 121 | if joy.buttons[self._home_button]: 122 | self._reference_pos = deepcopy(self._arm_interface.home) 123 | self._last_joy_update = rospy.get_time() 124 | else: 125 | # Parse the joystick input to set the joint angle reference 126 | for joint in self._arm_interface.joint_names: 127 | if abs(joy.axes[self._axes[joint]]) < 0.8: 128 | continue 129 | else: 130 | self._reference_pos[joint] += self._axes_gain[joint] * \ 131 | joy.axes[self._axes[joint]] 132 | # Check for the joint limits 133 | self._reference_pos[joint] = self._check_joint_limits(self._reference_pos[joint], joint) 134 | self._last_joy_update = rospy.get_time() 135 | except Exception, e: 136 | print 'Error during joy parsing, message=', e 137 | 138 | if __name__ == '__main__': 139 | # Start the node 140 | node_name = os.path.splitext(os.path.basename(__file__))[0] 141 | rospy.init_node(node_name) 142 | rospy.loginfo('Starting [%s] node' % node_name) 143 | 144 | controller = JointPositionController() 145 | 146 | rospy.spin() 147 | rospy.loginfo('Shuting down [%s] node' % node_name) 148 | -------------------------------------------------------------------------------- /uuv_manipulators_control/scripts/jt_cartesian_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import argparse 18 | import rospy 19 | import sys 20 | import os 21 | import numpy as np 22 | 23 | import PyKDL 24 | from uuv_manipulators_control import CartesianController 25 | 26 | 27 | class JTCartesianController(CartesianController): 28 | """ 29 | Jacobian transpose controller 30 | """ 31 | 32 | LABEL = 'Jacobian transpose cartesian controller' 33 | def __init__(self): 34 | """ 35 | Class constructor 36 | """ 37 | CartesianController.__init__(self) 38 | # Retrieve the controller parameters from the parameter server 39 | Kd_tag = '~Kd' 40 | Kp_tag = '~Kp' 41 | if not rospy.has_param(Kd_tag): 42 | rospy.ROSException('Kd gain vector not available for tag=%s' % Kd_tag) 43 | if not rospy.has_param(Kp_tag): 44 | rospy.ROSException('Kp gain vector not available for tag=%s' % Kp_tag) 45 | 46 | self._Kd = rospy.get_param(Kd_tag) 47 | self._Kp = rospy.get_param(Kp_tag) 48 | 49 | rospy.loginfo('Kp=%s', str(self._Kp)) 50 | rospy.loginfo('Kd=%s', str(self._Kd)) 51 | 52 | # Initialization flag, to wait the end-effector get to the home 53 | # position 54 | self._is_init = False 55 | 56 | self._run() 57 | 58 | def _update(self): 59 | # Leave if ROS is not running or command is not valid 60 | if rospy.is_shutdown() or self._last_goal is None: 61 | return 62 | # Calculate the goal pose 63 | goal = self._get_goal() 64 | 65 | # End-effector's pose 66 | ee_pose = self._arm_interface.get_ee_pose_as_frame() 67 | # End-effector's velocity 68 | ee_vel = self._arm_interface.get_ee_vel_as_kdl_twist() 69 | # Calculate pose error 70 | error = PyKDL.diff(goal, ee_pose) 71 | # End-effector wrench to achieve target 72 | wrench = np.matrix(np.zeros(6)).T 73 | for i in range(len(wrench)): 74 | wrench[i] = -(self._Kp[i] * error[i] + self._Kd[i] * ee_vel[i]) 75 | 76 | # Compute jacobian transpose 77 | JT = self._arm_interface.jacobian_transpose() 78 | # Calculate the torques for the joints 79 | tau = JT * wrench 80 | # Store current pose target 81 | self._last_goal = goal 82 | 83 | self.publish_goal() 84 | self.publish_joint_efforts(tau) 85 | 86 | if __name__ == '__main__': 87 | # Start the node 88 | node_name = os.path.splitext(os.path.basename(__file__))[0] 89 | rospy.init_node(node_name) 90 | rospy.loginfo('Starting [%s] node' % node_name) 91 | 92 | jt_controller = JTCartesianController() 93 | 94 | rospy.spin() 95 | rospy.loginfo('Shutting down [%s] node' % node_name) 96 | -------------------------------------------------------------------------------- /uuv_manipulators_control/scripts/set_joint_config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import argparse 18 | import sys 19 | import rospy 20 | import time 21 | from std_msgs.msg import Float64 22 | 23 | 24 | if __name__ == '__main__': 25 | # Create argument parser 26 | parser = argparse.ArgumentParser(description='uuv_manipulators_control::set_joint_config') 27 | parser.add_argument('--namespace', metavar='NS', type=str) 28 | parser.add_argument('--arm', metavar='NS', type=str) 29 | parser.add_argument('--config', metavar='CONFIG', type=str) 30 | 31 | # Filter out the garbage 32 | arguments = [a for a in sys.argv if ':=' not in a and '.py' not in a] 33 | 34 | args = parser.parse_args(arguments) 35 | 36 | # Manipulator's namespace 37 | assert args.namespace is not None, "Manipulator's namespace not given" 38 | namespace = args.namespace 39 | if namespace[0] != '/': 40 | namespace = '/' + namespace 41 | if namespace[-1] != '/': 42 | namespace = namespace + '/' 43 | 44 | # Desired configuration (the joint position must be specified in the 45 | # parameter server) 46 | assert args.config is not None, "Manipulator's desired configuration not given" 47 | config = args.config 48 | 49 | assert args.arm is not None, 'Manipulator name must be given' 50 | arm_name = args.arm 51 | 52 | rospy.init_node('set_joint_config') 53 | 54 | assert not rospy.is_shutdown(), 'ROS was not initialized' 55 | 56 | try: 57 | # Read the desired joint configuration from the parameter server 58 | assert rospy.has_param(namespace + 'arms/' + arm_name + '/default_configs/' + config), 'Desired configuration not in the parameter namespace, config=%s' % config 59 | 60 | joint_pos = rospy.get_param(namespace + 'arms/' + arm_name + '/default_configs/' + config) 61 | 62 | print 'Set joint configuration=', joint_pos 63 | print 'Output topics:' 64 | pub = {} 65 | for joint in joint_pos: 66 | print ' ', joint, '=', namespace + arm_name + '/' + joint + '/position_controller/command' 67 | pub[joint] = rospy.Publisher(namespace + arm_name + '/' + joint + '/position_controller/command', Float64, queue_size=1) 68 | 69 | start_time = time.clock() 70 | duration = 5 71 | rate = rospy.Rate(100.0) 72 | rospy.loginfo('Publishing command message for %s seconds' % duration) 73 | while time.clock() <= start_time + duration: 74 | if rospy.is_shutdown(): 75 | print 'ROS is not running' 76 | break 77 | 78 | for joint in joint_pos: 79 | pos_msg = Float64() 80 | pos_msg.data = joint_pos[joint] 81 | pub[joint].publish(pos_msg) 82 | 83 | rate.sleep() 84 | rospy.loginfo('Finishing setting joint configuration') 85 | 86 | except rospy.ROSInterruptException: 87 | print 'uuv_manipulators_control::set_joint_config::Exception' 88 | print 'Leaving uuv_manipulators_control::set_joint_config' 89 | -------------------------------------------------------------------------------- /uuv_manipulators_control/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | from distutils.core import setup 18 | from catkin_pkg.python_setup import generate_distutils_setup 19 | 20 | d = generate_distutils_setup() 21 | d['packages'] = ['uuv_manipulators_control'] 22 | d['package_dir'] = {'': 'src'} 23 | 24 | setup(**d) 25 | -------------------------------------------------------------------------------- /uuv_manipulators_control/src/uuv_manipulators_control/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | from .cartesian_controller import CartesianController 17 | -------------------------------------------------------------------------------- /uuv_manipulators_control/src/uuv_manipulators_control/cartesian_controller.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | import rospy 17 | from uuv_manipulator_interfaces import ArmInterface 18 | from geometry_msgs.msg import Twist, PoseStamped, Quaternion, Vector3 19 | from visualization_msgs.msg import Marker 20 | from std_msgs.msg import Float64, Bool 21 | from copy import deepcopy 22 | import numpy as np 23 | import tf 24 | import tf.transformations as trans 25 | from tf_conversions import posemath 26 | import PyKDL 27 | 28 | 29 | class CartesianController(object): 30 | LABEL = 'None' 31 | def __init__(self): 32 | # Timeout (to filter out inactivity) 33 | self._timeout = 0.5 34 | 35 | # Initializing the arm interface for the manipulator in the current 36 | # namespace 37 | self._arm_interface = ArmInterface() 38 | 39 | # Last goal for the end-effector pose/position 40 | self._last_goal = self._arm_interface.get_config_in_ee_frame('home') 41 | 42 | # Retrieve the publish rate 43 | self._publish_rate = 25 44 | if rospy.has_param('~publish_rate'): 45 | self._publish_rate = rospy.get_param('~publish_rate') 46 | 47 | if self._publish_rate <= 0: 48 | raise rospy.ROSException('Invalid negative publish rate') 49 | 50 | self._tau = 0.1 51 | 52 | # Get the transformation between the robot's base link and the 53 | # vehicle's base link 54 | self.listener = tf.TransformListener() 55 | 56 | # Get latest transform available 57 | latest = rospy.Time(0) 58 | base = self._arm_interface.namespace + 'base_link' 59 | # self.listener.waitForTransform(base, self._arm_interface.base_link, 60 | # latest, latest + rospy.Duration(100)) 61 | self.listener.waitForTransform(base, self._arm_interface.base_link, 62 | latest, rospy.Duration(100)) 63 | [pos, quat] = self.listener.lookupTransform( 64 | base, self._arm_interface.base_link, latest) 65 | 66 | rot = PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]) 67 | # Store transformation from the arm's base link and base 68 | self._trans = PyKDL.Frame(rot, PyKDL.Vector(pos[0], pos[1], pos[2])) 69 | 70 | # Velocity reference 71 | self._command = None 72 | 73 | # Last controller update 74 | self._last_controller_update = rospy.get_time() 75 | 76 | # Last velocity reference update time stamp 77 | self._last_reference_update = rospy.get_time() 78 | 79 | rospy.set_param('~name', self.LABEL) 80 | 81 | self._joint_effort_pub = dict() 82 | 83 | for joint in self._arm_interface.joint_names: 84 | self._joint_effort_pub[joint] = rospy.Publisher( 85 | self._arm_interface.namespace + 86 | joint + '/controller/command', 87 | Float64, queue_size=1) 88 | 89 | # Input velocity command subscriber, remap this topic to set a custom 90 | # command topic 91 | self._vel_command_sub = rospy.Subscriber( 92 | 'cmd_vel', Twist, self._vel_command_callback) 93 | 94 | # Topic that will receive the flag if the home button was pressed. If 95 | # the flag is true, the manipulator's goal is set to the stow 96 | # configuration 97 | self._home_pressed_sub = rospy.Subscriber( 98 | 'home_pressed', Bool, self._home_button_pressed) 99 | 100 | # Topic publishes the current goal set as reference to the manipulator 101 | self._goal_pub = rospy.Publisher( 102 | 'reference', PoseStamped, queue_size=1) 103 | 104 | # Topic to publish a visual marker for visualization of the current 105 | # reference in RViz 106 | self._goal_marker_pub = rospy.Publisher( 107 | 'reference_marker', Marker, queue_size=1) 108 | 109 | def _update(self, event): 110 | raise NotImplementedError() 111 | 112 | def _run(self): 113 | rate = rospy.Rate(self._publish_rate) 114 | while not rospy.is_shutdown(): 115 | self._update() 116 | rate.sleep() 117 | 118 | def _filter_input(self, state, cmd, dt): 119 | alpha = np.exp(- 1 * dt / self._tau) 120 | return state * alpha + (1.0 - alpha) * cmd 121 | 122 | def _get_goal(self): 123 | if self._command is None or rospy.get_time() - self._last_reference_update > 0.1: 124 | return self._last_goal 125 | 126 | next_goal = deepcopy(self._last_goal) 127 | next_goal.p += PyKDL.Vector(self._command[0], self._command[1], self._command[2]) 128 | 129 | q_step = trans.quaternion_from_euler(self._command[3], 130 | self._command[4], 131 | self._command[5]) 132 | q_last = trans.quaternion_from_euler(*self._last_goal.M.GetRPY()) 133 | q_next = trans.quaternion_multiply(q_last, q_step) 134 | 135 | next_goal.M = PyKDL.Rotation.Quaternion(*q_next) 136 | 137 | g_pos = [next_goal.p.x(), next_goal.p.y(), next_goal.p.z()] 138 | g_quat = next_goal.M.GetQuaternion() 139 | if self._arm_interface.inverse_kinematics(g_pos, g_quat) is not None: 140 | return next_goal 141 | else: 142 | print 'Next goal could not be resolved by the inv. kinematics solver.' 143 | return self._last_goal 144 | 145 | def _home_button_pressed(self, msg): 146 | if msg.data: 147 | self._command = np.zeros(6) 148 | self._last_goal = self._arm_interface.get_config_in_ee_frame('home') 149 | 150 | def _vel_command_callback(self, msg): 151 | dt = rospy.get_time() - self._last_reference_update 152 | if dt > 0.1: 153 | self._command = np.zeros(6) 154 | self._last_reference_update = rospy.get_time() 155 | return 156 | 157 | if self._command is None: 158 | self._command = np.zeros(6) 159 | self._command[0] = self._filter_input(self._command[0], msg.linear.x, dt) * dt 160 | self._command[1] = self._filter_input(self._command[1], msg.linear.y, dt) * dt 161 | self._command[2] = self._filter_input(self._command[2], msg.linear.z, dt) * dt 162 | 163 | self._command[3] = self._filter_input(self._command[3], msg.angular.x, dt) * dt 164 | self._command[4] = self._filter_input(self._command[4], msg.angular.y, dt) * dt 165 | self._command[5] = self._filter_input(self._command[5], msg.angular.z, dt) * dt 166 | 167 | self._last_reference_update = rospy.get_time() 168 | 169 | def publish_goal(self): 170 | # Publish the reference topic 171 | msg = PoseStamped() 172 | msg.header.frame_id = self._arm_interface.base_link 173 | msg.header.stamp = rospy.Time.now() 174 | 175 | msg.pose.position.x = self._last_goal.p.x() 176 | msg.pose.position.y = self._last_goal.p.y() 177 | msg.pose.position.z = self._last_goal.p.z() 178 | 179 | msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) 180 | 181 | self._goal_pub.publish(msg) 182 | 183 | marker = Marker() 184 | marker.header.frame_id = self._arm_interface.base_link 185 | marker.header.stamp = rospy.Time.now() 186 | marker.ns = self._arm_interface.arm_name 187 | marker.id = 0 188 | marker.type = Marker.SPHERE 189 | marker.action = Marker.MODIFY 190 | marker.pose.position = Vector3(self._last_goal.p.x(), 191 | self._last_goal.p.y(), 192 | self._last_goal.p.z()) 193 | marker.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion()) 194 | marker.scale = Vector3(0.2, 0.2, 0.2) 195 | marker.color.a = 1.0 196 | marker.color.r = 1.0 197 | marker.color.g = 0.0 198 | marker.color.b = 0.0 199 | 200 | self._goal_marker_pub.publish(marker) 201 | 202 | def publish_joint_efforts(self, tau): 203 | # Publish torques 204 | t = np.asarray(tau).squeeze() 205 | for i, name in enumerate(self._arm_interface.joint_names): 206 | torque = Float64() 207 | torque.data = t[i] 208 | self._joint_effort_pub[name].publish(torque) 209 | # Update the time stamp 210 | self._last_controller_update = rospy.get_time() 211 | -------------------------------------------------------------------------------- /uuv_manipulators_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package uuv_manipulators_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * UPDATE Catkin packages format to 2 43 | Signed-off-by: Musa Morena Marcusso Manhaes 44 | * Contributors: Musa Morena Marcusso Manhaes 45 | 46 | 0.5.4 (2018-07-04) 47 | ------------------ 48 | * UPDATE Catkin packages format to 2 49 | Signed-off-by: Musa Morena Marcusso Manhaes 50 | * Contributors: Musa Morena Marcusso Manhaes 51 | 52 | 0.5.3 (2018-07-04) 53 | ------------------ 54 | * ADD CHANGELOG files 55 | Signed-off-by: Musa Morena Marcusso Manhaes 56 | * Contributors: Musa Morena Marcusso Manhaes 57 | 58 | 0.5.1 (2018-07-03) 59 | ------------------ 60 | * CHANGE Bump version to 0.5.2 61 | Signed-off-by: Musa Morena Marcusso Manhaes 62 | * CHANGE Version 63 | * CHANGE Package versions 64 | Signed-off-by: Musa Morena Marcusso Manhaes 65 | * FIX Typos and package version 66 | Signed-off-by: Musa Morena Marcusso Manhaes 67 | * FIX Missing hardware_interface namespace. 68 | Signed-off-by: Musa Morena Marcusso Manhaes 69 | * fix several files not being installed (can now source install/setup.bash) 70 | Signed-off-by: Sebastian Scherer 71 | * initial commit 72 | Signed-off-by: Sebastian Scherer (CR/AEI) 73 | * Contributors: Musa Morena Marcusso Manhaes, Sebastian Scherer, Sebastian Scherer (CR/AEI) 74 | -------------------------------------------------------------------------------- /uuv_manipulators_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uuv_manipulators_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY urdf 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | PATTERN "*~" EXCLUDE) 11 | -------------------------------------------------------------------------------- /uuv_manipulators_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uuv_manipulators_description 4 | 0.6.1 5 | Common macros to build the robot description of manipulators. 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | -------------------------------------------------------------------------------- /uuv_manipulators_description/urdf/common.xacro: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | transmission_interface/SimpleTransmission 28 | 29 | hardware_interface/EffortJointInterface 30 | 31 | 32 | hardware_interface/EffortJointInterface 33 | 1 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | transmission_interface/SimpleTransmission 42 | 43 | hardware_interface/PositionJointInterface 44 | 45 | 46 | hardware_interface/PositionJointInterface 47 | 1 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 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package uuv_manipulators_kinematics 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | * FIX Parse output from KDL parser 8 | Signed-off-by: Musa Morena Marcusso Manhaes 9 | * Contributors: Musa Morena Marcusso Manhaes 10 | 11 | 0.6.0 (2018-07-31) 12 | ------------------ 13 | * MV uuv_manipulators package transfered from uuv_simulator 14 | Signed-off-by: Musa Morena Marcusso Manhaes 15 | * Contributors: Musa Morena Marcusso Manhaes 16 | 17 | 0.5.13 (2018-07-24) 18 | ------------------- 19 | 20 | 0.5.12 (2018-07-23) 21 | ------------------- 22 | * RM Forward/inverse kinematics tests for now 23 | Signed-off-by: Musa Morena Marcusso Manhaes 24 | * CHANGE Launch tests from test_kinematic_interfaces.test 25 | Signed-off-by: Musa Morena Marcusso Manhaes 26 | * MV Kinematics interfaces test file 27 | Signed-off-by: Musa Morena Marcusso Manhaes 28 | * RM Old test launch files 29 | Signed-off-by: Musa Morena Marcusso Manhaes 30 | * Contributors: Musa Morena Marcusso Manhaes 31 | 32 | 0.5.11 (2018-07-21) 33 | ------------------- 34 | 35 | 0.5.10 (2018-07-10) 36 | ------------------- 37 | 38 | 0.5.9 (2018-07-09) 39 | ------------------ 40 | 41 | 0.5.8 (2018-07-07) 42 | ------------------ 43 | 44 | 0.5.7 (2018-07-06) 45 | ------------------ 46 | 47 | 0.5.6 (2018-07-06) 48 | ------------------ 49 | 50 | 0.5.5 (2018-07-05) 51 | ------------------ 52 | * RM Merge messages from the change log 53 | Signed-off-by: Musa Morena Marcusso Manhaes 54 | * UPDATE Catkin packages format to 2 55 | Signed-off-by: Musa Morena Marcusso Manhaes 56 | * Contributors: Musa Morena Marcusso Manhaes 57 | 58 | 0.5.4 (2018-07-04) 59 | ------------------ 60 | * UPDATE Catkin packages format to 2 61 | Signed-off-by: Musa Morena Marcusso Manhaes 62 | * Contributors: Musa Morena Marcusso Manhaes 63 | 64 | 0.5.3 (2018-07-04) 65 | ------------------ 66 | * ADD CHANGELOG files 67 | Signed-off-by: Musa Morena Marcusso Manhaes 68 | * Contributors: Musa Morena Marcusso Manhaes 69 | 70 | 0.5.1 (2018-07-03) 71 | ------------------ 72 | * CHANGE Bump version to 0.5.2 73 | Signed-off-by: Musa Morena Marcusso Manhaes 74 | * CHANGE Version 75 | * CHANGE Package versions 76 | Signed-off-by: Musa Morena Marcusso Manhaes 77 | * FIX Typos and package version 78 | Signed-off-by: Musa Morena Marcusso Manhaes 79 | * FIX Package dependencies for rosdep 80 | Signed-off-by: Musa Morena Marcusso Manhaes 81 | * ADD Functionalities for generation of Jacobian matrices 82 | Signed-off-by: Musa Morena Marcusso Manhaes 83 | * FIX PyKDL call for Jacobian computation. 84 | Signed-off-by: Musa Morena Marcusso Manhães 85 | * CHANGE Package configuration for catkin tools. 86 | Signed-off-by: Musa Morena Marcusso Manhães 87 | * FIX Jacobian matrix null output. 88 | Signed-off-by: Musa Morena Marcusso Manhães 89 | * RM Redundant initialization of the KDL inverse kinematics solver. 90 | Signed-off-by: Musa Morena Marcusso Manhães 91 | * ADD Test for forward and inverse kinematics. 92 | Signed-off-by: Musa Morena Marcusso Manhães 93 | * CHANGE Move inverse kinematics method to the kinematic chain interface class. 94 | Signed-off-by: Musa Morena Marcusso Manhães 95 | * CHANGE Move the inverse kinematics solver to the arm interface. 96 | Signed-off-by: Musa Morena Marcusso Manhães 97 | * FIX CMakeLists files 98 | Signed-off-by: Musa Morena Marcusso Manhães 99 | * ADD Option to generate Jacobian matrices different final joint entities. 100 | Signed-off-by: Musa Morena Marcusso Manhães 101 | * ADD Test units for the arm interface. 102 | Signed-off-by: Musa Morena Marcusso Manhães 103 | * uuv_manipulator_interfaces: catch sqrt of 0 error 104 | Signed-off-by: Sebastian Scherer 105 | * Removing unused MimicJointPlugin. 106 | Signed-off-by: Musa Morena Marcusso Manhães 107 | * Adding the information for mimic joints (gripper control) 108 | Signed-off-by: Musa Morena Marcusso Manhães 109 | * Correcting import of xml_reflection package. 110 | Signed-off-by: Musa Morena Marcusso Manhães 111 | * install subdirectories in modules via setup.py 112 | Signed-off-by: Sebastian Scherer 113 | * fix several files not being installed (can now source install/setup.bash) 114 | Signed-off-by: Sebastian Scherer 115 | * initial commit 116 | Signed-off-by: Sebastian Scherer (CR/AEI) 117 | * Contributors: Musa Morena Marcusso Manhaes, Musa Morena Marcusso Manhães, Sebastian Scherer, Sebastian Scherer (CR/AEI) 118 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uuv_manipulators_kinematics) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | gazebo_ros 8 | control_toolbox 9 | rospy 10 | uuv_manipulators_msgs 11 | geometry_msgs 12 | sensor_msgs 13 | std_msgs 14 | tf) 15 | 16 | find_package(orocos_kdl REQUIRED) 17 | 18 | catkin_python_setup() 19 | 20 | catkin_package() 21 | 22 | ############# 23 | ## Install ## 24 | ############# 25 | 26 | catkin_install_python(PROGRAMS scripts/kinematics_service.py 27 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 28 | 29 | if(CATKIN_ENABLE_TESTING) 30 | find_package(rostest REQUIRED) 31 | 32 | foreach(TEST_LAUNCHERS 33 | test/test_kinematic_interfaces.test) 34 | add_rostest(${TEST_LAUNCHERS}) 35 | endforeach() 36 | endif() 37 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uuv_manipulators_kinematics 4 | 0.6.1 5 | Implementation of interfaces to operate manipulators and grippers along with kinematics solver services. 6 | 7 | Luiz Ricardo Douat 8 | Musa Morena Marcusso Manhaes 9 | Sebastian Scherer 10 | 11 | Apache-2.0 12 | 13 | catkin 14 | 15 | gazebo_ros 16 | control_toolbox 17 | rospy 18 | orocos_kdl 19 | python-numpy 20 | python-lxml 21 | python-yaml 22 | uuv_manipulators_msgs 23 | geometry_msgs 24 | sensor_msgs 25 | std_msgs 26 | tf 27 | urdfdom_py 28 | kdl_parser_py 29 | 30 | rosunit 31 | rostest 32 | 33 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/scripts/kinematics_service.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import PyKDL 18 | import rospy 19 | import numpy as np 20 | from kdl_parser_py.urdf import treeFromUrdfModel 21 | from urdf_parser_py.urdf import URDF 22 | from uuv_manipulator_interfaces import ArmInterface 23 | from uuv_manipulators_msgs.msg import EndPointState 24 | from uuv_manipulators_msgs.srv import SolveIK, SolveIKResponse 25 | from geometry_msgs.msg import Pose, Twist, Wrench, Vector3 26 | from sensor_msgs.msg import JointState 27 | from std_msgs.msg import Float64 28 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 29 | 30 | import sys 31 | import os 32 | import numpy as np 33 | 34 | 35 | class KinematicsService(object): 36 | def __init__(self): 37 | ns = [item for item in rospy.get_namespace().split('/') if len(item) > 0] 38 | if len(ns) != 2: 39 | rospy.ROSException('The controller must be called in the manipulator namespace') 40 | 41 | self._namespace = ns[0] 42 | self._arm_name = ns[1] 43 | 44 | if self._namespace[0] != '/': 45 | self._namespace = '/' + self._namespace 46 | 47 | if self._namespace[-1] != '/': 48 | self._namespace += '/' 49 | # The arm interface loads the parameters from the URDF file and 50 | # parameter server and initializes the KDL tree 51 | self._arm_interface = ArmInterface() 52 | 53 | self._base_link = self._arm_interface.base_link 54 | self._tip_link = self._arm_interface.tip_link 55 | 56 | self._tip_frame = PyKDL.Frame() 57 | 58 | # KDL Solvers 59 | self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_interface.chain) 60 | self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR(self._arm_interface.chain, 61 | self._arm_interface._fk_p_kdl, 62 | self._ik_v_kdl, 63 | 100, 64 | 1e-6) 65 | self._dyn_kdl = PyKDL.ChainDynParam(self._arm_interface.chain, 66 | PyKDL.Vector.Zero()) 67 | 68 | # Add a callback function to calculate the end effector's state by each 69 | # update of the joint state 70 | self._arm_interface.add_callback('joint_states', 71 | self.on_update_endeffector_state) 72 | # Publish the current manipulability index at each update of the joint 73 | # states 74 | # self._arm_interface.add_callback('joint_states', 75 | # self.publish_man_index) 76 | 77 | # Publish the end effector state 78 | self._endeffector_state_pub = rospy.Publisher('endpoint_state', 79 | EndPointState, 80 | queue_size=1) 81 | 82 | # Publish the manipulability index 83 | self._man_index_pub = rospy.Publisher('man_index', 84 | Float64, 85 | queue_size=1) 86 | 87 | self._services = dict() 88 | # Provide the service to calculate the inverse kinematics using the KDL solver 89 | self._services['ik'] = rospy.Service('ik_solver', SolveIK, self.solve_ik) 90 | 91 | @property 92 | def joint_names(self): 93 | return self._arm_interface.joint_names 94 | 95 | @property 96 | def joint_angles(self): 97 | return self._arm_interface.joint_angles 98 | 99 | @property 100 | def joint_velocities(self): 101 | return self._arm_interface.joint_velocities 102 | 103 | @property 104 | def joint_efforts(self): 105 | return self._arm_interface.joint_efforts 106 | 107 | @property 108 | def home(self): 109 | return self._arm_interface.home 110 | 111 | def solve_ik(self, req): 112 | out = SolveIKResponse() 113 | out.isValid = False 114 | out.joints = JointState() 115 | 116 | pos = [req.pose.position.x, req.pose.position.y, req.pose.position.z] 117 | orientation = [req.pose.orientation.x, req.pose.orientation.y, 118 | req.pose.orientation.z, req.pose.orientation.w] 119 | 120 | result_ik = self._arm_interface.inverse_kinematics(pos, orientation) 121 | 122 | if result_ik is not None: 123 | for i, name in zip(range(len(self.joint_names)), self.joint_names): 124 | out.joints.name.append(name) 125 | out.joints.position.append(result_ik[i]) 126 | out.isValid = True 127 | return out 128 | 129 | def publish_man_index(self): 130 | # Retrieve current jacobian matrix 131 | w_msg = Float64() 132 | w_msg.data = self._arm_interface.man_index 133 | self._man_index_pub.publish(w_msg) 134 | 135 | def on_update_endeffector_state(self): 136 | state_msg = EndPointState() 137 | # Store everything in the end point state message 138 | state_msg.pose.position.x = self._arm_interface.endeffector_pose['position'][0] 139 | state_msg.pose.position.y = self._arm_interface.endeffector_pose['position'][1] 140 | state_msg.pose.position.z = self._arm_interface.endeffector_pose['position'][2] 141 | 142 | state_msg.pose.orientation.x = self._arm_interface.endeffector_pose['orientation'][0] 143 | state_msg.pose.orientation.y = self._arm_interface.endeffector_pose['orientation'][1] 144 | state_msg.pose.orientation.z = self._arm_interface.endeffector_pose['orientation'][2] 145 | state_msg.pose.orientation.w = self._arm_interface.endeffector_pose['orientation'][3] 146 | 147 | state_msg.twist.linear.x = self._arm_interface.endeffector_twist['linear'][0] 148 | state_msg.twist.linear.y = self._arm_interface.endeffector_twist['linear'][1] 149 | state_msg.twist.linear.z = self._arm_interface.endeffector_twist['linear'][2] 150 | 151 | state_msg.twist.angular.x = self._arm_interface.endeffector_twist['angular'][0] 152 | state_msg.twist.angular.y = self._arm_interface.endeffector_twist['angular'][1] 153 | state_msg.twist.angular.z = self._arm_interface.endeffector_twist['angular'][2] 154 | 155 | state_msg.wrench.force.x = self._arm_interface.endeffector_wrench['force'][0] 156 | state_msg.wrench.force.y = self._arm_interface.endeffector_wrench['force'][1] 157 | state_msg.wrench.force.z = self._arm_interface.endeffector_wrench['force'][2] 158 | 159 | state_msg.wrench.torque.x = self._arm_interface.endeffector_wrench['torque'][0] 160 | state_msg.wrench.torque.y = self._arm_interface.endeffector_wrench['torque'][0] 161 | state_msg.wrench.torque.z = self._arm_interface.endeffector_wrench['torque'][0] 162 | 163 | self._endeffector_state_pub.publish(state_msg) 164 | 165 | if __name__ == '__main__': 166 | # Start the node 167 | node_name = os.path.splitext(os.path.basename(__file__))[0] 168 | rospy.init_node(node_name) 169 | rospy.loginfo('Starting [%s] node' % node_name) 170 | 171 | manipulator_kin = KinematicsService() 172 | 173 | rospy.spin() 174 | rospy.loginfo('Shuting down [%s] node' % node_name) 175 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | from distutils.core import setup 18 | from catkin_pkg.python_setup import generate_distutils_setup 19 | 20 | d = generate_distutils_setup() 21 | d['packages'] = ['uuv_kinematics_utils', 'uuv_manipulator_interfaces', 'uuv_manipulator_nodes'] 22 | d['package_dir'] = {'': 'src'} 23 | 24 | setup(**d) 25 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/src/uuv_kinematics_utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | from .endeffector_state import EndEffectorState 17 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/src/uuv_kinematics_utils/endeffector_state.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | import numpy as np 17 | from uuv_manipulators_msgs.msg import EndPointState 18 | from geometry_msgs.msg import Pose 19 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 20 | import PyKDL 21 | 22 | 23 | class EndEffectorState(object): 24 | def __init__(self): 25 | self._pose = {'position': [0, 0, 0], 26 | 'orientation': [0, 0, 0, 0]} 27 | 28 | self._velocity = {'linear': [0, 0, 0], 29 | 'angular': [0, 0, 0]} 30 | 31 | self._effort = {'force': [0, 0, 0], 32 | 'torque': [0, 0, 0]} 33 | 34 | @property 35 | def pose(self): 36 | return self._pose 37 | 38 | @property 39 | def position(self): 40 | return self._pose['position'] 41 | 42 | @position.setter 43 | def position(self, value): 44 | assert value.size == 3 45 | self._pose['position'] = value 46 | 47 | @property 48 | def orientation(self): 49 | return self._pose['orientation'] 50 | 51 | @orientation.setter 52 | def orientation(self, value): 53 | assert value.size == 4 54 | self._pose['orientation'] = value 55 | 56 | @property 57 | def linear_velocity(self): 58 | return self._velocity['linear'] 59 | 60 | @linear_velocity.setter 61 | def linear_velocity(self, value): 62 | assert value.size == 3 63 | self._velocity['linear'] = value 64 | 65 | @property 66 | def angular_velocity(self): 67 | return self._velocity['angular'] 68 | 69 | @angular_velocity.setter 70 | def angular_velocity(self, value): 71 | assert value.size == 3 72 | self._velocity['angular'] = value 73 | 74 | @property 75 | def force(self): 76 | return self._effort['force'] 77 | 78 | @force.setter 79 | def force(self, value): 80 | assert value.size == 3 81 | self._effort['force'] = value 82 | 83 | @property 84 | def torque(self): 85 | return self._effort['torque'] 86 | 87 | @torque.setter 88 | def torque(self, value): 89 | assert value.size == 3 90 | self._effort['torque'] = value 91 | 92 | def to_msg(self): 93 | pose = Pose() 94 | pose.position.x = self._pose['position'][0] 95 | pose.position.y = self._pose['position'][1] 96 | pose.position.z = self._pose['position'][2] 97 | 98 | pose.orientation.x = self._pose['orientation'][0] 99 | pose.orientation.y = self._pose['orientation'][1] 100 | pose.orientation.z = self._pose['orientation'][2] 101 | pose.orientation.w = self._pose['orientation'][3] 102 | 103 | return pose 104 | 105 | def to_frame(self): 106 | quaternion = (self._pose['orientation'][0], 107 | self._pose['orientation'][1], 108 | self._pose['orientation'][2], 109 | self._pose['orientation'][3]) 110 | 111 | euler = euler_from_quaternion(quaternion) 112 | frame = PyKDL.Frame(PyKDL.Rotation.RPY(euler[0], euler[1], euler[2]), 113 | PyKDL.Vector(self._pose['position'][0], 114 | self._pose['position'][1], 115 | self._pose['position'][2])) 116 | return frame 117 | 118 | def to_kdl_twist(self): 119 | vel = PyKDL.Vector(self._velocity['linear'][0], 120 | self._velocity['linear'][1], 121 | self._velocity['linear'][2]) 122 | rot = PyKDL.Vector(self._velocity['angular'][0], 123 | self._velocity['angular'][1], 124 | self._velocity['angular'][2]) 125 | return PyKDL.Twist(vel, rot) 126 | 127 | def parse_message(self, msg): 128 | self._pose['position'] = [msg.pose.position.x, 129 | msg.pose.position.y, 130 | msg.pose.position.z] 131 | 132 | self._pose['orientation'] = [msg.pose.orientation.x, 133 | msg.pose.orientation.y, 134 | msg.pose.orientation.z, 135 | msg.pose.orientation.w] 136 | 137 | self._velocity['linear'] = [msg.twist.linear.x, 138 | msg.twist.linear.y, 139 | msg.twist.linear.z] 140 | 141 | self._velocity['angular'] = [msg.twist.angular.x, 142 | msg.twist.angular.y, 143 | msg.twist.angular.z] 144 | 145 | self._effort['force'] = [msg.wrench.force.x, 146 | msg.wrench.force.y, 147 | msg.wrench.force.z] 148 | 149 | self._effort['torque'] = [msg.wrench.torque.x, 150 | msg.wrench.torque.y, 151 | msg.wrench.torque.z] 152 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | from .arm import ArmInterface 17 | from .gripper import GripperInterface 18 | from .kin_chain import KinChainInterface 19 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/src/uuv_manipulator_interfaces/arm.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | from copy import deepcopy 17 | import rospy 18 | 19 | import numpy as np 20 | import PyKDL 21 | from uuv_manipulators_msgs.msg import EndPointState 22 | from uuv_kinematics_utils import EndEffectorState 23 | from uuv_manipulator_interfaces.kin_chain import KinChainInterface 24 | from sensor_msgs.msg import JointState 25 | from std_msgs.msg import Float64 26 | from tf.transformations import quaternion_from_euler, euler_from_quaternion 27 | 28 | 29 | class ArmInterface(KinChainInterface): 30 | """Interface class to the arm.""" 31 | 32 | STATE = ['moving', 'disabled'] 33 | def __init__(self, namespace=None, arm_name=None, compute_fk_for_all=False): 34 | """Class constructor""" 35 | if None in [namespace, arm_name]: 36 | ns = [item for item in rospy.get_namespace().split('/') if len(item) > 0] 37 | if len(ns) != 2: 38 | rospy.ROSException('The controller must be called inside the namespace the manipulator namespace') 39 | 40 | namespace = ns[0] 41 | arm_name = ns[1] 42 | 43 | param_path = '/' + namespace + '/arms/' + arm_name 44 | 45 | # Callbacks called in topic handlers 46 | self._callbacks = {} 47 | 48 | # Published topics 49 | self._pubTopics = {} 50 | # Subscribed topics 51 | self._subTopics = {} 52 | 53 | # Initializing the end-effector state 54 | self._endeffector_state = EndEffectorState() 55 | 56 | # Retrieve default configurations 57 | self._default_configs = dict() 58 | if rospy.has_param(param_path + '/default_configs'): 59 | self._default_configs = rospy.get_param(param_path + '/default_configs') 60 | else: 61 | self._default_configs = None 62 | 63 | base_link = None 64 | tip_link = None 65 | 66 | self._man_index = 0.0 67 | 68 | assert rospy.has_param(param_path + '/base_link'), 'Base link name not available in the namespace ' + namespace 69 | assert rospy.has_param(param_path + '/tip_link'), 'Tip link name not available in the namespace ' + namespace 70 | 71 | # Retrieve the names of the base and tip links 72 | base_link = rospy.get_param(param_path + '/base_link') 73 | tip_link = rospy.get_param(param_path + '/tip_link') 74 | 75 | # Initialize kinematics chain interface 76 | KinChainInterface.__init__(self, 77 | name=arm_name, 78 | base=base_link, 79 | ee_link=tip_link, 80 | namespace=namespace, 81 | arm_name=arm_name, 82 | compute_fk_for_all=compute_fk_for_all) 83 | 84 | self.set_joint_names(self._joint_names) 85 | 86 | self._subTopics['joint_states'] = rospy.Subscriber( 87 | self._namespace + 'joint_states', 88 | JointState, 89 | self._on_joint_states, 90 | queue_size=1, 91 | tcp_nodelay=True) 92 | 93 | def __del__(self): 94 | """Class destructor.""" 95 | for topic in self._subTopics: 96 | self._subTopics[topic].unregister() 97 | for topic in self._pubTopics: 98 | self._pubTopics[topic].unregister() 99 | 100 | @property 101 | def endeffector_pose(self): 102 | return dict(position=self._endeffector_state.position, 103 | orientation=self._endeffector_state.orientation) 104 | 105 | @property 106 | def endeffector_twist(self): 107 | return dict(linear=self._endeffector_state.linear_velocity, 108 | angular=self._endeffector_state.angular_velocity) 109 | 110 | @property 111 | def endeffector_wrench(self): 112 | return dict(force=self._endeffector_state.force, 113 | torque=self._endeffector_state.torque) 114 | 115 | @property 116 | def home(self): 117 | """ 118 | Returns the joint configuration of the home states 119 | """ 120 | if 'home' not in self._default_configs: 121 | return None 122 | else: 123 | return self._default_configs['home'] 124 | 125 | @property 126 | def default_configs(self): 127 | return self._default_configs 128 | 129 | @property 130 | def man_index(self): 131 | """ 132 | Returns the manipulability index 133 | """ 134 | return self._man_index 135 | 136 | def _on_joint_states(self, msg): 137 | # Store the joint states 138 | self.update_joint_states(msg) 139 | 140 | # Update manipulability index 141 | self.update_man_index() 142 | # Update end-effector state 143 | self.update_endeffector_state() 144 | 145 | if 'joint_states' in self._callbacks: 146 | if len(self._callbacks['joint_states']): 147 | for fcn in self._callbacks['joint_states']: 148 | fcn() 149 | 150 | def update_man_index(self): 151 | # Retrieve current jacobian matrix 152 | J = self.jacobian() 153 | det = np.linalg.det(np.dot(J, J.transpose())) 154 | self._man_index = 0.0 155 | if det > 0: 156 | self._man_index = np.sqrt(det) 157 | 158 | def update_endeffector_state(self): 159 | # Read joint states from the kinematics interface 160 | q = self.joint_angles 161 | qd = self.joint_velocities 162 | eff = self.joint_efforts 163 | 164 | # Compute the pose of all frames, if requested 165 | if self._compute_fk_for_all: 166 | for idx in xrange(self.n_links): 167 | self._frames[idx] = self.forward_position_kinematics(q, segment_idx=idx) 168 | # End effector pose 169 | pose = self.forward_position_kinematics(q) 170 | vel = self.forward_velocity_kinematics(q, qd) 171 | wrench = np.dot(self.jacobian(q), np.array(eff.values())) 172 | # Store everything in the end point state message 173 | self._endeffector_state.position = pose[0:3] 174 | self._endeffector_state.orientation = pose[3::] 175 | 176 | self._endeffector_state.linear_velocity = np.array([vel.vel.x(), 177 | vel.vel.y(), 178 | vel.vel.z()]) 179 | self._endeffector_state.angular_velocity = np.array([vel.rot.x(), 180 | vel.rot.y(), 181 | vel.rot.z()]) 182 | self._endeffector_state.force = np.array([wrench[0, 0], 183 | wrench[0, 1], 184 | wrench[0, 2]]) 185 | self._endeffector_state.torque = np.array([wrench[0, 3], 186 | wrench[0, 4], 187 | wrench[0, 5]]) 188 | 189 | def set_joint_names(self, jnt_names): 190 | self._joint_names = jnt_names 191 | 192 | # Adapting the joint names to the namespace 193 | if self._default_configs is not None: 194 | new_configs = {} 195 | for config in self._default_configs: 196 | new_configs[config] = {} 197 | for joint in self._default_configs[config]: 198 | for j in self._joint_names: 199 | if joint in j: 200 | new_configs[config][j] = self._default_configs[config][joint] 201 | self._default_configs = new_configs 202 | 203 | def get_ee_pose_as_msg(self): 204 | return self._endeffector_state.to_msg() 205 | 206 | def get_ee_pose_as_frame(self): 207 | return self._endeffector_state.to_frame() 208 | 209 | def get_ee_vel_as_kdl_twist(self): 210 | return self._endeffector_state.to_kdl_twist() 211 | 212 | def get_config_in_ee_frame(self, config='home'): 213 | assert config in self._default_configs, 'Invalid default configuration' 214 | 215 | j_pos = self._default_configs[config] 216 | pose = self.forward_position_kinematics(j_pos) 217 | 218 | quaternion = (pose[3], pose[4], pose[5], pose[6]) 219 | 220 | euler = euler_from_quaternion(quaternion) 221 | frame = PyKDL.Frame(PyKDL.Rotation.RPY(euler[0], euler[1], euler[2]), 222 | PyKDL.Vector(pose[0], pose[1], pose[2])) 223 | return frame 224 | 225 | def get_config(self, config='home'): 226 | assert config in self._default_configs, 'Invalid default configuration' 227 | return self._default_configs[config] 228 | 229 | def add_callback(self, topic_name, function_handle): 230 | if topic_name not in self._subTopics: 231 | print 'ArmInterface - Invalid topic name' 232 | return 233 | 234 | if topic_name not in self._callbacks: 235 | self._callbacks[topic_name] = [] 236 | 237 | self._callbacks[topic_name].append(function_handle) 238 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/src/uuv_manipulator_nodes/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 The UUV Simulator Authors. 2 | # All rights reserved. 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 | from .endpoint_state import EndPointStateNode 17 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/src/uuv_manipulator_nodes/endpoint_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import sys 18 | import argparse 19 | import rospy, os 20 | from manipulator_kinematics import ManipulatorKinematics 21 | from uuv_manipulators_msgs.msg import EndPointState 22 | from geometry_msgs.msg import Vector3 23 | import numpy as np 24 | 25 | 26 | class EndPointStateNode: 27 | def __init__(self, namespace): 28 | # Storing the manipulator's namespace 29 | self._namespace = namespace 30 | if self._namespace[0] != '/': 31 | self._namespace = '/' + self._namespace 32 | 33 | # Calling the interface to the kinematics KDL interface 34 | self._kinematics = ManipulatorKinematics(self._namespace) 35 | 36 | # Creating a publisher for the end point state 37 | self._pub = rospy.Publisher(self._namespace + '/endpoint_state', EndPointState, queue_size=1) 38 | rate = rospy.Rate(500) 39 | try: 40 | while not rospy.is_shutdown(): 41 | self._update() 42 | rate.sleep() 43 | except rospy.exceptions.ROSInterruptException: 44 | pass 45 | 46 | def _update(self): 47 | state_msg = EndPointState 48 | # Read joint states from the kinematics interface 49 | q = self._kinematics.joint_angles.values() 50 | qd = self._kinematics.joint_velocities.values() 51 | eff = self._kinematics.joint_efforts.values() 52 | # Storing the pose 53 | pose = self._kinematics.forward_position_kinematics(q) 54 | state_msg.pose.position.x = pose[0] 55 | state_msg.pose.position.y = pose[1] 56 | state_msg.pose.position.z = pose[2] 57 | 58 | state_msg.pose.orientation.x = pose[3] 59 | state_msg.pose.orientation.y = pose[4] 60 | state_msg.pose.orientation.z = pose[5] 61 | state_msg.pose.orientation.w = pose[6] 62 | # Storing velocity 63 | vel = self._kinematics.forward_velocity_kinematics(qd) 64 | state_msg.twist.linear = Vector3(*vel.vel) 65 | state_msg.twist.angular = Vector3(*vel.rot) 66 | # Calculating wrench 67 | wrench = np.dot(self._kinematics.jacobian(q), np.array(eff)) 68 | self._pub.publish(state_msg) 69 | 70 | if __name__ == '__main__': 71 | # Create argument parser 72 | parser = argparse.ArgumentParser(description='End point state publisher') 73 | parser.add_argument('--namespace', metavar='NS', type=str) 74 | 75 | # Filter out the garbage 76 | arguments = [a for a in sys.argv if ':=' not in a and '.py' not in a] 77 | args = parser.parse_args(arguments) 78 | 79 | if not args.namespace: 80 | raise rospy.ROSException('Namespace was not given') 81 | 82 | namespace = args.namespace 83 | 84 | node_name = 'endpoint_state_publisher' 85 | rospy.init_node(node_name) 86 | node = EndPointStateNode(namespace) 87 | rospy.spin() 88 | rospy.loginfo('Shutting down, node=' + node_name) 89 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/test/test_arm_interface.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Copyright (c) 2016 The UUV Simulator Authors. 3 | # All rights reserved. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | from __future__ import print_function 17 | PKG = 'uuv_manipulators_kinematics' 18 | import roslib; roslib.load_manifest(PKG) 19 | 20 | import sys 21 | import unittest 22 | import numpy as np 23 | from uuv_manipulator_interfaces import ArmInterface 24 | 25 | 26 | class TestArmInterface(unittest.TestCase): 27 | def test_init_interface(self): 28 | arm = ArmInterface() 29 | # Test if the namespace and arm name are correct 30 | self.assertEquals(arm.namespace, '/rexrov/', 'Invalid robot namespace') 31 | self.assertEquals(arm.arm_name, 'oberon7', 'Invalid arm name') 32 | self.assertEquals(arm.base_link, 'oberon7/base', 'Invalid manipulator base name') 33 | self.assertEquals(arm.tip_link, 'oberon7/end_effector', 'Invalid end-effector link name') 34 | self.assertNotEquals(len(arm.joint_names), 0, 'The list of joint names is empty') 35 | self.assertEquals(arm.n_links, 6, 'Invalid number of links, n_links=' + str(arm.n_links)) 36 | 37 | for name in arm.joint_names: 38 | self.assertIn(name, arm.joint_angles, 'Joint name %s not listed in the joint positions dictionary' % name) 39 | self.assertIn(name, arm.joint_velocities, 'Joint name %s not listed in the joint velocities dictionary' % name) 40 | self.assertIn(name, arm.joint_efforts, 'Joint name %s not listed in the joint efforts dictionary' % name) 41 | 42 | def test_joints_to_kdl(self): 43 | arm = ArmInterface() 44 | for idx, name in zip(range(len(arm.joint_names)), arm.joint_names): 45 | for t in ['positions', 'torques']: 46 | jnt_array = arm.joints_to_kdl(t, last_joint=name) 47 | self.assertEquals(jnt_array.rows(), idx + 1, 48 | 'Invalid number of joints, joint_idx=%d, last_joint=%s, n_joints=%d' % (idx, name, jnt_array.rows())) 49 | 50 | def test_jacobian(self): 51 | arm = ArmInterface() 52 | jac = arm.jacobian() 53 | self.assertIsNotNone(jac, 'Jacobian matrix is invalid') 54 | self.assertEquals(jac.shape, (arm.n_links, 6), 'The full Jacobian matrix has the wrong size') 55 | 56 | for idx, name in zip(range(len(arm.link_names)), arm.link_names): 57 | self.assertEquals(arm.jacobian(end_link=name).shape, (arm.n_links, 6)) 58 | self.assertEquals(arm.jacobian_transpose(end_link=name).shape, (6, arm.n_links)) 59 | 60 | def test_home_config(self): 61 | arm = ArmInterface() 62 | self.assertIsNotNone(arm.home, 'Home configuration is invalid') 63 | 64 | 65 | if __name__ == '__main__': 66 | import rosunit 67 | rosunit.unitrun(PKG, 'test_arm_interface', TestArmInterface) 68 | -------------------------------------------------------------------------------- /uuv_manipulators_kinematics/test/test_kinematic_interfaces.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package uuv_manipulators_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.6.1 (2018-08-05) 6 | ------------------ 7 | 8 | 0.6.0 (2018-07-31) 9 | ------------------ 10 | * MV uuv_manipulators package transfered from uuv_simulator 11 | Signed-off-by: Musa Morena Marcusso Manhaes 12 | * Contributors: Musa Morena Marcusso Manhaes 13 | 14 | 0.5.13 (2018-07-24) 15 | ------------------- 16 | 17 | 0.5.12 (2018-07-23) 18 | ------------------- 19 | 20 | 0.5.11 (2018-07-21) 21 | ------------------- 22 | 23 | 0.5.10 (2018-07-10) 24 | ------------------- 25 | 26 | 0.5.9 (2018-07-09) 27 | ------------------ 28 | 29 | 0.5.8 (2018-07-07) 30 | ------------------ 31 | 32 | 0.5.7 (2018-07-06) 33 | ------------------ 34 | 35 | 0.5.6 (2018-07-06) 36 | ------------------ 37 | 38 | 0.5.5 (2018-07-05) 39 | ------------------ 40 | * RM Merge messages from the change log 41 | Signed-off-by: Musa Morena Marcusso Manhaes 42 | * UPDATE Catkin packages format to 2 43 | Signed-off-by: Musa Morena Marcusso Manhaes 44 | * Contributors: Musa Morena Marcusso Manhaes 45 | 46 | 0.5.4 (2018-07-04) 47 | ------------------ 48 | * UPDATE Catkin packages format to 2 49 | Signed-off-by: Musa Morena Marcusso Manhaes 50 | * Contributors: Musa Morena Marcusso Manhaes 51 | 52 | 0.5.3 (2018-07-04) 53 | ------------------ 54 | * ADD CHANGELOG files 55 | Signed-off-by: Musa Morena Marcusso Manhaes 56 | * Contributors: Musa Morena Marcusso Manhaes 57 | 58 | 0.5.1 (2018-07-03) 59 | ------------------ 60 | * CHANGE Bump version to 0.5.2 61 | Signed-off-by: Musa Morena Marcusso Manhaes 62 | * CHANGE Version 63 | * CHANGE Package versions 64 | Signed-off-by: Musa Morena Marcusso Manhaes 65 | * FIX Typos and package version 66 | Signed-off-by: Musa Morena Marcusso Manhaes 67 | * initial commit 68 | Signed-off-by: Sebastian Scherer (CR/AEI) 69 | * Contributors: Musa Morena Marcusso Manhaes, Sebastian Scherer (CR/AEI) 70 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uuv_manipulators_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | geometry_msgs 7 | sensor_msgs 8 | std_msgs 9 | ) 10 | 11 | add_message_files(DIRECTORY msg FILES 12 | EndPointState.msg 13 | EndeffectorCommand.msg 14 | EndeffectorState.msg 15 | ArmConfigCommand.msg 16 | ) 17 | 18 | add_service_files(DIRECTORY srv FILES 19 | SolveIK.srv 20 | ) 21 | 22 | generate_messages(DEPENDENCIES 23 | geometry_msgs 24 | sensor_msgs 25 | std_msgs 26 | ) 27 | 28 | catkin_package(CATKIN_DEPENDS 29 | message_runtime 30 | geometry_msgs 31 | sensor_msgs 32 | std_msgs 33 | ) 34 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/msg/ArmConfigCommand.msg: -------------------------------------------------------------------------------- 1 | # Commands to drive the arm to default configuration 2 | string command # Operation tag 3 | # Default commands 4 | string HOME = home 5 | # Place for arguments, if needed 6 | string args 7 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/msg/EndPointState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist twist 4 | geometry_msgs/Wrench wrench 5 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/msg/EndeffectorCommand.msg: -------------------------------------------------------------------------------- 1 | # Commands to the end-effector 2 | string command # Operation tag 3 | # Default commands 4 | string EE_MOVE = move 5 | string EE_STOP = stop 6 | # Place for arguments, if needed 7 | float64 ratio 8 | # Default ratios of aperture 9 | float64 EE_CLOSED = 0.0 10 | float64 EE_OPEN = 100.0 11 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/msg/EndeffectorState.msg: -------------------------------------------------------------------------------- 1 | # States of the end-effector 2 | time stamp 3 | string state 4 | # Default states 5 | string MOVING = moving 6 | string DISABLED = disabled 7 | string READY = ready 8 | # Current state 9 | float64 position 10 | float64 effort 11 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uuv_manipulators_msgs 4 | 0.6.1 5 | Definitions of messages and services for the manipulator control package. 6 | 7 | mam0box 8 | Apache-2.0 9 | 10 | catkin 11 | 12 | message_generation 13 | 14 | geometry_msgs 15 | sensor_msgs 16 | std_msgs 17 | 18 | message_runtime 19 | 20 | -------------------------------------------------------------------------------- /uuv_manipulators_msgs/srv/SolveIK.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose pose 2 | sensor_msgs/JointState seed_angles 3 | --- 4 | sensor_msgs/JointState joints 5 | bool isValid 6 | --------------------------------------------------------------------------------