├── .gitignore ├── CHANGELOG.rst ├── LICENSE ├── README.rst ├── baxter_gazebo ├── CMakeLists.txt ├── launch │ └── baxter_world.launch ├── package.xml ├── src │ └── baxter_gazebo_ros_control_plugin.cpp └── worlds │ ├── baxter.world │ └── baxter_gas_station.world ├── baxter_sim_controllers ├── .gitignore ├── CMakeLists.txt ├── config │ └── baxter_sim_controllers_plugins.xml ├── include │ └── baxter_sim_controllers │ │ ├── baxter_effort_controller.h │ │ ├── baxter_gripper_controller.h │ │ ├── baxter_head_controller.h │ │ ├── baxter_position_controller.h │ │ └── baxter_velocity_controller.h ├── package.xml └── src │ ├── baxter_effort_controller.cpp │ ├── baxter_gripper_controller.cpp │ ├── baxter_head_controller.cpp │ ├── baxter_position_controller.cpp │ └── baxter_velocity_controller.cpp ├── baxter_sim_examples ├── CMakeLists.txt ├── launch │ └── baxter_pick_and_place_demo.launch ├── models │ ├── block │ │ └── model.urdf │ └── cafe_table │ │ ├── materials │ │ └── textures │ │ │ ├── Maple.jpg │ │ │ └── Wood_Floor_Dark.jpg │ │ ├── meshes │ │ └── cafe_table.dae │ │ ├── model.config │ │ └── model.sdf ├── package.xml └── scripts │ └── ik_pick_and_place_demo.py ├── baxter_sim_hardware ├── CMakeLists.txt ├── config │ ├── baxter_left_electric_gripper_controller.yaml │ ├── baxter_right_electric_gripper_controller.yaml │ └── baxter_sim_controllers.yaml ├── images │ └── researchsdk.png ├── include │ └── baxter_sim_hardware │ │ └── baxter_emulator.h ├── launch │ ├── baxter_sdk_control.launch │ ├── baxter_sdk_position_rqt.launch │ ├── baxter_sdk_position_rqt.perspective │ ├── baxter_sdk_velocity_rqt.launch │ └── baxter_sdk_velocity_rqt.perspective ├── package.xml └── src │ └── baxter_emulator.cpp ├── baxter_sim_io ├── CMakeLists.txt ├── include │ └── baxter_sim_io │ │ ├── baxter_io.hpp │ │ └── qnode.hpp ├── package.xml ├── resources │ ├── arm.png │ ├── cuff.png │ ├── cuff_grasp_p.png │ ├── cuff_grasp_r.png │ ├── cuff_ok_p.png │ ├── cuff_ok_r.png │ ├── cuff_squeeze_p.png │ ├── cuff_squeeze_r.png │ ├── left.png │ ├── logo.png │ ├── nav.png │ ├── nav_cancel_p.png │ ├── nav_cancel_r.png │ ├── nav_ok_p.png │ ├── nav_ok_r.png │ ├── nav_show_p.png │ ├── nav_show_r.png │ ├── right.png │ ├── robot_arm.png │ ├── sim_io.qrc │ └── torso.png ├── src │ ├── baxter_io.cpp │ ├── main.cpp │ └── qnode.cpp └── ui │ └── baxter_io.ui ├── baxter_sim_kinematics ├── CMakeLists.txt ├── include │ └── baxter_sim_kinematics │ │ ├── arm_kinematics.h │ │ └── position_kinematics.h ├── launch │ └── baxter_sim_kinematics.launch ├── package.xml └── src │ ├── arm_kinematics.cpp │ └── position_kinematics.cpp ├── baxter_simulator.rosinstall └── baxter_simulator ├── CMakeLists.txt └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | 3 | # C extensions 4 | *.so 5 | 6 | # Packages 7 | *.egg 8 | *.egg-info 9 | dist 10 | build 11 | eggs 12 | parts 13 | bin 14 | var 15 | sdist 16 | develop-eggs 17 | .installed.cfg 18 | lib 19 | lib64 20 | 21 | # Installer logs 22 | pip-log.txt 23 | 24 | # Unit test / coverage reports 25 | .coverage 26 | .tox 27 | nosetests.xml 28 | 29 | # Translations 30 | *.mo 31 | 32 | # Mr Developer 33 | .mr.developer.cfg 34 | .project 35 | .cproject 36 | cmake_install.cmake 37 | .pydevproject 38 | *.swp 39 | CATKIN_IGNORE 40 | catkin 41 | catkin_generated 42 | devel 43 | 44 | cpp 45 | docs 46 | msg_gen 47 | srv_gen 48 | *.smoketest 49 | *.cfgc 50 | *_msgs/src/ 51 | */src/**/cfg 52 | */*/src/**/* 53 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 1.2.12 (2016-1-11) 2 | --------------------------------- 3 | - This update fixes the conditional yaml-cpp preprocessor define 4 | - Fixed deprecation warning in baxter_sim_kinematics Transforms functions 5 | 6 | 1.2.11 (2016-1-10) 7 | --------------------------------- 8 | - This update is to only compile yaml-cpp functions with preprocessor define 9 | if the detected system version is yaml-cpp 0.5 or greater. This prevents grippers 10 | from working in EOL Ubuntu Saucy, but allows the ros_buildfarm to compile. 11 | 12 | 1.2.1 (2016-1-6) 13 | --------------------------------- 14 | - This update is to fix the Isolated Install builds required by the ROS Buildfarm 15 | - Added yaml-cpp as a dependency for baxter_sim_controllers 16 | - Properly exported the include directory of baxter_sim_kinematics 17 | so that baxter_sim_hardware could utilize its header files 18 | - Properly added install targets for executables in baxter_sim_kinematics 19 | and baxter_sim_hardware 20 | - Updated headers to properly use / include OpenCV2 dependencies 21 | 22 | 1.2.0 (2015-12-21) 23 | --------------------------------- 24 | - Added a timestamp to the endpoint state topic (thanks @jarvisshultz for contributing) 25 | - Added Signal Handlers to catch Ctrl+C in baxter_sim_io nodes 26 | - General baxter_sim_io code cleanup 27 | - Added run depends for baxter_simulator meta package for all packages in this repo 28 | - Fixed invalid world -> base transform quaternion (thanks @JohnPeterson for contributing) 29 | - Changed Navigator topics to be in sync with baxter_core_msgs 30 | - Added Electric Gripper controllers and emulation (thanks @k-okada for contributing the first pass) 31 | - Improved the fidelity and accuracy of movement by re-tuning the PID loops for the joints (thanks @davetcoleman for the hints) 32 | - Electric Grippers are optional, and can be enabled by setting the top level _electric_gripper for baxter_world.launch 33 | - Added the Simulated IK Pick and Place demo to demonstrate grippers and spawning Gazebo Objects 34 | - Made robot_description an optional arg for baxter_world, in case you have already spawned a custom robot_description 35 | - Fixed an issue in the Position Kinematics node that would cause it to fail if a joint with in its name was added to /robot/joint_states 36 | 37 | 0.9.0 (2015-1-2) 38 | --------------------------------- 39 | - Updates baxter_simulator to ROS Indigo (thanks @rethink-hmalaichamee) 40 | - Moves some include files to better support catkin_tools builds (thanks @davetcoleman for contributing) 41 | - Changes rosinstall file to use HTTPS rather than SSH for wstool source management for baxter_simulator 42 | 43 | 0.8.1 (2014-12-4) 44 | --------------------------------- 45 | - Fixes simulated inverse kinematics service to compute IK from _gripper rather than _wrist 46 | - Simplifies forward kinematics function to compute FK for just the endpoint rather than every joint 47 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013-2015, Rethink Robotics 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | 3. Neither the name of the Rethink Robotics nor the names of its 13 | contributors may be used to endorse or promote products derived from 14 | this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | baxter_simulator 2 | ============== 3 | 4 | Gazebo simulation with emulated interfaces for the Baxter Research Robot 5 | 6 | Code & Tickets 7 | -------------- 8 | 9 | +-----------------+----------------------------------------------------------------+ 10 | | Documentation | http://sdk.rethinkrobotics.com/wiki | 11 | +-----------------+----------------------------------------------------------------+ 12 | | Issues | https://github.com/RethinkRobotics/baxter_simulator/issues | 13 | +-----------------+----------------------------------------------------------------+ 14 | | Contributions | http://sdk.rethinkrobotics.com/wiki/Contributions | 15 | +-----------------+----------------------------------------------------------------+ 16 | 17 | baxter_simulator Repository Overview 18 | ------------------------------------ 19 | 20 | :: 21 | 22 | . 23 | | 24 | +-- baxter_simulator/ baxter_simulator metapackage 25 | | 26 | +-- baxter_gazebo/ Gazebo interface for the Baxter that loads the models into simulation 27 | | +-- src/ 28 | | +-- launch/ 29 | | +-- worlds/ 30 | | 31 | +-- baxter_sim_controllers/ Controller plugins for Baxter 32 | | +-- src/ 33 | | +-- include/ 34 | | +-- config/ 35 | | 36 | +-- baxter_sim_examples/ Examples specific to Baxter in Simulation 37 | | +-- scripts/ (use baxter_examples for examples that will work both in 38 | | +-- include/ simulation AND the real Baxter robot) 39 | | +-- launch/ 40 | | +-- models/ 41 | | 42 | +-- baxter_sim_hardware/ This emulates the hardware interfaces of Baxter 43 | | +-- src/ 44 | | +-- include/ 45 | | +-- config/ 46 | | +-- launch/ 47 | | 48 | +-- baxter_sim_io/ QT based navigator plugins for baxter 49 | | +-- src/ 50 | | +-- include/ 51 | | +-- ui/ 52 | | 53 | +-- baxter_sim_kinematics/ Implementation of IK, FK and gravity compensation for baxter 54 | | +-- src/ 55 | | +-- include/ 56 | | +-- launch/ 57 | 58 | 59 | 60 | Other Baxter Repositories 61 | ------------------------- 62 | 63 | +------------------+-----------------------------------------------------+ 64 | | baxter | https://github.com/RethinkRobotics/baxter | 65 | +------------------+-----------------------------------------------------+ 66 | | baxter_interface | https://github.com/RethinkRobotics/baxter_interface | 67 | +------------------+-----------------------------------------------------+ 68 | | baxter_tools | https://github.com/RethinkRobotics/baxter_tools | 69 | +------------------+-----------------------------------------------------+ 70 | | baxter_examples | https://github.com/RethinkRobotics/baxter_examples | 71 | +------------------+-----------------------------------------------------+ 72 | | baxter_common | https://github.com/RethinkRobotics/baxter_common | 73 | +------------------+-----------------------------------------------------+ 74 | 75 | Latest Release Information 76 | -------------------------- 77 | 78 | http://sdk.rethinkrobotics.com/wiki/Release-Changes 79 | -------------------------------------------------------------------------------- /baxter_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(baxter_gazebo) 3 | 4 | find_package(catkin 5 | REQUIRED COMPONENTS 6 | roscpp 7 | gazebo_ros 8 | gazebo_ros_control 9 | controller_manager 10 | baxter_core_msgs 11 | ) 12 | 13 | find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | find_package(gazebo REQUIRED) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS 19 | baxter_core_msgs 20 | gazebo_ros_control 21 | roscpp 22 | controller_manager_msgs 23 | DEPENDS 24 | gazebo 25 | ) 26 | 27 | link_directories( 28 | ${GAZEBO_LIBRARY_DIRS} 29 | ${catkin_LIBRARY_DIRS} 30 | ) 31 | 32 | include_directories(include 33 | ${Boost_INCLUDE_DIR} 34 | ${catkin_INCLUDE_DIRS} 35 | ${GAZEBO_INCLUDE_DIRS} 36 | ) 37 | 38 | add_library(baxter_gazebo_ros_control 39 | src/baxter_gazebo_ros_control_plugin.cpp 40 | ) 41 | 42 | target_link_libraries(baxter_gazebo_ros_control 43 | ${catkin_LIBRARIES} 44 | ${GAZEBO_LIBRARIES} 45 | gazebo_ros_control 46 | ) 47 | 48 | add_dependencies( 49 | baxter_gazebo_ros_control 50 | ${catkin_EXPORTED_TARGETS} 51 | ) 52 | 53 | install( 54 | TARGETS baxter_gazebo_ros_control 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | ) 57 | 58 | install( 59 | DIRECTORY launch worlds 60 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 61 | ) 62 | -------------------------------------------------------------------------------- /baxter_gazebo/launch/baxter_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /baxter_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_gazebo 4 | 1.2.12 5 | 6 | Baxter Gazebo plugins and launch files 7 | 8 | 9 | 10 | Rethink Robotics Inc. 11 | 12 | BSD 13 | http://sdk.rethinkrobotics.com 14 | 15 | https://github.com/RethinkRobotics/baxter_simulator 16 | 17 | 18 | https://github.com/RethinkRobotics/baxter_simulator/issues 19 | 20 | Rethink Robotics Inc. 21 | Dave Coleman 22 | 23 | catkin 24 | 25 | roscpp 26 | baxter_core_msgs 27 | gazebo_ros_control 28 | controller_manager_msgs 29 | 30 | roscpp 31 | tf2_ros 32 | baxter_core_msgs 33 | gazebo_ros_control 34 | controller_manager_msgs 35 | baxter_sim_hardware 36 | gazebo_ros 37 | baxter_description 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /baxter_gazebo/worlds/baxter.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | 1000.0 13 | 14 | 0.0 0.0 -9.81 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /baxter_gazebo/worlds/baxter_gas_station.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | 9 | model://sun 10 | 11 | 12 | 13 | model://gas_station 14 | gas_station 15 | -2.0 7.0 0 0 0 0 16 | 17 | 18 | 1000.0 19 | 20 | 0.0 0.0 -9.81 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /baxter_sim_controllers/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | lib -------------------------------------------------------------------------------- /baxter_sim_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(baxter_sim_controllers) 3 | 4 | find_package(catkin 5 | REQUIRED COMPONENTS 6 | roscpp 7 | controller_interface 8 | control_toolbox 9 | effort_controllers 10 | baxter_core_msgs 11 | ) 12 | find_package(Boost REQUIRED COMPONENTS system) 13 | find_package(PkgConfig REQUIRED) 14 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 15 | find_path(YAML_CPP_INCLUDE_DIR 16 | NAMES yaml_cpp.h 17 | PATHS ${YAML_CPP_INCLUDE_DIRS}) 18 | find_library(YAML_CPP_LIBRARY 19 | NAMES YAML_CPP 20 | PATHS ${YAML_CPP_LIBRARY_DIRS}) 21 | if(${YAML_CPP_VERSION} VERSION_LESS 0.5) 22 | add_definitions(-DDEPRECATED_YAML_CPP_VERSION) 23 | endif() 24 | 25 | catkin_package( 26 | CATKIN_DEPENDS 27 | controller_interface 28 | control_toolbox 29 | effort_controllers 30 | baxter_core_msgs 31 | DEPENDS yaml-cpp 32 | INCLUDE_DIRS include 33 | LIBRARIES ${PROJECT_NAME} 34 | ) 35 | 36 | include_directories(include 37 | ${catkin_INCLUDE_DIRS} 38 | ${Boost_INCLUDE_DIR} 39 | ) 40 | 41 | add_library(${PROJECT_NAME} 42 | src/baxter_position_controller.cpp 43 | src/baxter_velocity_controller.cpp 44 | src/baxter_effort_controller.cpp 45 | src/baxter_head_controller.cpp 46 | src/baxter_gripper_controller.cpp 47 | ) 48 | 49 | target_link_libraries(${PROJECT_NAME} 50 | ${catkin_LIBRARIES} 51 | ${Boost_LIBRARIES} 52 | ${YAML_CPP_LIBRARIES} 53 | ) 54 | 55 | add_dependencies(${PROJECT_NAME} 56 | ${catkin_EXPORTED_TARGETS} 57 | baxter_core_msgs_gencpp 58 | ) 59 | 60 | install( 61 | TARGETS ${PROJECT_NAME} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | ) 64 | 65 | install( 66 | DIRECTORY config include 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 68 | ) 69 | -------------------------------------------------------------------------------- /baxter_sim_controllers/config/baxter_sim_controllers_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The BaxterPositionController tracks position commands for the entire Baxter arm. It expects a EffortJointInterface type of hardware interface. 8 | 9 | 10 | 11 | 14 | 15 | The BaxterVelocityController tracks velocity commands for the entire Baxter arm. It expects a EffortJointInterface type of hardware interface. 16 | 17 | 18 | 19 | 22 | 23 | The BaxterEffortController tracks effort commands for the Baxter joints. It expects a EffortJointInterface type of hardware interface. 24 | 25 | 26 | 27 | 30 | 31 | The BaxterHeadController tracks position commands for the Baxter head. It expects a EffortJointInterface type of hardware interface. 32 | 33 | 34 | 35 | 38 | 39 | The BaxterGripperController tracks gripper commands for the Baxter end effector. It expects a EffortJointInterface type of hardware interface. 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /baxter_sim_controllers/include/baxter_sim_controllers/baxter_effort_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Multiple joint effort controller for Baxter SDK 33 | */ 34 | 35 | #ifndef BAXTER_EFFORT_CONTROLLER_H 36 | #define BAXTER_EFFORT_CONTROLLER_H 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include // the input command 49 | 50 | #include // used for controlling individual joints 51 | 52 | namespace baxter_sim_controllers { 53 | 54 | class BaxterEffortController : public controller_interface::Controller< 55 | hardware_interface::EffortJointInterface> { 56 | 57 | public: 58 | BaxterEffortController(); 59 | ~BaxterEffortController(); 60 | 61 | bool init(hardware_interface::EffortJointInterface *robot, 62 | ros::NodeHandle &n); 63 | void starting(const ros::Time& time); 64 | void stopping(const ros::Time& time); 65 | void update(const ros::Time& time, const ros::Duration& period); 66 | void updateCommands(); 67 | 68 | private: 69 | ros::NodeHandle nh_; 70 | size_t n_joints_; 71 | std::string topic_name; 72 | std_msgs::Float64 m; 73 | std::map joint_to_index_map_; // allows incoming messages to be quickly ordered 74 | bool new_command_; // true when an unproccessed new command is in the realtime buffer 75 | 76 | // Command subscriber 77 | ros::Subscriber effort_command_sub_; 78 | 79 | // Command publisher 80 | ros::Publisher effort_command_pub_[10]; 81 | 82 | /** 83 | * @brief Callback from a recieved goal from the published topic message 84 | * @param msg trajectory goal 85 | */ 86 | void commandCB(const baxter_core_msgs::JointCommandConstPtr& msg); 87 | 88 | // Create an effort-based joint effort controller for every joint 89 | std::vector > effort_controllers_; 90 | 91 | }; 92 | 93 | } // namespace 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /baxter_sim_controllers/include/baxter_sim_controllers/baxter_gripper_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2014, Kei Okada 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | 31 | #ifndef BAXTER_GRIPPER_CONTROLLER_H_ 32 | #define BAXTER_GRIPPER_CONTROLLER_H_ 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include // used for controlling individual joints 46 | 47 | 48 | namespace baxter_sim_controllers 49 | { 50 | class BaxterGripperController: public controller_interface::Controller 51 | { 52 | 53 | public: 54 | BaxterGripperController(); 55 | ~BaxterGripperController(); 56 | 57 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) ; 58 | void starting(const ros::Time& time) ; 59 | void stopping(const ros::Time& time) ; 60 | void update(const ros::Time& time, const ros::Duration& period) ; 61 | void updateCommands() ; 62 | 63 | private: 64 | ros::NodeHandle nh_; 65 | 66 | /**< Last commanded position. */ 67 | realtime_tools::RealtimeBuffer gripper_command_buffer; 68 | 69 | size_t n_joints; 70 | std::string topic_name; 71 | // Indices for mimic and orignal joints 72 | int mimic_idx_; 73 | int main_idx_; 74 | 75 | std::map joint_to_index_map; // allows incoming messages to be quickly ordered 76 | 77 | bool new_command; // true when an unproccessed new command is in the realtime buffer 78 | size_t update_counter; 79 | 80 | // Command subscriber 81 | ros::Subscriber gripper_command_sub; 82 | 83 | /** 84 | * @brief Callback from a recieved goal from the published topic message 85 | * @param msg trajectory goal 86 | */ 87 | void commandCB(const baxter_core_msgs::EndEffectorCommandConstPtr& msg); 88 | 89 | // Create an effort-based joint position controller for every joint 90 | std::vector< 91 | boost::shared_ptr< 92 | effort_controllers::JointPositionController> > gripper_controllers; 93 | }; 94 | 95 | } // namespace 96 | 97 | #endif /* BAXTER_GRIPPER_CONTROLLER_H_ */ 98 | -------------------------------------------------------------------------------- /baxter_sim_controllers/include/baxter_sim_controllers/baxter_head_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | 31 | #ifndef BAXTER_HEAD_CONTROLLER_H_ 32 | #define BAXTER_HEAD_CONTROLLER_H_ 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | //#include // the input command 44 | #include 45 | #include // used for controlling individual joints 46 | 47 | 48 | namespace baxter_sim_controllers 49 | { 50 | 51 | class BaxterHeadController: public controller_interface::Controller 52 | { 53 | 54 | public: 55 | BaxterHeadController(); 56 | ~BaxterHeadController(); 57 | 58 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 59 | void starting(const ros::Time& time); 60 | void stopping(const ros::Time& time); 61 | void update(const ros::Time& time, const ros::Duration& period); 62 | void updateCommands(); 63 | 64 | private: 65 | ros::NodeHandle nh_; 66 | 67 | /**< Last commanded position. */ 68 | realtime_tools::RealtimeBuffer head_command_buffer; 69 | 70 | size_t n_joints; 71 | std::string topic_name; 72 | 73 | std::map joint_to_index_map; // allows incoming messages to be quickly ordered 74 | 75 | bool new_command; // true when an unproccessed new command is in the realtime buffer 76 | size_t update_counter; 77 | 78 | // Command subscriber 79 | ros::Subscriber head_command_sub; 80 | 81 | /** 82 | * @brief Callback from a recieved goal from the published topic message 83 | * @param msg trajectory goal 84 | */ 85 | void commandCB(const baxter_core_msgs::HeadPanCommandConstPtr& msg); 86 | 87 | // Create an effort-based joint position controller for every joint 88 | std::vector< 89 | boost::shared_ptr< 90 | effort_controllers::JointPositionController> > head_controllers; 91 | 92 | }; 93 | 94 | } // namespace 95 | 96 | #endif /* BAXTER_HEAD_CONTROLLER_H_ */ 97 | -------------------------------------------------------------------------------- /baxter_sim_controllers/include/baxter_sim_controllers/baxter_position_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, Open Source Robotics Foundation 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Open Source Robotics Foundation 18 | * nor the names of its contributors may be 19 | * used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /** 37 | * \author Dave Coleman 38 | * \desc Multiple joint position controller for Baxter SDK 39 | */ 40 | 41 | #ifndef BAXTER_POSITION_CONTROLLER_H 42 | #define BAXTER_POSITION_CONTROLLER_H 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include // the input command 55 | 56 | #include // used for controlling individual joints 57 | 58 | 59 | namespace baxter_sim_controllers 60 | { 61 | 62 | class BaxterPositionController: public controller_interface::Controller 63 | { 64 | 65 | public: 66 | BaxterPositionController(); 67 | ~BaxterPositionController(); 68 | 69 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 70 | void starting(const ros::Time& time); 71 | void stopping(const ros::Time& time); 72 | void update(const ros::Time& time, const ros::Duration& period); 73 | void updateCommands(); 74 | 75 | private: 76 | ros::NodeHandle nh_; 77 | 78 | /**< Last commanded position. */ 79 | realtime_tools::RealtimeBuffer position_command_buffer_; 80 | 81 | size_t n_joints_; 82 | std::string topic_name; 83 | 84 | std::map joint_to_index_map_; // allows incoming messages to be quickly ordered 85 | 86 | bool verbose_; 87 | bool new_command_; // true when an unproccessed new command is in the realtime buffer 88 | size_t update_counter_; 89 | 90 | // Command subscriber 91 | ros::Subscriber position_command_sub_; 92 | 93 | /** 94 | * @brief Callback from a recieved goal from the published topic message 95 | * @param msg trajectory goal 96 | */ 97 | void commandCB(const baxter_core_msgs::JointCommandConstPtr& msg); 98 | 99 | // Create an effort-based joint position controller for every joint 100 | std::vector< 101 | boost::shared_ptr< 102 | effort_controllers::JointPositionController> > position_controllers_; 103 | 104 | }; 105 | 106 | } // namespace 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /baxter_sim_controllers/include/baxter_sim_controllers/baxter_velocity_controller.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, Open Source Robotics Foundation 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Open Source Robotics Foundation 18 | * nor the names of its contributors may be 19 | * used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /** 37 | * \author Dave Coleman 38 | * \desc Multiple joint velocity controller for Baxter SDK 39 | */ 40 | 41 | #ifndef BAXTER_VELOCITY_CONTROLLER_H 42 | #define BAXTER_VELOCITY_CONTROLLER_H 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include // the input command 55 | 56 | #include // used for controlling individual joints 57 | 58 | 59 | namespace baxter_sim_controllers 60 | { 61 | 62 | class BaxterVelocityController: public controller_interface::Controller 63 | { 64 | 65 | public: 66 | BaxterVelocityController(); 67 | ~BaxterVelocityController(); 68 | 69 | bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); 70 | void starting(const ros::Time& time); 71 | void stopping(const ros::Time& time); 72 | void update(const ros::Time& time, const ros::Duration& period); 73 | void updateCommands(); 74 | 75 | private: 76 | ros::NodeHandle nh_; 77 | 78 | // Last commanded velocity 79 | realtime_tools::RealtimeBuffer velocity_command_buffer_; 80 | 81 | size_t n_joints_; 82 | std::string topic_name; 83 | 84 | std::map joint_to_index_map_; // allows incoming messages to be quickly ordered 85 | 86 | bool verbose_; 87 | bool new_command_; // true when an unproccessed new command is in the realtime buffer 88 | size_t update_counter_; 89 | 90 | // Command subscriber 91 | ros::Subscriber velocity_command_sub_; 92 | 93 | /** 94 | * @brief Callback from a recieved goal from the published topic message 95 | * @param msg trajectory goal 96 | */ 97 | void commandCB(const baxter_core_msgs::JointCommandConstPtr& msg); 98 | 99 | // Create an effort-based joint velocity controller for every joint 100 | std::vector< 101 | boost::shared_ptr< 102 | effort_controllers::JointVelocityController> > velocity_controllers_; 103 | 104 | }; 105 | 106 | } // namespace 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /baxter_sim_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_sim_controllers 4 | 1.2.12 5 | 6 | Baxter specific controllers for Gazebo use 7 | 8 | 9 | 10 | Rethink Robotics Inc. 11 | 12 | BSD 13 | http://sdk.rethinkrobotics.com 14 | 15 | https://github.com/RethinkRobotics/baxter_simulator 16 | 17 | 18 | https://github.com/RethinkRobotics/baxter_simulator/issues 19 | 20 | Rethink Robotics Inc. 21 | Dave Coleman 22 | 23 | catkin 24 | 25 | controller_interface 26 | control_toolbox 27 | effort_controllers 28 | baxter_core_msgs 29 | yaml-cpp 30 | 31 | controller_interface 32 | control_toolbox 33 | effort_controllers 34 | baxter_core_msgs 35 | yaml-cpp 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /baxter_sim_controllers/src/baxter_effort_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Multiple joint effort controller for Baxter SDK 33 | */ 34 | 35 | #include 36 | #include 37 | 38 | 39 | namespace baxter_sim_controllers { 40 | 41 | BaxterEffortController::BaxterEffortController() 42 | : new_command_(true) 43 | //update_counter_(0) 44 | {} 45 | 46 | BaxterEffortController::~BaxterEffortController() 47 | { 48 | effort_command_sub_.shutdown(); 49 | } 50 | 51 | bool BaxterEffortController::init( 52 | hardware_interface::EffortJointInterface *robot, ros::NodeHandle &nh) 53 | { 54 | // Store nodehandle 55 | nh_ = nh; 56 | 57 | // Get joint sub-controllers 58 | XmlRpc::XmlRpcValue xml_struct; 59 | if( !nh_.getParam("joints", xml_struct) ) 60 | { 61 | ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); 62 | return false; 63 | } 64 | 65 | // Make sure it's a struct 66 | if(xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) 67 | { 68 | ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); 69 | return false; 70 | } 71 | 72 | // Get number of joints 73 | n_joints_ = xml_struct.size(); 74 | ROS_INFO_STREAM("Initializing BaxterEffortController with "<second.getType() != XmlRpc::XmlRpcValue::TypeStruct) 84 | { 85 | ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); 86 | return false; 87 | } 88 | 89 | // Get joint names from the parameter server 90 | std::string joint_name[n_joints_]; 91 | // Get joint controller name 92 | std::string joint_controller_name = joint_it->first; 93 | 94 | // Get the joint-namespace nodehandle 95 | { 96 | ros::NodeHandle joint_nh(nh_, "joints/"+joint_controller_name); 97 | ROS_INFO_STREAM_NAMED("init","Loading sub-controller '" << joint_controller_name 98 | << "', Namespace: " << joint_nh.getNamespace()); 99 | 100 | effort_controllers_[i].reset(new effort_controllers::JointEffortController()); 101 | effort_controllers_[i]->init(robot, joint_nh); 102 | 103 | if( !joint_nh.getParam("joint",joint_name[i])) 104 | { 105 | ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", joint_nh.getNamespace().c_str()); 106 | return false; 107 | } 108 | // Create a publisher for every joint controller that publishes to the command topic under that controller 109 | effort_command_pub_[i]=joint_nh.advertise("command",1); 110 | 111 | } // end of joint-namespaces 112 | 113 | // Add joint name to map (allows unordered list to quickly be mapped to the ordered index) 114 | joint_to_index_map_.insert(std::pair 115 | (joint_name[i],i)); 116 | // increment joint i 117 | ++i; 118 | } 119 | 120 | // Get controller topic name that it will subscribe to 121 | if( nh_.getParam("topic", topic_name) ) // They provided a custom topic to subscribe to 122 | { 123 | // Get a node handle that is relative to the base path 124 | ros::NodeHandle nh_base("~"); 125 | 126 | // Create command subscriber custom to baxter 127 | effort_command_sub_ = nh_base.subscribe 128 | (topic_name, 1, &BaxterEffortController::commandCB, this); 129 | } 130 | else // default "command" topic 131 | { 132 | // Create command subscriber custom to baxter 133 | effort_command_sub_ = nh_.subscribe 134 | ("command", 1, &BaxterEffortController::commandCB, this); 135 | } 136 | 137 | return true; 138 | } 139 | 140 | 141 | 142 | void BaxterEffortController::starting(const ros::Time& time) 143 | { 144 | for(int i=0; istarting(time); 146 | } 147 | 148 | void BaxterEffortController::stopping(const ros::Time& time) 149 | { 150 | 151 | } 152 | 153 | void BaxterEffortController::update(const ros::Time& time, const ros::Duration& period) 154 | { 155 | // Check if there are commands to be updated 156 | if(!new_command_) 157 | return; 158 | new_command_=false; 159 | for(size_t i=0; iupdate(time, period); 163 | } 164 | } 165 | 166 | void BaxterEffortController::commandCB(const baxter_core_msgs::JointCommandConstPtr& msg) 167 | { 168 | //Check if the number of joints and effort values are equal 169 | if( msg->command.size() != msg->names.size() ) 170 | { 171 | ROS_ERROR_STREAM_NAMED("update","List of names does not match list of efforts size, " 172 | << msg->command.size() << " != " << msg->names.size() ); 173 | return; 174 | } 175 | 176 | std::map::iterator name_it; 177 | // Map incoming joint names and effort values to the correct internal ordering 178 | for(size_t i=0; inames.size(); i++) 179 | { 180 | // Check if the joint name is in our map 181 | name_it = joint_to_index_map_.find(msg->names[i]); 182 | 183 | if( name_it != joint_to_index_map_.end() ) 184 | { 185 | // Joint is in the map, so we'll update the joint effort 186 | m.data=msg->command[i]; 187 | // Publish the joint effort to the corresponding joint controller 188 | effort_command_pub_[name_it->second].publish( m ); 189 | } 190 | } 191 | // Update that new effort values are waiting to be sent to the joints 192 | new_command_ = true; 193 | } 194 | 195 | 196 | } // namespace 197 | 198 | PLUGINLIB_EXPORT_CLASS( baxter_sim_controllers::BaxterEffortController,controller_interface::ControllerBase) 199 | -------------------------------------------------------------------------------- /baxter_sim_controllers/src/baxter_gripper_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2014 Kei Okada 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace baxter_sim_controllers { 35 | 36 | BaxterGripperController::BaxterGripperController() 37 | : new_command(true), 38 | update_counter(0), 39 | mimic_idx_(0), 40 | main_idx_(0) { 41 | } 42 | 43 | BaxterGripperController::~BaxterGripperController() { 44 | gripper_command_sub.shutdown(); 45 | } 46 | 47 | bool BaxterGripperController::init(hardware_interface::EffortJointInterface *robot, 48 | ros::NodeHandle &nh) { 49 | // Store nodehandle 50 | nh_ = nh; 51 | 52 | // Get joint sub-controllers 53 | XmlRpc::XmlRpcValue xml_struct; 54 | if (!nh_.getParam("joints", xml_struct)) { 55 | ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", 56 | nh_.getNamespace().c_str()); 57 | return false; 58 | } 59 | 60 | // Make sure it's a struct 61 | if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 62 | ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", 63 | nh_.getNamespace().c_str()); 64 | return false; 65 | } 66 | 67 | // Get number of joints 68 | n_joints = xml_struct.size(); 69 | ROS_INFO_STREAM( 70 | "Initializing BaxterGripperController with "<second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 78 | ROS_ERROR( 79 | "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", 80 | nh_.getNamespace().c_str()); 81 | return false; 82 | } 83 | 84 | // Get joint controller name 85 | std::string joint_controller_name = joint_it->first; 86 | 87 | // Get the joint-namespace nodehandle 88 | { 89 | ros::NodeHandle joint_nh(nh_, "joints/" + joint_controller_name); 90 | ROS_INFO_STREAM_NAMED( 91 | "init", 92 | "Loading sub-controller '" << joint_controller_name << "', Namespace: " << joint_nh.getNamespace()); 93 | 94 | gripper_controllers[i].reset( 95 | new effort_controllers::JointPositionController()); 96 | gripper_controllers[i]->init(robot, joint_nh); 97 | 98 | } // end of joint-namespaces 99 | 100 | // Set mimic indices 101 | if ( gripper_controllers[i]->joint_urdf_->mimic ) { 102 | mimic_idx_ = i; 103 | } 104 | else{ 105 | main_idx_ = i; 106 | } 107 | 108 | // Add joint name to map (allows unordered list to quickly be mapped to the ordered index) 109 | joint_to_index_map.insert( 110 | std::pair(gripper_controllers[i]->getJointName(), 111 | i)); 112 | 113 | // increment joint i 114 | ++i; 115 | } 116 | 117 | // Get controller topic name that it will subscribe to 118 | if (nh_.getParam("topic", topic_name)) { // They provided a custom topic to subscribe to 119 | 120 | // Get a node handle that is relative to the base path 121 | ros::NodeHandle nh_base("~"); 122 | 123 | // Create command subscriber custom to baxter 124 | gripper_command_sub = nh_base.subscribe( 125 | topic_name, 1, &BaxterGripperController::commandCB, this); 126 | } else // default "command" topic 127 | { 128 | // Create command subscriber custom to baxter 129 | gripper_command_sub = nh_.subscribe( 130 | "command", 1, &BaxterGripperController::commandCB, this); 131 | } 132 | return true; 133 | } 134 | 135 | void BaxterGripperController::starting(const ros::Time& time) { 136 | } 137 | 138 | void BaxterGripperController::stopping(const ros::Time& time) { 139 | 140 | } 141 | 142 | void BaxterGripperController::update(const ros::Time& time, 143 | const ros::Duration& period) { 144 | // Debug info 145 | update_counter++; 146 | //TODO: Change to ROS Param (20 Hz) 147 | if (update_counter % 5 == 0) { 148 | updateCommands(); 149 | } 150 | 151 | // Apply joint commands 152 | for (size_t i = 0; i < n_joints; i++) { 153 | // Update the individual joint controllers 154 | gripper_controllers[i]->update(time, period); 155 | } 156 | } 157 | 158 | void BaxterGripperController::updateCommands() { 159 | // Check if we have a new command to publish 160 | if (!new_command) 161 | return; 162 | 163 | // Go ahead and assume we have proccessed the current message 164 | new_command = false; 165 | 166 | // Get latest command 167 | const baxter_core_msgs::EndEffectorCommand &command = *(gripper_command_buffer 168 | .readFromRT()); 169 | 170 | ROS_DEBUG_STREAM("Gripper update commands " << command.command << " " << command.args); 171 | if ( command.command != baxter_core_msgs::EndEffectorCommand::CMD_GO) 172 | return; 173 | 174 | double cmd_position = gripper_controllers[main_idx_]->getPosition(); 175 | #ifndef DEPRECATED_YAML_CPP_VERSION 176 | YAML::Node args = YAML::Load(command.args); 177 | if ( args["position"] ) { 178 | cmd_position = args["position"].as(); 179 | // Check Command Limits: 180 | if(cmd_position < 0.0){ 181 | cmd_position = 0.0; 182 | } 183 | else if(cmd_position > 100.0){ 184 | cmd_position = 100.0; 185 | } 186 | // cmd = ratio * range 187 | cmd_position = (cmd_position/100.0) * 188 | (gripper_controllers[main_idx_]->joint_urdf_->limits->upper - gripper_controllers[main_idx_]->joint_urdf_->limits->lower); 189 | } 190 | #endif 191 | // Update the individual joint controllers 192 | ROS_DEBUG_STREAM(gripper_controllers[main_idx_]->joint_urdf_->name << "->setCommand(" << cmd_position << ")"); 193 | gripper_controllers[main_idx_]->setCommand(cmd_position); 194 | gripper_controllers[mimic_idx_]->setCommand(gripper_controllers[mimic_idx_]->joint_urdf_->mimic->multiplier*cmd_position+gripper_controllers[mimic_idx_]->joint_urdf_->mimic->offset); 195 | } 196 | 197 | void BaxterGripperController::commandCB( 198 | const baxter_core_msgs::EndEffectorCommandConstPtr& msg) { 199 | // the writeFromNonRT can be used in RT, if you have the guarantee that 200 | // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) 201 | // * there is only one single rt thread 202 | gripper_command_buffer.writeFromNonRT(*msg); 203 | 204 | new_command = true; 205 | } 206 | 207 | } // namespace 208 | 209 | PLUGINLIB_EXPORT_CLASS(baxter_sim_controllers::BaxterGripperController, 210 | controller_interface::ControllerBase) 211 | -------------------------------------------------------------------------------- /baxter_sim_controllers/src/baxter_head_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | #include 31 | #include 32 | 33 | namespace baxter_sim_controllers { 34 | 35 | BaxterHeadController::BaxterHeadController() 36 | : new_command(true), 37 | update_counter(0) { 38 | } 39 | 40 | BaxterHeadController::~BaxterHeadController() { 41 | head_command_sub.shutdown(); 42 | } 43 | 44 | bool BaxterHeadController::init(hardware_interface::EffortJointInterface *robot, 45 | ros::NodeHandle &nh) { 46 | // Store nodehandle 47 | nh_ = nh; 48 | 49 | // Get joint sub-controllers 50 | XmlRpc::XmlRpcValue xml_struct; 51 | if (!nh_.getParam("joints", xml_struct)) { 52 | ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", 53 | nh_.getNamespace().c_str()); 54 | return false; 55 | } 56 | 57 | // Make sure it's a struct 58 | if (xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 59 | ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')", 60 | nh_.getNamespace().c_str()); 61 | return false; 62 | } 63 | 64 | // Get number of joints 65 | n_joints = xml_struct.size(); 66 | ROS_INFO_STREAM( 67 | "Initializing BaxterHeadController with "<second.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 76 | ROS_ERROR( 77 | "The 'joints/joint_controller' parameter is not a struct (namespace '%s')", 78 | nh_.getNamespace().c_str()); 79 | return false; 80 | } 81 | 82 | // Get joint controller name 83 | std::string joint_controller_name = joint_it->first; 84 | 85 | // Get the joint-namespace nodehandle 86 | { 87 | ros::NodeHandle joint_nh(nh_, "joints/" + joint_controller_name); 88 | ROS_INFO_STREAM_NAMED( 89 | "init", 90 | "Loading sub-controller '" << joint_controller_name << "', Namespace: " << joint_nh.getNamespace()); 91 | 92 | head_controllers[i].reset( 93 | new effort_controllers::JointPositionController()); 94 | head_controllers[i]->init(robot, joint_nh); 95 | 96 | } // end of joint-namespaces 97 | 98 | // Add joint name to map (allows unordered list to quickly be mapped to the ordered index) 99 | joint_to_index_map.insert( 100 | std::pair(head_controllers[i]->getJointName(), 101 | i)); 102 | 103 | // increment joint i 104 | ++i; 105 | } 106 | 107 | // Get controller topic name that it will subscribe to 108 | if (nh_.getParam("topic", topic_name)) { // They provided a custom topic to subscribe to 109 | 110 | // Get a node handle that is relative to the base path 111 | ros::NodeHandle nh_base("~"); 112 | 113 | // Create command subscriber custom to baxter 114 | head_command_sub = nh_base.subscribe( 115 | topic_name, 1, &BaxterHeadController::commandCB, this); 116 | } else // default "command" topic 117 | { 118 | // Create command subscriber custom to baxter 119 | head_command_sub = nh_.subscribe( 120 | "command", 1, &BaxterHeadController::commandCB, this); 121 | } 122 | 123 | return true; 124 | } 125 | 126 | void BaxterHeadController::starting(const ros::Time& time) { 127 | baxter_core_msgs::HeadPanCommand initial_command; 128 | 129 | // Fill in the initial command 130 | for (int i = 0; i < n_joints; i++) { 131 | initial_command.target = head_controllers[i]->getPosition(); 132 | } 133 | head_command_buffer.initRT(initial_command); 134 | new_command = true; 135 | } 136 | 137 | void BaxterHeadController::stopping(const ros::Time& time) { 138 | 139 | } 140 | 141 | void BaxterHeadController::update(const ros::Time& time, 142 | const ros::Duration& period) { 143 | // Debug info 144 | update_counter++; 145 | if (update_counter % 100 == 0) 146 | 147 | updateCommands(); 148 | 149 | // Apply joint commands 150 | for (size_t i = 0; i < n_joints; i++) { 151 | // Update the individual joint controllers 152 | head_controllers[i]->update(time, period); 153 | } 154 | } 155 | 156 | void BaxterHeadController::updateCommands() { 157 | // Check if we have a new command to publish 158 | if (!new_command) 159 | return; 160 | 161 | // Go ahead and assume we have proccessed the current message 162 | new_command = false; 163 | 164 | // Get latest command 165 | const baxter_core_msgs::HeadPanCommand &command = *(head_command_buffer 166 | .readFromRT()); 167 | 168 | head_controllers[0]->setCommand(command.target); 169 | } 170 | 171 | void BaxterHeadController::commandCB( 172 | const baxter_core_msgs::HeadPanCommandConstPtr& msg) { 173 | // the writeFromNonRT can be used in RT, if you have the guarantee that 174 | // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) 175 | // * there is only one single rt thread 176 | head_command_buffer.writeFromNonRT(*msg); 177 | 178 | new_command = true; 179 | } 180 | 181 | } // namespace 182 | 183 | PLUGINLIB_EXPORT_CLASS(baxter_sim_controllers::BaxterHeadController, 184 | controller_interface::ControllerBase) 185 | 186 | -------------------------------------------------------------------------------- /baxter_sim_controllers/src/baxter_position_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, Open Source Robotics Foundation 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Open Source Robotics Foundation 18 | * nor the names of its contributors may be 19 | * used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /** 37 | * \author Dave Coleman 38 | * \desc Multiple joint position controller for Baxter SDK 39 | */ 40 | 41 | #include 42 | #include 43 | 44 | namespace baxter_sim_controllers { 45 | 46 | BaxterPositionController::BaxterPositionController() 47 | : new_command_(true), 48 | update_counter_(0) 49 | {} 50 | 51 | BaxterPositionController::~BaxterPositionController() 52 | { 53 | position_command_sub_.shutdown(); 54 | } 55 | 56 | bool BaxterPositionController::init( 57 | hardware_interface::EffortJointInterface *robot, ros::NodeHandle &nh) 58 | { 59 | // Store nodehandle 60 | nh_ = nh; 61 | 62 | // Get joint sub-controllers 63 | XmlRpc::XmlRpcValue xml_struct; 64 | if( !nh_.getParam("joints", xml_struct) ) 65 | { 66 | ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); 67 | return false; 68 | } 69 | 70 | // Make sure it's a struct 71 | if(xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) 72 | { 73 | ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); 74 | return false; 75 | } 76 | 77 | // Get number of joints 78 | n_joints_ = xml_struct.size(); 79 | ROS_INFO_STREAM("Initializing BaxterPositionController with "<second.getType() != XmlRpc::XmlRpcValue::TypeStruct) 89 | { 90 | ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); 91 | return false; 92 | } 93 | 94 | // Get joint controller name 95 | std::string joint_controller_name = joint_it->first; 96 | 97 | // Get the joint-namespace nodehandle 98 | { 99 | ros::NodeHandle joint_nh(nh_, "joints/"+joint_controller_name); 100 | ROS_INFO_STREAM_NAMED("init","Loading sub-controller '" << joint_controller_name 101 | << "', Namespace: " << joint_nh.getNamespace()); 102 | 103 | position_controllers_[i].reset(new effort_controllers::JointPositionController()); 104 | position_controllers_[i]->init(robot, joint_nh); 105 | 106 | // DEBUG 107 | //position_controllers_[i]->printDebug(); 108 | 109 | } // end of joint-namespaces 110 | 111 | // Add joint name to map (allows unordered list to quickly be mapped to the ordered index) 112 | joint_to_index_map_.insert(std::pair 113 | (position_controllers_[i]->getJointName(),i)); 114 | 115 | // increment joint i 116 | ++i; 117 | } 118 | 119 | // Get controller topic name that it will subscribe to 120 | if( nh_.getParam("topic", topic_name) ) // They provided a custom topic to subscribe to 121 | { 122 | // Get a node handle that is relative to the base path 123 | ros::NodeHandle nh_base("~"); 124 | 125 | // Create command subscriber custom to baxter 126 | position_command_sub_ = nh_base.subscribe 127 | (topic_name, 1, &BaxterPositionController::commandCB, this); 128 | } 129 | else // default "command" topic 130 | { 131 | // Create command subscriber custom to baxter 132 | position_command_sub_ = nh_.subscribe 133 | ("command", 1, &BaxterPositionController::commandCB, this); 134 | } 135 | 136 | return true; 137 | } 138 | 139 | 140 | 141 | void BaxterPositionController::starting(const ros::Time& time) 142 | { 143 | baxter_core_msgs::JointCommand initial_command; 144 | 145 | // Fill in the initial command 146 | for(int i=0; igetJointName()); 149 | initial_command.command.push_back(position_controllers_[i]->getPosition()); 150 | } 151 | position_command_buffer_.initRT(initial_command); 152 | new_command_ = true; 153 | } 154 | 155 | void BaxterPositionController::stopping(const ros::Time& time) 156 | { 157 | 158 | } 159 | 160 | void BaxterPositionController::update(const ros::Time& time, const ros::Duration& period) 161 | { 162 | // Debug info 163 | verbose_ = false; 164 | update_counter_ ++; 165 | if( update_counter_ % 100 == 0 ) 166 | verbose_ = true; 167 | 168 | updateCommands(); 169 | 170 | // Apply joint commands 171 | for(size_t i=0; iupdate(time, period); 175 | } 176 | } 177 | 178 | void BaxterPositionController::updateCommands() 179 | { 180 | // Check if we have a new command to publish 181 | if( !new_command_ ) 182 | return; 183 | 184 | // Go ahead and assume we have proccessed the current message 185 | new_command_ = false; 186 | 187 | // Get latest command 188 | const baxter_core_msgs::JointCommand &command = *(position_command_buffer_.readFromRT()); 189 | 190 | // Error check message data 191 | if( command.command.size() != command.names.size() ) 192 | { 193 | ROS_ERROR_STREAM_NAMED("update","List of names does not match list of angles size, " 194 | << command.command.size() << " != " << command.names.size() ); 195 | return; 196 | } 197 | 198 | std::map::iterator name_it; 199 | 200 | // Map incoming joint names and angles to the correct internal ordering 201 | for(size_t i=0; isecond]->setCommand( command.command[i] ); 210 | } 211 | } 212 | } 213 | 214 | void BaxterPositionController::commandCB(const baxter_core_msgs::JointCommandConstPtr& msg) 215 | { 216 | // the writeFromNonRT can be used in RT, if you have the guarantee that 217 | // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) 218 | // * there is only one single rt thread 219 | position_command_buffer_.writeFromNonRT(*msg); 220 | 221 | new_command_ = true; 222 | } 223 | 224 | 225 | } // namespace 226 | 227 | PLUGINLIB_EXPORT_CLASS( baxter_sim_controllers::BaxterPositionController,controller_interface::ControllerBase) 228 | -------------------------------------------------------------------------------- /baxter_sim_controllers/src/baxter_velocity_controller.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2013, Open Source Robotics Foundation 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Open Source Robotics Foundation 18 | * nor the names of its contributors may be 19 | * used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | /** 37 | * \author Dave Coleman 38 | * \desc Multiple joint velocity controller for Baxter SDK 39 | */ 40 | 41 | #include 42 | #include 43 | 44 | namespace baxter_sim_controllers { 45 | 46 | BaxterVelocityController::BaxterVelocityController() 47 | : new_command_(true), 48 | update_counter_(0) 49 | {} 50 | 51 | BaxterVelocityController::~BaxterVelocityController() 52 | { 53 | velocity_command_sub_.shutdown(); 54 | } 55 | 56 | bool BaxterVelocityController::init( 57 | hardware_interface::EffortJointInterface *robot, ros::NodeHandle &nh) 58 | { 59 | // Store nodehandle 60 | nh_ = nh; 61 | 62 | // Get joint sub-controllers 63 | XmlRpc::XmlRpcValue xml_struct; 64 | if( !nh_.getParam("joints", xml_struct) ) 65 | { 66 | ROS_ERROR("No 'joints' parameter in controller (namespace '%s')", nh_.getNamespace().c_str()); 67 | return false; 68 | } 69 | 70 | // Make sure it's a struct 71 | if(xml_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct) 72 | { 73 | ROS_ERROR("The 'joints' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); 74 | return false; 75 | } 76 | 77 | // Get number of joints 78 | n_joints_ = xml_struct.size(); 79 | ROS_INFO_STREAM("Initializing BaxterVelocityController with "<second.getType() != XmlRpc::XmlRpcValue::TypeStruct) 89 | { 90 | ROS_ERROR("The 'joints/joint_controller' parameter is not a struct (namespace '%s')",nh_.getNamespace().c_str()); 91 | return false; 92 | } 93 | 94 | // Get joint controller name 95 | std::string joint_controller_name = joint_it->first; 96 | 97 | // Get the joint-namespace nodehandle 98 | { 99 | ros::NodeHandle joint_nh(nh_, "joints/"+joint_controller_name); 100 | ROS_INFO_STREAM_NAMED("init","Loading sub-controller '" << joint_controller_name 101 | << "', Namespace: " << joint_nh.getNamespace()); 102 | 103 | velocity_controllers_[i].reset(new effort_controllers::JointVelocityController()); 104 | velocity_controllers_[i]->init(robot, joint_nh); 105 | 106 | // DEBUG 107 | //velocity_controllers_[i]->printDebug(); 108 | 109 | } // end of joint-namespaces 110 | 111 | // Add joint name to map (allows unordered list to quickly be mapped to the ordered index) 112 | joint_to_index_map_.insert(std::pair 113 | (velocity_controllers_[i]->getJointName(),i)); 114 | 115 | // increment joint i 116 | ++i; 117 | } 118 | 119 | // Get controller topic name that it will subscribe to 120 | if( nh_.getParam("topic", topic_name) ) // They provided a custom topic to subscribe to 121 | { 122 | // Get a node handle that is relative to the base path 123 | ros::NodeHandle nh_base("~"); 124 | 125 | // Create command subscriber custom to baxter 126 | velocity_command_sub_ = nh_base.subscribe 127 | (topic_name, 1, &BaxterVelocityController::commandCB, this); 128 | } 129 | else // default "command" topic 130 | { 131 | // Create command subscriber custom to baxter 132 | velocity_command_sub_ = nh_.subscribe 133 | ("command", 1, &BaxterVelocityController::commandCB, this); 134 | } 135 | 136 | return true; 137 | } 138 | 139 | 140 | 141 | void BaxterVelocityController::starting(const ros::Time& time) 142 | { 143 | baxter_core_msgs::JointCommand initial_command; 144 | 145 | // Fill in the initial command 146 | for(int i=0; igetJointName()); 149 | initial_command.command.push_back(0); 150 | } 151 | velocity_command_buffer_.initRT(initial_command); 152 | new_command_ = true; 153 | } 154 | 155 | void BaxterVelocityController::stopping(const ros::Time& time) 156 | { 157 | 158 | } 159 | 160 | void BaxterVelocityController::update(const ros::Time& time, const ros::Duration& period) 161 | { 162 | // Debug info 163 | verbose_ = false; 164 | update_counter_ ++; 165 | if( update_counter_ % 100 == 0 ) 166 | verbose_ = true; 167 | 168 | updateCommands(); 169 | 170 | // Apply joint commands 171 | for(size_t i=0; iupdate(time, period); 175 | } 176 | } 177 | 178 | void BaxterVelocityController::updateCommands() 179 | { 180 | // Check if we have a new command to publish 181 | if( !new_command_ ) 182 | return; 183 | 184 | // Go ahead and assume we have proccessed the current message 185 | new_command_ = false; 186 | 187 | // Get latest command 188 | const baxter_core_msgs::JointCommand &command = *(velocity_command_buffer_.readFromRT()); 189 | 190 | // Error check message data 191 | if( command.command.size() != command.names.size() ) 192 | { 193 | ROS_ERROR_STREAM_NAMED("update","List of names does not match list of velocities size, " 194 | << command.command.size() << " != " << command.names.size() ); 195 | return; 196 | } 197 | 198 | std::map::iterator name_it; 199 | 200 | // Map incoming joint names and velocities to the correct internal ordering 201 | for(size_t i=0; isecond]->setCommand( command.command[i] ); 210 | } 211 | } 212 | } 213 | 214 | void BaxterVelocityController::commandCB(const baxter_core_msgs::JointCommandConstPtr& msg) 215 | { 216 | // the writeFromNonRT can be used in RT, if you have the guarantee that 217 | // * no non-rt thread is calling the same function (we're not subscribing to ros callbacks) 218 | // * there is only one single rt thread 219 | velocity_command_buffer_.writeFromNonRT(*msg); 220 | 221 | new_command_ = true; 222 | } 223 | 224 | 225 | } // namespace 226 | 227 | PLUGINLIB_EXPORT_CLASS( baxter_sim_controllers::BaxterVelocityController,controller_interface::ControllerBase) 228 | -------------------------------------------------------------------------------- /baxter_sim_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(baxter_sim_examples) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | rospack 7 | baxter_core_msgs 8 | baxter_gazebo 9 | baxter_interface 10 | baxter_tools 11 | baxter_tools 12 | gazebo_ros 13 | gazebo_msgs 14 | ) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS 18 | rospy 19 | rospack 20 | baxter_core_msgs 21 | baxter_gazebo 22 | baxter_interface 23 | baxter_tools 24 | baxter_tools 25 | gazebo_ros 26 | gazebo_msgs 27 | ) 28 | 29 | 30 | ############# 31 | ## Install ## 32 | ############# 33 | 34 | install(PROGRAMS 35 | scripts/ik_pick_and_place_demo.py 36 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | ) 38 | 39 | foreach(dir launch models) 40 | install(DIRECTORY ${dir}/ 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 42 | endforeach(dir) 43 | 44 | -------------------------------------------------------------------------------- /baxter_sim_examples/launch/baxter_pick_and_place_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /baxter_sim_examples/models/block/model.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | Gazebo/Red 23 | 1000 24 | 1000 25 | 26 | 27 | -------------------------------------------------------------------------------- /baxter_sim_examples/models/cafe_table/materials/textures/Maple.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_examples/models/cafe_table/materials/textures/Maple.jpg -------------------------------------------------------------------------------- /baxter_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg -------------------------------------------------------------------------------- /baxter_sim_examples/models/cafe_table/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Cafe table 5 | 1.0 6 | model-1_4.sdf 7 | model.sdf 8 | 9 | 10 | Nate Koenig 11 | nate@osrfoundation.org 12 | 13 | 14 | 15 | A model of a cafe table. 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /baxter_sim_examples/models/cafe_table/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0.755 0 0 0 8 | 9 | 10 | 0.913 0.913 0.04 11 | 12 | 13 | 14 | 15 | 16 | 0 0 0.37 0 0 0 17 | 18 | 19 | 0.042 0.042 0.74 20 | 21 | 22 | 23 | 24 | 25 | 0 0 0.02 0 0 0 26 | 27 | 28 | 0.56 0.56 0.04 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | model://cafe_table/meshes/cafe_table.dae 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /baxter_sim_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_sim_examples 4 | 1.2.12 5 | The baxter_sim_examples package 6 | 7 | 8 | Rethink Robotics Inc. 9 | 10 | BSD 11 | http://sdk.rethinkrobotics.com 12 | 13 | https://github.com/RethinkRobotics/baxter_simulator 14 | 15 | 16 | https://github.com/RethinkRobotics/baxter_simulator/issues 17 | 18 | Rethink Robotics Inc. 19 | Ian McMahon 20 | 21 | catkin 22 | 23 | rospy 24 | rospack 25 | baxter_core_msgs 26 | baxter_gazebo 27 | baxter_interface 28 | baxter_tools 29 | gazebo_ros 30 | gazebo_msgs 31 | 32 | rospy 33 | rospack 34 | baxter_core_msgs 35 | baxter_gazebo 36 | baxter_interface 37 | baxter_tools 38 | gazebo_ros 39 | gazebo_msgs 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /baxter_sim_examples/scripts/ik_pick_and_place_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2013-2015, Rethink Robotics 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # 1. Redistributions of source code must retain the above copyright notice, 10 | # this list of conditions and the following disclaimer. 11 | # 2. Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # 3. Neither the name of the Rethink Robotics nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | 30 | """ 31 | Baxter RSDK Inverse Kinematics Pick and Place Demo 32 | """ 33 | import argparse 34 | import struct 35 | import sys 36 | import copy 37 | 38 | import rospy 39 | import rospkg 40 | 41 | from gazebo_msgs.srv import ( 42 | SpawnModel, 43 | DeleteModel, 44 | ) 45 | from geometry_msgs.msg import ( 46 | PoseStamped, 47 | Pose, 48 | Point, 49 | Quaternion, 50 | ) 51 | from std_msgs.msg import ( 52 | Header, 53 | Empty, 54 | ) 55 | 56 | from baxter_core_msgs.srv import ( 57 | SolvePositionIK, 58 | SolvePositionIKRequest, 59 | ) 60 | 61 | import baxter_interface 62 | 63 | class PickAndPlace(object): 64 | def __init__(self, limb, hover_distance = 0.15, verbose=True): 65 | self._limb_name = limb # string 66 | self._hover_distance = hover_distance # in meters 67 | self._verbose = verbose # bool 68 | self._limb = baxter_interface.Limb(limb) 69 | self._gripper = baxter_interface.Gripper(limb) 70 | ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" 71 | self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK) 72 | rospy.wait_for_service(ns, 5.0) 73 | # verify robot is enabled 74 | print("Getting robot state... ") 75 | self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION) 76 | self._init_state = self._rs.state().enabled 77 | print("Enabling robot... ") 78 | self._rs.enable() 79 | 80 | def move_to_start(self, start_angles=None): 81 | print("Moving the {0} arm to start pose...".format(self._limb_name)) 82 | if not start_angles: 83 | start_angles = dict(zip(self._joint_names, [0]*7)) 84 | self._guarded_move_to_joint_position(start_angles) 85 | self.gripper_open() 86 | rospy.sleep(1.0) 87 | print("Running. Ctrl-c to quit") 88 | 89 | def ik_request(self, pose): 90 | hdr = Header(stamp=rospy.Time.now(), frame_id='base') 91 | ikreq = SolvePositionIKRequest() 92 | ikreq.pose_stamp.append(PoseStamped(header=hdr, pose=pose)) 93 | try: 94 | resp = self._iksvc(ikreq) 95 | except (rospy.ServiceException, rospy.ROSException), e: 96 | rospy.logerr("Service call failed: %s" % (e,)) 97 | return False 98 | # Check if result valid, and type of seed ultimately used to get solution 99 | # convert rospy's string representation of uint8[]'s to int's 100 | resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type) 101 | limb_joints = {} 102 | if (resp_seeds[0] != resp.RESULT_INVALID): 103 | seed_str = { 104 | ikreq.SEED_USER: 'User Provided Seed', 105 | ikreq.SEED_CURRENT: 'Current Joint Angles', 106 | ikreq.SEED_NS_MAP: 'Nullspace Setpoints', 107 | }.get(resp_seeds[0], 'None') 108 | if self._verbose: 109 | print("IK Solution SUCCESS - Valid Joint Solution Found from Seed Type: {0}".format( 110 | (seed_str))) 111 | # Format solution into Limb API-compatible dictionary 112 | limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) 113 | if self._verbose: 114 | print("IK Joint Solution:\n{0}".format(limb_joints)) 115 | print("------------------") 116 | else: 117 | rospy.logerr("INVALID POSE - No Valid Joint Solution Found.") 118 | return False 119 | return limb_joints 120 | 121 | def _guarded_move_to_joint_position(self, joint_angles): 122 | if joint_angles: 123 | self._limb.move_to_joint_positions(joint_angles) 124 | else: 125 | rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.") 126 | 127 | def gripper_open(self): 128 | self._gripper.open() 129 | rospy.sleep(1.0) 130 | 131 | def gripper_close(self): 132 | self._gripper.close() 133 | rospy.sleep(1.0) 134 | 135 | def _approach(self, pose): 136 | approach = copy.deepcopy(pose) 137 | # approach with a pose the hover-distance above the requested pose 138 | approach.position.z = approach.position.z + self._hover_distance 139 | joint_angles = self.ik_request(approach) 140 | self._guarded_move_to_joint_position(joint_angles) 141 | 142 | def _retract(self): 143 | # retrieve current pose from endpoint 144 | current_pose = self._limb.endpoint_pose() 145 | ik_pose = Pose() 146 | ik_pose.position.x = current_pose['position'].x 147 | ik_pose.position.y = current_pose['position'].y 148 | ik_pose.position.z = current_pose['position'].z + self._hover_distance 149 | ik_pose.orientation.x = current_pose['orientation'].x 150 | ik_pose.orientation.y = current_pose['orientation'].y 151 | ik_pose.orientation.z = current_pose['orientation'].z 152 | ik_pose.orientation.w = current_pose['orientation'].w 153 | joint_angles = self.ik_request(ik_pose) 154 | # servo up from current pose 155 | self._guarded_move_to_joint_position(joint_angles) 156 | 157 | def _servo_to_pose(self, pose): 158 | # servo down to release 159 | joint_angles = self.ik_request(pose) 160 | self._guarded_move_to_joint_position(joint_angles) 161 | 162 | def pick(self, pose): 163 | # open the gripper 164 | self.gripper_open() 165 | # servo above pose 166 | self._approach(pose) 167 | # servo to pose 168 | self._servo_to_pose(pose) 169 | # close gripper 170 | self.gripper_close() 171 | # retract to clear object 172 | self._retract() 173 | 174 | def place(self, pose): 175 | # servo above pose 176 | self._approach(pose) 177 | # servo to pose 178 | self._servo_to_pose(pose) 179 | # open the gripper 180 | self.gripper_open() 181 | # retract to clear object 182 | self._retract() 183 | 184 | def load_gazebo_models(table_pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)), 185 | table_reference_frame="world", 186 | block_pose=Pose(position=Point(x=0.6725, y=0.1265, z=0.7825)), 187 | block_reference_frame="world"): 188 | # Get Models' Path 189 | model_path = rospkg.RosPack().get_path('baxter_sim_examples')+"/models/" 190 | # Load Table SDF 191 | table_xml = '' 192 | with open (model_path + "cafe_table/model.sdf", "r") as table_file: 193 | table_xml=table_file.read().replace('\n', '') 194 | # Load Block URDF 195 | block_xml = '' 196 | with open (model_path + "block/model.urdf", "r") as block_file: 197 | block_xml=block_file.read().replace('\n', '') 198 | # Spawn Table SDF 199 | rospy.wait_for_service('/gazebo/spawn_sdf_model') 200 | try: 201 | spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) 202 | resp_sdf = spawn_sdf("cafe_table", table_xml, "/", 203 | table_pose, table_reference_frame) 204 | except rospy.ServiceException, e: 205 | rospy.logerr("Spawn SDF service call failed: {0}".format(e)) 206 | # Spawn Block URDF 207 | rospy.wait_for_service('/gazebo/spawn_urdf_model') 208 | try: 209 | spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel) 210 | resp_urdf = spawn_urdf("block", block_xml, "/", 211 | block_pose, block_reference_frame) 212 | except rospy.ServiceException, e: 213 | rospy.logerr("Spawn URDF service call failed: {0}".format(e)) 214 | 215 | def delete_gazebo_models(): 216 | # This will be called on ROS Exit, deleting Gazebo models 217 | # Do not wait for the Gazebo Delete Model service, since 218 | # Gazebo should already be running. If the service is not 219 | # available since Gazebo has been killed, it is fine to error out 220 | try: 221 | delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) 222 | resp_delete = delete_model("cafe_table") 223 | resp_delete = delete_model("block") 224 | except rospy.ServiceException, e: 225 | rospy.loginfo("Delete Model service call failed: {0}".format(e)) 226 | 227 | def main(): 228 | """RSDK Inverse Kinematics Pick and Place Example 229 | 230 | A Pick and Place example using the Rethink Inverse Kinematics 231 | Service which returns the joint angles a requested Cartesian Pose. 232 | This ROS Service client is used to request both pick and place 233 | poses in the /base frame of the robot. 234 | 235 | Note: This is a highly scripted and tuned demo. The object location 236 | is "known" and movement is done completely open loop. It is expected 237 | behavior that Baxter will eventually mis-pick or drop the block. You 238 | can improve on this demo by adding perception and feedback to close 239 | the loop. 240 | """ 241 | rospy.init_node("ik_pick_and_place_demo") 242 | # Load Gazebo Models via Spawning Services 243 | # Note that the models reference is the /world frame 244 | # and the IK operates with respect to the /base frame 245 | load_gazebo_models() 246 | # Remove models from the scene on shutdown 247 | rospy.on_shutdown(delete_gazebo_models) 248 | 249 | # Wait for the All Clear from emulator startup 250 | rospy.wait_for_message("/robot/sim/started", Empty) 251 | 252 | limb = 'left' 253 | hover_distance = 0.15 # meters 254 | # Starting Joint angles for left arm 255 | starting_joint_angles = {'left_w0': 0.6699952259595108, 256 | 'left_w1': 1.030009435085784, 257 | 'left_w2': -0.4999997247485215, 258 | 'left_e0': -1.189968899785275, 259 | 'left_e1': 1.9400238130755056, 260 | 'left_s0': -0.08000397926829805, 261 | 'left_s1': -0.9999781166910306} 262 | pnp = PickAndPlace(limb, hover_distance) 263 | # An orientation for gripper fingers to be overhead and parallel to the obj 264 | overhead_orientation = Quaternion( 265 | x=-0.0249590815779, 266 | y=0.999649402929, 267 | z=0.00737916180073, 268 | w=0.00486450832011) 269 | block_poses = list() 270 | # The Pose of the block in its initial location. 271 | # You may wish to replace these poses with estimates 272 | # from a perception node. 273 | block_poses.append(Pose( 274 | position=Point(x=0.7, y=0.15, z=-0.129), 275 | orientation=overhead_orientation)) 276 | # Feel free to add additional desired poses for the object. 277 | # Each additional pose will get its own pick and place. 278 | block_poses.append(Pose( 279 | position=Point(x=0.75, y=0.0, z=-0.129), 280 | orientation=overhead_orientation)) 281 | # Move to the desired starting angles 282 | pnp.move_to_start(starting_joint_angles) 283 | idx = 0 284 | while not rospy.is_shutdown(): 285 | print("\nPicking...") 286 | pnp.pick(block_poses[idx]) 287 | print("\nPlacing...") 288 | idx = (idx+1) % len(block_poses) 289 | pnp.place(block_poses[idx]) 290 | return 0 291 | 292 | if __name__ == '__main__': 293 | sys.exit(main()) 294 | -------------------------------------------------------------------------------- /baxter_sim_hardware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(baxter_sim_hardware) 3 | 4 | find_package(catkin 5 | REQUIRED COMPONENTS 6 | std_msgs 7 | roscpp 8 | baxter_core_msgs 9 | cv_bridge 10 | image_transport 11 | baxter_sim_kinematics 12 | tf 13 | ) 14 | 15 | find_package(Boost REQUIRED COMPONENTS system) 16 | find_package(cmake_modules REQUIRED) 17 | find_package(OpenCV REQUIRED) 18 | 19 | find_package(Eigen REQUIRED) 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | CATKIN_DEPENDS 23 | roscpp 24 | baxter_core_msgs 25 | cv_bridge 26 | image_transport 27 | baxter_sim_kinematics 28 | DEPENDS OpenCV 29 | LIBRARIES ${PROJECT_NAME} 30 | ) 31 | 32 | include_directories(include 33 | ${catkin_INCLUDE_DIRS} 34 | ${Boost_INCLUDE_DIRS} 35 | ${EIGEN_INCLUDE_DIRS} 36 | ${OpenCV_INCLUDE_DIRS} 37 | ) 38 | 39 | add_executable(baxter_emulator src/baxter_emulator.cpp) 40 | target_link_libraries(baxter_emulator ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 41 | add_dependencies(baxter_emulator ${catkin_EXPORTED_TARGETS}) 42 | 43 | install( 44 | TARGETS baxter_emulator 45 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 46 | ) 47 | 48 | 49 | install( 50 | DIRECTORY config images include launch 51 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 52 | ) 53 | -------------------------------------------------------------------------------- /baxter_sim_hardware/config/baxter_left_electric_gripper_controller.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | 3 | # Baxter SDK Controllers: Gripper -------------------------- 4 | left_gripper_controller: 5 | type: baxter_sim_controllers/BaxterGripperController 6 | topic: /robot/end_effector/left_gripper/command 7 | properties: /robot/end_effector/left_gripper/properties 8 | joints: 9 | l_gripper_l_finger_controller: 10 | joint: l_gripper_l_finger_joint 11 | pid: {p: 1000, i: 0.1, d: 0.01} 12 | l_gripper_r_finger_controller: 13 | joint: l_gripper_r_finger_joint 14 | pid: {p: 1000, i: 0.1, d: 0.01} 15 | -------------------------------------------------------------------------------- /baxter_sim_hardware/config/baxter_right_electric_gripper_controller.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | 3 | # Baxter SDK Controllers: Gripper -------------------------- 4 | right_gripper_controller: 5 | type: baxter_sim_controllers/BaxterGripperController 6 | topic: /robot/end_effector/right_gripper/command 7 | properties: /robot/end_effector/right_gripper/properties 8 | joints: 9 | r_gripper_l_finger_controller: 10 | joint: r_gripper_l_finger_joint 11 | pid: {p: 1000, i: 0.1, d: 0.01} 12 | r_gripper_r_finger_controller: 13 | joint: r_gripper_r_finger_joint 14 | pid: {p: 1000, i: 0.1, d: 0.01} 15 | -------------------------------------------------------------------------------- /baxter_sim_hardware/config/baxter_sim_controllers.yaml: -------------------------------------------------------------------------------- 1 | robot: 2 | # Publish all joint states ----------------------------------- 3 | 4 | joint_state_controller: 5 | type: joint_state_controller/JointStateController 6 | publish_rate: 50 7 | 8 | # Baxter SDK Controllers: Head -------------------------- 9 | head_position_controller: 10 | type: baxter_sim_controllers/BaxterHeadController 11 | topic: /robot/head/command_head_pan 12 | joints: 13 | head_controller: 14 | joint: head_pan 15 | pid: {p: 10.0, i: 0.01, d: 5.0} 16 | 17 | # Baxter SDK Controllers: Position -------------------------- 18 | right_joint_position_controller: 19 | type: baxter_sim_controllers/BaxterPositionController 20 | topic: /robot/limb/right/joint_command 21 | joints: 22 | right_s0_controller: 23 | joint: right_s0 24 | pid: {p: 700, i: 0.01, d: 100} 25 | right_s1_controller: 26 | joint: right_s1 27 | pid: {p: 10000, i: 100, d: 100} 28 | right_e0_controller: 29 | joint: right_e0 30 | pid: {p: 4500, i: 35, d: 1} 31 | right_e1_controller: 32 | joint: right_e1 33 | pid: {p: 5500, i: 60, d: 2} 34 | right_w0_controller: 35 | joint: right_w0 36 | pid: {p: 1000, i: 30, d: 0.01} 37 | right_w1_controller: 38 | joint: right_w1 39 | pid: {p: 900, i: 0.1, d: 0.01} 40 | right_w2_controller: 41 | joint: right_w2 42 | pid: {p: 1000, i: 0.1, d: 0.01} 43 | 44 | left_joint_position_controller: 45 | type: baxter_sim_controllers/BaxterPositionController 46 | topic: /robot/limb/left/joint_command 47 | joints: 48 | left_s0_controller: 49 | joint: left_s0 50 | pid: {p: 700, i: 0.01, d: 25} 51 | left_s1_controller: 52 | joint: left_s1 53 | pid: {p: 10000, i: 100, d: 100} 54 | left_e0_controller: 55 | joint: left_e0 56 | pid: {p: 4500, i: 35, d: 1} 57 | left_e1_controller: 58 | joint: left_e1 59 | pid: {p: 5500, i: 60, d: 2} 60 | left_w0_controller: 61 | joint: left_w0 62 | pid: {p: 1000, i: 30, d: 0.01} 63 | left_w1_controller: 64 | joint: left_w1 65 | pid: {p: 900, i: 0.1, d: 0.01} 66 | left_w2_controller: 67 | joint: left_w2 68 | pid: {p: 1000, i: 0.1, d: 0.01} 69 | 70 | # Baxter SDK Controllers: Velocity -------------------------- 71 | right_joint_velocity_controller: 72 | type: baxter_sim_controllers/BaxterVelocityController 73 | topic: /robot/limb/right/joint_command 74 | joints: 75 | right_s0_controller: 76 | joint: right_s0 77 | pid: {p: 200.0, i: 0.01, d: 0.1} 78 | right_s1_controller: 79 | joint: right_s1 80 | pid: {p: 800.0, i: 0.01, d: 0.1} 81 | right_e0_controller: 82 | joint: right_e0 83 | pid: {p: 300.0, i: 0.01, d: 0.1} 84 | right_e1_controller: 85 | joint: right_e1 86 | pid: {p: 500.0, i: 0.01, d: 0.1} 87 | right_w0_controller: 88 | joint: right_w0 89 | pid: {p: 200.0, i: 0.01, d: 0.1} 90 | right_w1_controller: 91 | joint: right_w1 92 | pid: {p: 100.0, i: 0.01, d: 0.1} 93 | right_w2_controller: 94 | joint: right_w2 95 | pid: {p: 100.0, i: 0.01, d: 0.1} 96 | 97 | left_joint_velocity_controller: 98 | type: baxter_sim_controllers/BaxterVelocityController 99 | topic: /robot/limb/left/joint_command 100 | joints: 101 | left_s0_controller: 102 | joint: left_s0 103 | pid: {p: 200.0, i: 0.01, d: 0.1} 104 | left_s1_controller: 105 | joint: left_s1 106 | pid: {p: 800.0, i: 0.01, d: 0.1} 107 | left_e0_controller: 108 | joint: left_e0 109 | pid: {p: 300.0, i: 0.01, d: 0.1} 110 | left_e1_controller: 111 | joint: left_e1 112 | pid: {p: 500.0, i: 0.01, d: 0.1} 113 | left_w0_controller: 114 | joint: left_w0 115 | pid: {p: 200.0, i: 0.01, d: 0.1} 116 | left_w1_controller: 117 | joint: left_w1 118 | pid: {p: 100.0, i: 0.01, d: 0.1} 119 | left_w2_controller: 120 | joint: left_w2 121 | pid: {p: 100.0, i: 0.01, d: 0.1} 122 | 123 | # Individual Joint Controllers: Position -------------------------- 124 | right_s0_position_controller: 125 | type: effort_controllers/JointPositionController 126 | joint: right_s0 127 | pid: {p: 700, i: 0.01, d: 100} 128 | right_s1_position_controller: 129 | type: effort_controllers/JointPositionController 130 | joint: right_s1 131 | pid: {p: 10000, i: 100, d: 100} 132 | right_e0_position_controller: 133 | type: effort_controllers/JointPositionController 134 | joint: right_e0 135 | pid: {p: 4500, i: 35, d: 1} 136 | right_e1_position_controller: 137 | type: effort_controllers/JointPositionController 138 | joint: right_e1 139 | pid: {p: 5500, i: 60, d: 2} 140 | right_w0_position_controller: 141 | type: effort_controllers/JointPositionController 142 | joint: right_w0 143 | pid: {p: 1000, i: 30, d: 0.01} 144 | right_w1_position_controller: 145 | type: effort_controllers/JointPositionController 146 | joint: right_w1 147 | pid: {p: 900, i: 0.1, d: 0.01} 148 | right_w2_position_controller: 149 | type: effort_controllers/JointPositionController 150 | joint: right_w2 151 | pid: {p: 1000, i: 0.1, d: 0.01} 152 | 153 | left_s0_position_controller: 154 | type: effort_controllers/JointPositionController 155 | joint: left_s0 156 | pid: {p: 700, i: 0.01, d: 25} 157 | left_s1_position_controller: 158 | type: effort_controllers/JointPositionController 159 | joint: left_s1 160 | pid: {p: 10000, i: 100, d: 100} 161 | left_e0_position_controller: 162 | type: effort_controllers/JointPositionController 163 | joint: left_e0 164 | pid: {p: 4500, i: 35, d: 1} 165 | left_e1_position_controller: 166 | type: effort_controllers/JointPositionController 167 | joint: left_e1 168 | pid: {p: 5500, i: 60, d: 2} 169 | left_w0_position_controller: 170 | type: effort_controllers/JointPositionController 171 | joint: left_w0 172 | pid: {p: 1000, i: 30, d: 0.01} 173 | left_w1_position_controller: 174 | type: effort_controllers/JointPositionController 175 | joint: left_w1 176 | pid: {p: 900, i: 0.1, d: 0.01} 177 | left_w2_position_controller: 178 | type: effort_controllers/JointPositionController 179 | joint: left_w2 180 | pid: {p: 1000, i: 0.1, d: 0.01} 181 | 182 | # Baxter SDK Controllers: Effort -------------------------- 183 | right_joint_effort_controller: 184 | type: baxter_sim_controllers/BaxterEffortController 185 | topic: /robot/limb/right/joint_command 186 | joints: 187 | right_s0_controller: 188 | joint: right_s0 189 | right_s1_controller: 190 | joint: right_s1 191 | right_e0_controller: 192 | joint: right_e0 193 | right_e1_controller: 194 | joint: right_e1 195 | right_w0_controller: 196 | joint: right_w0 197 | right_w1_controller: 198 | joint: right_w1 199 | right_w2_controller: 200 | joint: right_w2 201 | 202 | left_joint_effort_controller: 203 | type: baxter_sim_controllers/BaxterEffortController 204 | topic: /robot/limb/left/joint_command 205 | joints: 206 | left_s0_controller: 207 | joint: left_s0 208 | left_s1_controller: 209 | joint: left_s1 210 | left_e0_controller: 211 | joint: left_e0 212 | left_e1_controller: 213 | joint: left_e1 214 | left_w0_controller: 215 | joint: left_w0 216 | left_w1_controller: 217 | joint: left_w1 218 | left_w2_controller: 219 | joint: left_w2 220 | -------------------------------------------------------------------------------- /baxter_sim_hardware/images/researchsdk.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_hardware/images/researchsdk.png -------------------------------------------------------------------------------- /baxter_sim_hardware/include/baxter_sim_hardware/baxter_emulator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Node emulating the Baxter hardware interfaces for simulation 33 | * commands 34 | */ 35 | 36 | #ifndef baxter_emulator_H_ 37 | #define baxter_emulator_H_ 38 | 39 | #include "ros/ros.h" 40 | #include 41 | #include 42 | #include 43 | 44 | //Baxter Specific Messages 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include 56 | 57 | //ROS-Opencv Headers 58 | #include 59 | #include 60 | #include 61 | 62 | #include 63 | #include 64 | #include 65 | 66 | #include 67 | #include 68 | #include 69 | 70 | namespace baxter_en { 71 | 72 | class baxter_emulator { 73 | 74 | public: 75 | /** 76 | * Method to initialize the default values for all the variables, instantiate the publishers and * subscribers 77 | * @param img_path that refers the path of the image that loads on start up 78 | */ 79 | baxter_emulator() { 80 | } 81 | bool init(); 82 | 83 | /** 84 | * Method to start the publishers 85 | * @param Nodehandle to initialize the image transport 86 | * @param img_path that refers the path of the image that loads on start up 87 | */ 88 | void publish(const std::string &img_path); 89 | 90 | private: 91 | bool enable; 92 | //Subscribers 93 | ros::Subscriber enable_sub, stop_sub, reset_sub, left_laser_sub, 94 | right_laser_sub, nav_light_sub, head_nod_sub, jnt_st; 95 | 96 | // Gripper Publishers 97 | ros::Publisher left_grip_st_pub, right_grip_st_pub, left_grip_prop_pub, 98 | right_grip_prop_pub; 99 | // Infrared publishers 100 | ros::Publisher left_ir_pub, right_ir_pub, left_ir_int_pub, right_ir_int_pub, 101 | left_ir_state_pub, right_ir_state_pub; 102 | // Navigator publishers 103 | ros::Publisher left_inner_light_pub, right_inner_light_pub, 104 | left_outer_light_pub, right_outer_light_pub, 105 | torso_left_inner_light_pub, torso_right_inner_light_pub, 106 | torso_left_outer_light_pub, torso_right_outer_light_pub; 107 | // General state publishers 108 | ros::Publisher assembly_state_pub, head_pub; 109 | // Gravity Publishers 110 | ros::Publisher left_grav_pub, right_grav_pub; 111 | // Simulator has Started notification Publisher 112 | ros::Publisher sim_started_pub; 113 | 114 | ros::NodeHandle n; 115 | ros::Timer head_nod_timer; 116 | 117 | baxter_core_msgs::HeadState head_msg; 118 | baxter_core_msgs::AssemblyState assembly_state; 119 | baxter_core_msgs::EndEffectorState left_grip_st, right_grip_st; 120 | baxter_core_msgs::EndEffectorProperties left_grip_prop, right_grip_prop; 121 | baxter_core_msgs::AnalogIOState left_ir_state, right_ir_state; 122 | baxter_core_msgs::DigitalIOState leftIL_nav_light, leftOL_nav_light, 123 | torso_leftIL_nav_light, torso_leftOL_nav_light, rightIL_nav_light, 124 | rightOL_nav_light, torso_rightIL_nav_light, torso_rightOL_nav_light; 125 | baxter_core_msgs::SEAJointState left_gravity, right_gravity; 126 | sensor_msgs::JointState jstate_msg; 127 | sensor_msgs::Range left_ir, right_ir; 128 | std_msgs::UInt32 left_ir_int, right_ir_int; 129 | 130 | bool isStopped; 131 | 132 | /** 133 | * Callback function to enable the robot 134 | */ 135 | void enable_cb(const std_msgs::Bool &msg); 136 | 137 | /** 138 | * Callback function to stop the robot and capture the source of the stop 139 | */ 140 | void stop_cb(const std_msgs::Empty &msg); 141 | 142 | /** 143 | * Callback function to reset all the state values to False and 0s 144 | */ 145 | void reset_cb(const std_msgs::Empty &msg); 146 | 147 | /** 148 | * Callback function to update the left laser values 149 | */ 150 | void left_laser_cb(const sensor_msgs::LaserScan &msg); 151 | 152 | /** 153 | * Callback function to update the right laser values 154 | */ 155 | void right_laser_cb(const sensor_msgs::LaserScan &msg); 156 | 157 | /** 158 | * Callback function to update the navigators' light values 159 | */ 160 | void nav_light_cb(const baxter_core_msgs::DigitalOutputCommand &msg); 161 | 162 | /** 163 | * Callback function to capture if the head is nodding 164 | */ 165 | void head_nod_cb(const std_msgs::Bool &msg); 166 | 167 | /** 168 | * Method that updates the gravity variable 169 | */ 170 | void update_jnt_st(const sensor_msgs::JointState &msg); 171 | 172 | void reset_head_nod(const ros::TimerEvent &t); 173 | 174 | }; 175 | } // namespace 176 | 177 | #endif /* BAXTER_EMULATOR_H_ */ 178 | -------------------------------------------------------------------------------- /baxter_sim_hardware/launch/baxter_sdk_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 35 | 36 | 38 | 39 | 41 | 42 | 43 | 45 | 46 | 47 | 48 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /baxter_sim_hardware/launch/baxter_sdk_position_rqt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /baxter_sim_hardware/launch/baxter_sdk_position_rqt.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_plot/Plot': [2, 1], u'rqt_publisher/Publisher': [1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_plot__Plot__1": { 13 | "keys": {}, 14 | "groups": { 15 | "dock_widget__DataPlotWidget": { 16 | "keys": { 17 | "dockable": { 18 | "type": "repr", 19 | "repr": "True" 20 | }, 21 | "parent": { 22 | "type": "repr", 23 | "repr": "None" 24 | } 25 | }, 26 | "groups": {} 27 | }, 28 | "plugin": { 29 | "keys": { 30 | "plot_type": { 31 | "type": "repr", 32 | "repr": "1" 33 | }, 34 | "topics": { 35 | "type": "repr", 36 | "repr": "[u'/robot/left_joint_position_controller/joints/left_s0_controller/state/error', u'/robot/left_joint_position_controller/joints/left_s1_controller/state/error']" 37 | } 38 | }, 39 | "groups": {} 40 | } 41 | } 42 | }, 43 | "plugin__rqt_plot__Plot__2": { 44 | "keys": {}, 45 | "groups": { 46 | "dock_widget__DataPlotWidget": { 47 | "keys": { 48 | "dockable": { 49 | "type": "repr", 50 | "repr": "True" 51 | }, 52 | "parent": { 53 | "type": "repr", 54 | "repr": "None" 55 | } 56 | }, 57 | "groups": {} 58 | }, 59 | "plugin": { 60 | "keys": { 61 | "plot_type": { 62 | "type": "repr", 63 | "repr": "1" 64 | }, 65 | "topics": { 66 | "type": "repr", 67 | "repr": "[u'/robot/left_joint_position_controller/joints/left_s0_controller/state/set_point', u'/robot/left_joint_position_controller/joints/left_s1_controller/state/set_point']" 68 | } 69 | }, 70 | "groups": {} 71 | } 72 | } 73 | }, 74 | "plugin__rqt_publisher__Publisher__1": { 75 | "keys": {}, 76 | "groups": { 77 | "dock_widget__PublisherWidget": { 78 | "keys": { 79 | "dockable": { 80 | "type": "repr", 81 | "repr": "True" 82 | }, 83 | "parent": { 84 | "type": "repr", 85 | "repr": "None" 86 | } 87 | }, 88 | "groups": {} 89 | }, 90 | "plugin": { 91 | "keys": { 92 | "publishers": { 93 | "type": "repr", 94 | "repr": "u'[{\\'type_name\\': \\'baxter_core_msgs/JointCommand\\', \\'topic_name\\': \\'/robot/left_joint_position_controller/command\\', \\'enabled\\': False, \\'rate\\': 100.0, \\'expressions\\': {u\\'/robot/left_joint_position_controller/command/angles\\': \\'[sin(i/100*3)*1.7016799388]\\', u\\'/robot/left_joint_position_controller/command/names\\': \"[\\'left_s0\\']\"}, \\'publisher_id\\': 0, \\'counter\\': 299}, {\\'type_name\\': \\'baxter_core_msgs/JointCommand\\', \\'topic_name\\': \\'/robot/left_joint_position_controller/command\\', \\'enabled\\': False, \\'rate\\': 100.0, \\'expressions\\': {u\\'/robot/left_joint_position_controller/command/angles\\': \\'[sin(i/100*3)*1.597-0.55]\\', u\\'/robot/left_joint_position_controller/command/names\\': \"[\\'left_s1\\']\"}, \\'publisher_id\\': 1, \\'counter\\': 188}]'" 95 | } 96 | }, 97 | "groups": {} 98 | } 99 | } 100 | } 101 | } 102 | }, 103 | "mainwindow": { 104 | "keys": { 105 | "geometry": { 106 | "type": "repr(QByteArray.hex)", 107 | "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000100000000068c00000018000010930000062b00000690000000340000108f00000627000000000000')", 108 | "pretty-print": " + 4 ' " 109 | }, 110 | "state": { 111 | "type": "repr(QByteArray.hex)", 112 | "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd000000010000000300000a00000005c5fc0100000001fc0000000000000a000000026600fffffffc0200000002fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c006900730068006500720057006900640067006500740100000019000001060000009600fffffffc00000125000004b90000007a00fffffffc0100000002fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f00740057006900640067006500740100000000000004c20000011500fffffffb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0032005f005f00440061007400610050006c006f007400570069006400670065007401000004c8000005380000011500ffffff00000a000000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 113 | "pretty-print": " f Brqt_plot__Plot__1__DataPlotWidget Brqt_plot__Plot__2__DataPlotWidget 6MinimizedDockWidgetsToolbar " 114 | } 115 | }, 116 | "groups": { 117 | "toolbar_areas": { 118 | "keys": { 119 | "MinimizedDockWidgetsToolbar": { 120 | "type": "repr", 121 | "repr": "8" 122 | } 123 | }, 124 | "groups": {} 125 | } 126 | } 127 | } 128 | } 129 | } 130 | -------------------------------------------------------------------------------- /baxter_sim_hardware/launch/baxter_sdk_velocity_rqt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /baxter_sim_hardware/launch/baxter_sdk_velocity_rqt.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_plot/Plot': [2, 1], u'rqt_publisher/Publisher': [1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_plot__Plot__1": { 13 | "keys": {}, 14 | "groups": { 15 | "dock_widget__DataPlotWidget": { 16 | "keys": { 17 | "dockable": { 18 | "type": "repr", 19 | "repr": "True" 20 | }, 21 | "parent": { 22 | "type": "repr", 23 | "repr": "None" 24 | } 25 | }, 26 | "groups": {} 27 | }, 28 | "plugin": { 29 | "keys": { 30 | "plot_type": { 31 | "type": "repr", 32 | "repr": "1" 33 | }, 34 | "topics": { 35 | "type": "repr", 36 | "repr": "[u'/robot/left_joint_velocity_controller/joints/left_s0_controller/state/error', u'/robot/left_joint_velocity_controller/joints/left_s1_controller/state/error']" 37 | } 38 | }, 39 | "groups": {} 40 | } 41 | } 42 | }, 43 | "plugin__rqt_plot__Plot__2": { 44 | "keys": {}, 45 | "groups": { 46 | "dock_widget__DataPlotWidget": { 47 | "keys": { 48 | "dockable": { 49 | "type": "repr", 50 | "repr": "True" 51 | }, 52 | "parent": { 53 | "type": "repr", 54 | "repr": "None" 55 | } 56 | }, 57 | "groups": {} 58 | }, 59 | "plugin": { 60 | "keys": { 61 | "plot_type": { 62 | "type": "repr", 63 | "repr": "1" 64 | }, 65 | "topics": { 66 | "type": "repr", 67 | "repr": "[u'/robot/left_joint_velocity_controller/joints/left_s0_controller/state/set_point', u'/robot/left_joint_velocity_controller/joints/left_s1_controller/state/set_point']" 68 | } 69 | }, 70 | "groups": {} 71 | } 72 | } 73 | }, 74 | "plugin__rqt_publisher__Publisher__1": { 75 | "keys": {}, 76 | "groups": { 77 | "dock_widget__PublisherWidget": { 78 | "keys": { 79 | "dockable": { 80 | "type": "repr", 81 | "repr": "True" 82 | }, 83 | "parent": { 84 | "type": "repr", 85 | "repr": "None" 86 | } 87 | }, 88 | "groups": {} 89 | }, 90 | "plugin": { 91 | "keys": { 92 | "publishers": { 93 | "type": "repr", 94 | "repr": "u'[{\\'type_name\\': \\'baxter_core_msgs/JointVelocities\\', \\'topic_name\\': \\'/robot/joint_positions_controller/command\\', \\'enabled\\': False, \\'rate\\': 100.0, \\'expressions\\': {u\\'/robot/joint_positions_controller/command/angles\\': \\'[sin(i/100*3)*1.7016799388]\\', u\\'/robot/joint_positions_controller/command/names\\': \"[\\'left_s0\\']\"}, \\'publisher_id\\': 0, \\'counter\\': 299}, {\\'type_name\\': \\'baxter_core_msgs/JointVelocities\\', \\'topic_name\\': \\'/robot/joint_positions_controller/command\\', \\'enabled\\': False, \\'rate\\': 100.0, \\'expressions\\': {u\\'/robot/joint_positions_controller/command/angles\\': \\'[sin(i/100*3)*1.597-0.55]\\', u\\'/robot/joint_positions_controller/command/names\\': \"[\\'left_s1\\']\"}, \\'publisher_id\\': 1, \\'counter\\': 188}]'" 95 | } 96 | }, 97 | "groups": {} 98 | } 99 | } 100 | } 101 | } 102 | }, 103 | "mainwindow": { 104 | "keys": { 105 | "geometry": { 106 | "type": "repr(QByteArray.hex)", 107 | "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000100000000068c00000018000010930000062b00000690000000340000108f00000627000000000000')", 108 | "pretty-print": " + 4 ' " 109 | }, 110 | "state": { 111 | "type": "repr(QByteArray.hex)", 112 | "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd000000010000000300000a00000005c5fc0100000001fc0000000000000a000000026600fffffffc0200000002fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c006900730068006500720057006900640067006500740100000019000001060000009600fffffffc00000125000004b90000007a00fffffffc0100000002fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f00740057006900640067006500740100000000000004c20000011500fffffffb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0032005f005f00440061007400610050006c006f007400570069006400670065007401000004c8000005380000011500ffffff00000a000000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 113 | "pretty-print": " f Brqt_plot__Plot__1__DataPlotWidget Brqt_plot__Plot__2__DataPlotWidget 6MinimizedDockWidgetsToolbar " 114 | } 115 | }, 116 | "groups": { 117 | "toolbar_areas": { 118 | "keys": { 119 | "MinimizedDockWidgetsToolbar": { 120 | "type": "repr", 121 | "repr": "8" 122 | } 123 | }, 124 | "groups": {} 125 | } 126 | } 127 | } 128 | } 129 | } 130 | -------------------------------------------------------------------------------- /baxter_sim_hardware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_sim_hardware 4 | 1.2.12 5 | 6 | Publishes the state information and emulates few hardware interfaces for use with Baxter 7 | in Gazebo 8 | 9 | 10 | 11 | Rethink Robotics Inc. 12 | 13 | BSD 14 | http://sdk.rethinkrobotics.com 15 | 16 | https://github.com/RethinkRobotics/baxter_simulator 17 | 18 | 19 | https://github.com/RethinkRobotics/baxter_simulator/issues 20 | 21 | Rethink Robotics Inc. 22 | Hariharasudan Malaichamee 23 | Dave Coleman 24 | 25 | 26 | catkin 27 | 28 | std_msgs 29 | tf 30 | roscpp 31 | baxter_core_msgs 32 | cv_bridge 33 | image_transport 34 | baxter_sim_kinematics 35 | cmake_modules 36 | libopencv-dev 37 | 38 | roscpp 39 | baxter_core_msgs 40 | cv_bridge 41 | image_transport 42 | baxter_sim_kinematics 43 | controller_manager 44 | libopencv-dev 45 | 46 | 47 | -------------------------------------------------------------------------------- /baxter_sim_io/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ############################################################################## 2 | # CMake 3 | ############################################################################## 4 | 5 | cmake_minimum_required(VERSION 2.8.0) 6 | project(baxter_sim_io) 7 | 8 | ############################################################################## 9 | # Catkin 10 | ############################################################################## 11 | 12 | # qt_build provides the qt cmake glue, roscpp the comms for a default talker 13 | #find_package(catkin REQUIRED COMPONENTS qt_build roscpp baxter_core_msgs) 14 | find_package(catkin REQUIRED COMPONENTS roscpp baxter_core_msgs) 15 | find_package(Qt4 REQUIRED COMPONENTS QtCore QtGui) 16 | include_directories(${catkin_INCLUDE_DIRS}) 17 | # Use this to define what the package will export (e.g. libs, headers). 18 | # Since the default here is to produce only a binary, we don't worry about 19 | # exporting anything. 20 | catkin_package(INCLUDE_DIRS include 21 | CATKIN_DEPENDS 22 | baxter_core_msgs 23 | INCLUDE_DIRS include 24 | LIBRARIES ${PROJECT_NAME} 25 | ) 26 | 27 | include_directories(include 28 | ${catkin_INCLUDE_DIRS} 29 | ${QT_INCLUDES} 30 | ) 31 | ############################################################################## 32 | # Qt Environment 33 | ############################################################################## 34 | 35 | # this comes from qt_build's qt-ros.cmake which is automatically 36 | # included via the dependency call in manifest.xml 37 | 38 | ############################################################################## 39 | # Sections 40 | ############################################################################## 41 | include(${QT_USE_FILE}) 42 | include_directories(${CMAKE_CURRENT_BINARY_DIR}) 43 | 44 | file(GLOB QT_FORMS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ui/*.ui) 45 | file(GLOB QT_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} resources/*.qrc) 46 | file(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/baxter_sim_io/*.hpp) 47 | 48 | QT4_ADD_RESOURCES(QT_RESOURCES_CPP ${QT_RESOURCES}) 49 | QT4_WRAP_UI(QT_FORMS_HPP ${QT_FORMS}) 50 | QT4_WRAP_CPP(QT_MOC_HPP ${QT_MOC}) 51 | 52 | ############################################################################## 53 | # Sources 54 | ############################################################################## 55 | 56 | file(GLOB_RECURSE QT_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS src/*.cpp) 57 | 58 | ############################################################################## 59 | # Binaries 60 | ############################################################################## 61 | 62 | add_executable(baxter_sim_io ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP}) 63 | target_link_libraries(baxter_sim_io ${QT_LIBRARIES} ${ROS_LIBRARIES} ${catkin_LIBRARIES}) 64 | 65 | add_dependencies(baxter_sim_io baxter_core_msgs_gencpp) 66 | 67 | install(TARGETS baxter_sim_io 68 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 69 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 71 | 72 | install( 73 | DIRECTORY include resources ui 74 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 75 | ) 76 | -------------------------------------------------------------------------------- /baxter_sim_io/include/baxter_sim_io/baxter_io.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Includes the action listener events for the QT controls 33 | */ 34 | 35 | #ifndef BAXTER_SIM_IO_BAXTER_IO_H 36 | #define BAXTER_SIM_IO_BAXTER_IO_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include "ui_baxter_io.h" 42 | #include "qnode.hpp" 43 | 44 | namespace baxter_sim_io { 45 | 46 | class BaxterIO : public QMainWindow { 47 | Q_OBJECT 48 | 49 | public: 50 | BaxterIO(int argc, char** argv, QWidget *parent = 0); 51 | ~BaxterIO(); 52 | 53 | private: 54 | void setstyle(std::string ,std::string , QPushButton&); 55 | 56 | private Q_SLOTS: 57 | void on_left_arm_ok_pressed(); 58 | void on_left_arm_ok_released(); 59 | void on_left_arm_cancel_pressed(); 60 | void on_left_arm_cancel_released(); 61 | void on_left_arm_dial_sliderMoved(int position); 62 | void on_left_arm_show_pressed(); 63 | void on_left_arm_show_released(); 64 | void on_left_cuff_squeeze_pressed(); 65 | void on_left_cuff_squeeze_released(); 66 | void on_left_cuff_ok_pressed(); 67 | void on_left_cuff_ok_released(); 68 | void on_left_cuff_grasp_pressed(); 69 | void on_left_cuff_grasp_released(); 70 | void on_left_shoulder_ok_pressed(); 71 | void on_left_shoulder_ok_released(); 72 | void on_left_shoulder_cancel_pressed(); 73 | void on_left_shoulder_cancel_released(); 74 | void on_left_shoulder_show_pressed(); 75 | void on_left_shoulder_show_released(); 76 | void on_left_shoulder_dial_sliderMoved(int position); 77 | void on_right_shoulder_ok_pressed(); 78 | void on_right_shoulder_ok_released(); 79 | void on_right_shoulder_cancel_pressed(); 80 | void on_right_shoulder_cancel_released(); 81 | void on_right_shoulder_dial_sliderMoved(int position); 82 | void on_right_shoulder_show_pressed(); 83 | void on_right_shoulder_show_released(); 84 | void on_right_arm_ok_pressed(); 85 | void on_right_arm_ok_released(); 86 | void on_right_arm_cancel_pressed(); 87 | void on_right_arm_cancel_released(); 88 | void on_right_arm_dial_sliderMoved(int position); 89 | void on_right_arm_show_pressed(); 90 | void on_right_arm_show_released(); 91 | void on_right_cuff_squeeze_pressed(); 92 | void on_right_cuff_squeeze_released(); 93 | void on_right_cuff_ok_pressed(); 94 | void on_right_cuff_ok_released(); 95 | void on_right_cuff_grasp_pressed(); 96 | void on_right_cuff_grasp_released(); 97 | 98 | private: 99 | Ui::BaxterIO *ui; 100 | QNode qnode; 101 | }; 102 | 103 | } // namespace baxter_sim_io 104 | 105 | #endif // BAXTER_SIM_IO_BAXTER_IO_H 106 | -------------------------------------------------------------------------------- /baxter_sim_io/include/baxter_sim_io/qnode.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc ROS node that publishes the states captured from the QT events 33 | */ 34 | #ifndef baxter_sim_io_QNODE_HPP_ 35 | #define baxter_sim_io_QNODE_HPP_ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace baxter_sim_io { 45 | 46 | class QNode : public QThread { 47 | Q_OBJECT 48 | public: 49 | QNode(int argc, char** argv); 50 | virtual ~QNode(); 51 | bool init(); 52 | void run(); 53 | static baxter_core_msgs::NavigatorState left_arm_nav, right_arm_nav, 54 | left_shoulder_nav, right_shoulder_nav; 55 | static baxter_core_msgs::DigitalIOState left_cuff_squeeze, right_cuff_squeeze, 56 | left_cuff_ok, right_cuff_ok, left_cuff_grasp, right_cuff_grasp, 57 | left_shoulder, right_shoulder; 58 | 59 | void rosShutdown(); 60 | 61 | private: 62 | int init_argc; 63 | char** init_argv; 64 | ros::Publisher left_navigator, right_navigator, torso_left_navigator, torso_right_navigator, 65 | left_lower_cuff, right_lower_cuff, left_lower_button, right_lower_button, 66 | left_upper_button, right_upper_button, left_shoulder_button, 67 | right_shoulder_button; 68 | ros::Subscriber left_inner_light_sub, left_outer_light_sub, 69 | right_inner_light_sub, right_outer_light_sub, 70 | torso_left_inner_light_sub, torso_left_outer_light_sub, 71 | torso_right_inner_light_sub, torso_right_outer_light_sub; 72 | void left_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 73 | void left_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 74 | void right_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 75 | void right_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 76 | void torso_left_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 77 | void torso_left_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 78 | void torso_right_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 79 | void torso_right_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg); 80 | 81 | }; 82 | 83 | } // namespace baxter_sim_io 84 | 85 | #endif /* baxter_sim_io_QNODE_HPP_ */ 86 | -------------------------------------------------------------------------------- /baxter_sim_io/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_sim_io 4 | 1.2.12 5 | 6 | The Navigator buttons for the Baxter Research Robot are simulated, and their states are captured and published on the corresponding rostopics 7 | 8 | Rethink Robotics Inc. 9 | Hariharasudan Malaichamee 10 | BSD 11 | https://github.com/RethinkRobotics/baxter_simulator/issues 12 | 13 | catkin 14 | 15 | roscpp 16 | libqt4-dev 17 | baxter_core_msgs 18 | 19 | roscpp 20 | libqt4-dev 21 | baxter_core_msgs 22 | 23 | 24 | -------------------------------------------------------------------------------- /baxter_sim_io/resources/arm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/arm.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff_grasp_p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff_grasp_p.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff_grasp_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff_grasp_r.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff_ok_p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff_ok_p.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff_ok_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff_ok_r.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff_squeeze_p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff_squeeze_p.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/cuff_squeeze_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/cuff_squeeze_r.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/left.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/logo.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav_cancel_p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav_cancel_p.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav_cancel_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav_cancel_r.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav_ok_p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav_ok_p.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav_ok_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav_ok_r.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav_show_p.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav_show_p.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/nav_show_r.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/nav_show_r.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/right.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/robot_arm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/robot_arm.png -------------------------------------------------------------------------------- /baxter_sim_io/resources/sim_io.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | torso.png 4 | arm.png 5 | cuff_squeeze_p.png 6 | cuff_squeeze_r.png 7 | cuff.png 8 | left.png 9 | logo.png 10 | nav_ok_p.png 11 | nav_ok_r.png 12 | nav_show_p.png 13 | nav_show_r.png 14 | nav.png 15 | right.png 16 | robot_arm.png 17 | cuff_grasp_p.png 18 | cuff_grasp_r.png 19 | cuff_ok_p.png 20 | cuff_ok_r.png 21 | nav_cancel_p.png 22 | nav_cancel_r.png 23 | 24 | 25 | -------------------------------------------------------------------------------- /baxter_sim_io/resources/torso.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RethinkRobotics/baxter_simulator/46d01dc9bf22965799ab03db464858fea570fada/baxter_sim_io/resources/torso.png -------------------------------------------------------------------------------- /baxter_sim_io/src/baxter_io.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Includes the action listener events for the QT controls 33 | */ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace baxter_sim_io { 41 | 42 | using namespace Qt; 43 | 44 | void BaxterIO::setstyle(std::string pressed, std::string released, QPushButton& button) 45 | { 46 | 47 | QPixmap pix(QString::fromUtf8(released.c_str())); 48 | button.setMask(pix.mask()); 49 | button.setStyleSheet("QPushButton{background-image: url("+QString::fromUtf8(released.c_str())+");\n" 50 | "background-position: center;\n" 51 | "background-color: transparent;\n" 52 | "background-repeat: none;\n" 53 | "border: none;}\n" 54 | "QPushButton:pressed{background-image: url("+QString::fromUtf8(pressed.c_str())+");}"); 55 | } 56 | 57 | BaxterIO::BaxterIO(int argc, char** argv, QWidget *parent) 58 | : QMainWindow(parent), 59 | qnode(argc, argv), 60 | ui(new Ui::BaxterIO) { 61 | ui->setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class. 62 | 63 | std::string cancel_r=":/new/prefix/nav_cancel_r.png"; 64 | std::string cancel_p=":/new/prefix/nav_cancel_p.png"; 65 | std::string ok_r=":/new/prefix/nav_ok_r.png"; 66 | std::string ok_p=":/new/prefix/nav_ok_p.png"; 67 | std::string show_r=":/new/prefix/nav_show_r.png"; 68 | std::string show_p=":/new/prefix/nav_show_p.png"; 69 | 70 | std::string cuff_grasp_r=":/new/prefix/cuff_grasp_r.png"; 71 | std::string cuff_grasp_p=":/new/prefix/cuff_grasp_p.png"; 72 | std::string cuff_ok_r=":/new/prefix/cuff_ok_r.png"; 73 | std::string cuff_ok_p=":/new/prefix/cuff_ok_p.png"; 74 | std::string cuff_squeeze_r=":/new/prefix/cuff_squeeze_r.png"; 75 | std::string cuff_squeeze_p=":/new/prefix/cuff_squeeze_p.png"; 76 | 77 | //Configure the stylesheet for all the Qwidgets 78 | //left arm 79 | setstyle(cancel_p,cancel_r,*(ui->left_arm_cancel)); 80 | setstyle(ok_p,ok_r,*(ui->left_arm_ok)); 81 | setstyle(show_p,show_r,*(ui->left_arm_show)); 82 | //left shoulder 83 | setstyle(cancel_p,cancel_r,*(ui->left_shoulder_cancel)); 84 | setstyle(ok_p,ok_r,*(ui->left_shoulder_ok)); 85 | setstyle(show_p,show_r,*(ui->left_shoulder_show)); 86 | //left cuff 87 | setstyle(cuff_grasp_p,cuff_grasp_r,*(ui->left_cuff_grasp)); 88 | setstyle(cuff_ok_p,cuff_ok_r,*(ui->left_cuff_ok)); 89 | setstyle(cuff_squeeze_p,cuff_squeeze_r,*(ui->left_cuff_squeeze)); 90 | 91 | //right arm 92 | setstyle(cancel_p,cancel_r,*(ui->right_arm_cancel)); 93 | setstyle(ok_p,ok_r,*(ui->right_arm_ok)); 94 | setstyle(show_p,show_r,*(ui->right_arm_show)); 95 | //right shoulder 96 | setstyle(cancel_p,cancel_r,*(ui->right_shoulder_cancel)); 97 | setstyle(ok_p,ok_r,*(ui->right_shoulder_ok)); 98 | setstyle(show_p,show_r,*(ui->right_shoulder_show)); 99 | //right cuff 100 | setstyle(cuff_grasp_p,cuff_grasp_r,*(ui->right_cuff_grasp)); 101 | setstyle(cuff_ok_p,cuff_ok_r,*(ui->right_cuff_ok)); 102 | setstyle(cuff_squeeze_p,cuff_squeeze_r,*(ui->right_cuff_squeeze)); 103 | } 104 | 105 | BaxterIO::~BaxterIO() { 106 | delete ui; 107 | } 108 | 109 | void BaxterIO::on_left_arm_ok_pressed() { 110 | QNode::left_arm_nav.buttons[0] = true; 111 | } 112 | 113 | void BaxterIO::on_left_arm_ok_released() { 114 | QNode::left_arm_nav.buttons[0] = false; 115 | } 116 | 117 | void BaxterIO::on_left_arm_cancel_pressed() { 118 | QNode::left_arm_nav.buttons[1] = true; 119 | } 120 | 121 | void BaxterIO::on_left_arm_cancel_released() { 122 | QNode::left_arm_nav.buttons[1] = false; 123 | } 124 | 125 | void BaxterIO::on_left_arm_dial_sliderMoved(int position) { 126 | QNode::left_arm_nav.wheel = position; 127 | } 128 | 129 | void BaxterIO::on_left_arm_show_pressed() { 130 | QNode::left_arm_nav.buttons[2] = true; 131 | } 132 | 133 | void BaxterIO::on_left_arm_show_released() { 134 | QNode::left_arm_nav.buttons[2] = false; 135 | } 136 | 137 | void BaxterIO::on_left_cuff_squeeze_pressed() { 138 | QNode::left_cuff_squeeze.state = baxter_core_msgs::DigitalIOState::PRESSED; 139 | } 140 | 141 | void BaxterIO::on_left_cuff_squeeze_released() { 142 | QNode::left_cuff_squeeze.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 143 | } 144 | 145 | void BaxterIO::on_left_cuff_ok_pressed() { 146 | QNode::left_cuff_ok.state = baxter_core_msgs::DigitalIOState::PRESSED; 147 | } 148 | 149 | void BaxterIO::on_left_cuff_ok_released() { 150 | QNode::left_cuff_ok.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 151 | } 152 | 153 | void BaxterIO::on_left_cuff_grasp_pressed() { 154 | QNode::left_cuff_grasp.state = baxter_core_msgs::DigitalIOState::PRESSED; 155 | } 156 | 157 | void BaxterIO::on_left_cuff_grasp_released() { 158 | QNode::left_cuff_grasp.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 159 | } 160 | 161 | void BaxterIO::on_left_shoulder_ok_pressed() { 162 | QNode::left_shoulder_nav.buttons[0] = true; 163 | } 164 | 165 | void BaxterIO::on_left_shoulder_ok_released() { 166 | QNode::left_shoulder_nav.buttons[0] = false; 167 | } 168 | 169 | void BaxterIO::on_left_shoulder_cancel_pressed() { 170 | QNode::left_shoulder_nav.buttons[1] = true; 171 | } 172 | 173 | void BaxterIO::on_left_shoulder_cancel_released() { 174 | QNode::left_shoulder_nav.buttons[1] = false; 175 | } 176 | 177 | void BaxterIO::on_left_shoulder_show_pressed() { 178 | QNode::left_shoulder_nav.buttons[2] = true; 179 | } 180 | 181 | void BaxterIO::on_left_shoulder_show_released() { 182 | QNode::left_shoulder_nav.buttons[2] = false; 183 | } 184 | 185 | void BaxterIO::on_left_shoulder_dial_sliderMoved(int position) { 186 | QNode::left_shoulder_nav.wheel = position; 187 | } 188 | 189 | void BaxterIO::on_right_shoulder_ok_pressed() { 190 | QNode::right_shoulder_nav.buttons[0] = true; 191 | } 192 | 193 | void BaxterIO::on_right_shoulder_ok_released() { 194 | QNode::right_shoulder_nav.buttons[0] = false; 195 | } 196 | 197 | void BaxterIO::on_right_shoulder_cancel_pressed() { 198 | QNode::right_shoulder_nav.buttons[1] = true; 199 | } 200 | 201 | void BaxterIO::on_right_shoulder_cancel_released() { 202 | QNode::right_shoulder_nav.buttons[1] = false; 203 | } 204 | 205 | void BaxterIO::on_right_shoulder_dial_sliderMoved(int position) { 206 | QNode::right_shoulder_nav.wheel = position; 207 | } 208 | 209 | void BaxterIO::on_right_shoulder_show_pressed() { 210 | QNode::right_shoulder_nav.buttons[2] = true; 211 | } 212 | 213 | void BaxterIO::on_right_shoulder_show_released() { 214 | QNode::right_shoulder_nav.buttons[2] = false; 215 | } 216 | 217 | void BaxterIO::on_right_arm_ok_pressed() { 218 | QNode::right_arm_nav.buttons[0] = true; 219 | } 220 | 221 | void BaxterIO::on_right_arm_ok_released() { 222 | QNode::right_arm_nav.buttons[0] = false; 223 | } 224 | 225 | void BaxterIO::on_right_arm_cancel_pressed() { 226 | QNode::right_arm_nav.buttons[1] = true; 227 | } 228 | 229 | void BaxterIO::on_right_arm_cancel_released() { 230 | QNode::right_arm_nav.buttons[1] = false; 231 | } 232 | 233 | void BaxterIO::on_right_arm_dial_sliderMoved(int position) { 234 | QNode::right_arm_nav.wheel = position; 235 | } 236 | 237 | void BaxterIO::on_right_arm_show_pressed() { 238 | QNode::right_arm_nav.buttons[2] = true; 239 | } 240 | 241 | void BaxterIO::on_right_arm_show_released() { 242 | QNode::right_arm_nav.buttons[2] = false; 243 | } 244 | 245 | void BaxterIO::on_right_cuff_squeeze_pressed() { 246 | QNode::right_cuff_squeeze.state = baxter_core_msgs::DigitalIOState::PRESSED; 247 | } 248 | 249 | void BaxterIO::on_right_cuff_squeeze_released() { 250 | QNode::right_cuff_squeeze.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 251 | } 252 | 253 | void BaxterIO::on_right_cuff_ok_pressed() { 254 | QNode::right_cuff_ok.state = baxter_core_msgs::DigitalIOState::PRESSED; 255 | } 256 | 257 | void BaxterIO::on_right_cuff_ok_released() { 258 | QNode::right_cuff_ok.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 259 | } 260 | 261 | void BaxterIO::on_right_cuff_grasp_pressed() { 262 | QNode::right_cuff_grasp.state = baxter_core_msgs::DigitalIOState::PRESSED; 263 | } 264 | 265 | void BaxterIO::on_right_cuff_grasp_released() { 266 | QNode::right_cuff_grasp.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 267 | } 268 | 269 | } // namespace baxter_sim_io 270 | -------------------------------------------------------------------------------- /baxter_sim_io/src/main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | */ 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | void signalhandler(int sig) { 41 | if (sig == SIGINT) { 42 | qApp->quit(); 43 | } 44 | else if (sig == SIGTERM) { 45 | qApp->quit(); 46 | } 47 | } 48 | 49 | int main(int argc, char **argv) { 50 | 51 | QApplication app(argc, argv); 52 | baxter_sim_io::BaxterIO w(argc,argv); 53 | //Register Signal handler for ctrl+c 54 | signal(SIGINT,signalhandler); 55 | signal(SIGTERM,signalhandler); 56 | w.show(); 57 | app.connect(&app, SIGNAL(lastWindowClosed()), &app, SLOT(quit())); 58 | 59 | int result = app.exec(); 60 | return result; 61 | } 62 | 63 | baxter_core_msgs::NavigatorState baxter_sim_io::QNode::left_arm_nav, baxter_sim_io::QNode::right_arm_nav, 64 | baxter_sim_io::QNode::left_shoulder_nav, baxter_sim_io::QNode::right_shoulder_nav; 65 | 66 | baxter_core_msgs::DigitalIOState baxter_sim_io::QNode::left_cuff_squeeze, 67 | baxter_sim_io::QNode::right_cuff_squeeze, baxter_sim_io::QNode::left_cuff_ok, 68 | baxter_sim_io::QNode::right_cuff_ok, baxter_sim_io::QNode::left_cuff_grasp, 69 | baxter_sim_io::QNode::right_cuff_grasp, baxter_sim_io::QNode::left_shoulder, 70 | baxter_sim_io::QNode::right_shoulder; 71 | -------------------------------------------------------------------------------- /baxter_sim_io/src/qnode.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc ROS node that publishes the states captured from the QT events 33 | */ 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace baxter_sim_io { 43 | 44 | const std::string BAXTER_LEFTIL_TOPIC = 45 | "robot/digital_io/left_inner_light/state"; 46 | const std::string BAXTER_LEFTOL_TOPIC = 47 | "robot/digital_io/left_outer_light/state"; 48 | const std::string BAXTER_TORSO_LEFTIL_TOPIC = 49 | "robot/digital_io/torso_left_inner_light/state"; 50 | const std::string BAXTER_TORSO_LEFTOL_TOPIC = 51 | "robot/digital_io/torso_left_outer_light/state"; 52 | const std::string BAXTER_RIGHTIL_TOPIC = 53 | "robot/digital_io/right_inner_light/state"; 54 | const std::string BAXTER_RIGHTOL_TOPIC = 55 | "robot/digital_io/right_outer_light/state"; 56 | const std::string BAXTER_TORSO_RIGHTIL_TOPIC = 57 | "robot/digital_io/torso_right_inner_light/state"; 58 | const std::string BAXTER_TORSO_RIGHTOL_TOPIC = 59 | "robot/digital_io/torso_right_outer_light/state"; 60 | 61 | QNode::QNode(int argc, char** argv) 62 | : init_argc(argc), 63 | init_argv(argv) { 64 | init(); 65 | } 66 | 67 | QNode::~QNode() { 68 | if (ros::isStarted()) { 69 | ros::shutdown(); // explicitly needed since we use ros::start(); 70 | ros::waitForShutdown(); 71 | } 72 | wait(); 73 | } 74 | 75 | void quit(int sig) 76 | { 77 | ros::shutdown(); 78 | exit(0); 79 | } 80 | 81 | bool QNode::init() { 82 | ros::init(init_argc, init_argv, "baxter_sim_io"); 83 | if (!ros::master::check()) { 84 | return false; 85 | } 86 | ros::start(); // explicitly needed since our nodehandle is going out of scope. 87 | ros::NodeHandle n; 88 | 89 | left_navigator = n.advertise( 90 | "/robot/navigators/left_navigator/state", 1); 91 | right_navigator = n.advertise( 92 | "/robot/navigators/right_navigator/state", 1); 93 | torso_left_navigator = n.advertise( 94 | "/robot/navigators/torso_left_navigator/state", 1); 95 | torso_right_navigator = n.advertise( 96 | "/robot/navigators/torso_right_navigator/state", 1); 97 | 98 | left_lower_cuff = n.advertise( 99 | "/robot/digital_io/left_lower_cuff/state", 1); 100 | right_lower_cuff = n.advertise( 101 | "/robot/digital_io/right_lower_cuff/state", 1); 102 | 103 | left_lower_button = n.advertise( 104 | "/robot/digital_io/left_lower_button/state", 1); 105 | right_lower_button = n.advertise( 106 | "/robot/digital_io/right_lower_button/state", 1); 107 | 108 | left_upper_button = n.advertise( 109 | "/robot/digital_io/left_upper_button/state", 1); 110 | right_upper_button = n.advertise( 111 | "/robot/digital_io/right_upper_button/state", 1); 112 | left_shoulder_button = n.advertise( 113 | "/robot/digital_io/left_shoulder_button/state", 1); 114 | right_shoulder_button = n.advertise( 115 | "/robot/digital_io/right_shoulder_button/state", 1); 116 | 117 | QNode::left_arm_nav.button_names.push_back("ok"); 118 | QNode::left_arm_nav.button_names.push_back("back"); 119 | QNode::left_arm_nav.button_names.push_back("show"); 120 | QNode::left_arm_nav.buttons.push_back(false); 121 | QNode::left_arm_nav.buttons.push_back(false); 122 | QNode::left_arm_nav.buttons.push_back(false); 123 | QNode::left_arm_nav.light_names.push_back("inner"); 124 | QNode::left_arm_nav.light_names.push_back("outer"); 125 | QNode::left_arm_nav.lights.push_back(false); 126 | QNode::left_arm_nav.lights.push_back(false); 127 | QNode::left_arm_nav.wheel = 0; 128 | 129 | QNode::right_arm_nav.button_names.push_back("ok"); 130 | QNode::right_arm_nav.button_names.push_back("back"); 131 | QNode::right_arm_nav.button_names.push_back("show"); 132 | QNode::right_arm_nav.buttons.push_back(false); 133 | QNode::right_arm_nav.buttons.push_back(false); 134 | QNode::right_arm_nav.buttons.push_back(false); 135 | QNode::right_arm_nav.light_names.push_back("inner"); 136 | QNode::right_arm_nav.light_names.push_back("outer"); 137 | QNode::right_arm_nav.lights.push_back(false); 138 | QNode::right_arm_nav.lights.push_back(false); 139 | QNode::right_arm_nav.wheel = 0; 140 | 141 | QNode::left_shoulder_nav.button_names.push_back("ok"); 142 | QNode::left_shoulder_nav.button_names.push_back("back"); 143 | QNode::left_shoulder_nav.button_names.push_back("show"); 144 | QNode::left_shoulder_nav.buttons.push_back(false); 145 | QNode::left_shoulder_nav.buttons.push_back(false); 146 | QNode::left_shoulder_nav.buttons.push_back(false); 147 | QNode::left_shoulder_nav.light_names.push_back("inner"); 148 | QNode::left_shoulder_nav.light_names.push_back("outer"); 149 | QNode::left_shoulder_nav.lights.push_back(false); 150 | QNode::left_shoulder_nav.lights.push_back(false); 151 | QNode::left_shoulder_nav.wheel = 0; 152 | 153 | QNode::right_shoulder_nav.button_names.push_back("ok"); 154 | QNode::right_shoulder_nav.button_names.push_back("back"); 155 | QNode::right_shoulder_nav.button_names.push_back("show"); 156 | QNode::right_shoulder_nav.buttons.push_back(false); 157 | QNode::right_shoulder_nav.buttons.push_back(false); 158 | QNode::right_shoulder_nav.buttons.push_back(false); 159 | QNode::right_shoulder_nav.light_names.push_back("inner"); 160 | QNode::right_shoulder_nav.light_names.push_back("outer"); 161 | QNode::right_shoulder_nav.lights.push_back(false); 162 | QNode::right_shoulder_nav.lights.push_back(false); 163 | QNode::right_shoulder_nav.wheel = 0; 164 | 165 | QNode::left_cuff_squeeze.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 166 | QNode::left_cuff_squeeze.isInputOnly = true; 167 | QNode::right_cuff_squeeze.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 168 | QNode::right_cuff_squeeze.isInputOnly = true; 169 | QNode::left_cuff_ok.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 170 | QNode::left_cuff_ok.isInputOnly = true; 171 | QNode::right_cuff_ok.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 172 | QNode::right_cuff_ok.isInputOnly = true; 173 | QNode::left_cuff_grasp.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 174 | QNode::left_cuff_grasp.isInputOnly = true; 175 | QNode::right_cuff_grasp.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 176 | QNode::right_cuff_grasp.isInputOnly = true; 177 | QNode::left_shoulder.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 178 | QNode::left_shoulder.isInputOnly = true; 179 | QNode::right_shoulder.state = baxter_core_msgs::DigitalIOState::UNPRESSED; 180 | QNode::right_shoulder.isInputOnly = true; 181 | 182 | left_inner_light_sub = n.subscribe(BAXTER_LEFTIL_TOPIC, 100, 183 | &QNode::left_inner_light_sub_cb, this); 184 | left_outer_light_sub = n.subscribe(BAXTER_LEFTOL_TOPIC, 100, 185 | &QNode::left_outer_light_sub_cb, this); 186 | right_inner_light_sub = n.subscribe(BAXTER_RIGHTIL_TOPIC, 100, 187 | &QNode::right_inner_light_sub_cb, this); 188 | right_outer_light_sub = n.subscribe(BAXTER_RIGHTOL_TOPIC, 100, 189 | &QNode::right_outer_light_sub_cb, this); 190 | torso_left_inner_light_sub = n.subscribe(BAXTER_TORSO_LEFTIL_TOPIC, 100, 191 | &QNode::torso_left_inner_light_sub_cb, this); 192 | torso_left_outer_light_sub = n.subscribe(BAXTER_TORSO_LEFTOL_TOPIC, 100, 193 | &QNode::torso_left_outer_light_sub_cb, this); 194 | torso_right_inner_light_sub = n.subscribe(BAXTER_TORSO_RIGHTIL_TOPIC, 100, 195 | &QNode::torso_right_inner_light_sub_cb, this); 196 | torso_right_outer_light_sub = n.subscribe(BAXTER_TORSO_RIGHTOL_TOPIC, 100, 197 | &QNode::torso_right_outer_light_sub_cb, this); 198 | start(); 199 | return true; 200 | } 201 | 202 | void QNode::left_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 203 | QNode::left_arm_nav.lights[0] = bool(msg.state); 204 | } 205 | void QNode::left_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 206 | QNode::left_arm_nav.lights[1] = bool(msg.state); 207 | } 208 | void QNode::right_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 209 | QNode::right_arm_nav.lights[0] = bool(msg.state); 210 | } 211 | void QNode::right_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 212 | QNode::right_arm_nav.lights[1] = bool(msg.state); 213 | } 214 | void QNode::torso_left_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 215 | QNode::left_shoulder_nav.lights[0] = bool(msg.state); 216 | } 217 | void QNode::torso_left_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 218 | QNode::left_shoulder_nav.lights[1] = bool(msg.state); 219 | } 220 | void QNode::torso_right_inner_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 221 | QNode::right_shoulder_nav.lights[0] = bool(msg.state); 222 | } 223 | void QNode::torso_right_outer_light_sub_cb(const baxter_core_msgs::DigitalIOState &msg){ 224 | QNode::right_shoulder_nav.lights[1] = bool(msg.state); 225 | } 226 | 227 | void QNode::run() { 228 | ros::Rate loop_rate(100); 229 | while (ros::ok()) { 230 | 231 | left_navigator.publish(QNode::left_arm_nav); 232 | right_navigator.publish(QNode::right_arm_nav); 233 | torso_left_navigator.publish(QNode::left_shoulder_nav); 234 | torso_right_navigator.publish(QNode::right_shoulder_nav); 235 | 236 | left_lower_cuff.publish(QNode::left_cuff_squeeze); 237 | right_lower_cuff.publish(QNode::right_cuff_squeeze); 238 | 239 | left_lower_button.publish(QNode::left_cuff_ok); 240 | right_lower_button.publish(QNode::right_cuff_ok); 241 | 242 | left_upper_button.publish(QNode::left_cuff_grasp); 243 | right_upper_button.publish(QNode::right_cuff_grasp); 244 | 245 | left_shoulder_button.publish(QNode::left_shoulder); 246 | right_shoulder_button.publish(QNode::right_shoulder); 247 | ros::spinOnce(); 248 | loop_rate.sleep(); 249 | } 250 | ROS_INFO("Ros shutdown, proceeding to close the gui."); 251 | } 252 | 253 | } // namespace baxter_sim_io 254 | -------------------------------------------------------------------------------- /baxter_sim_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(baxter_sim_kinematics) 3 | 4 | find_package(catkin 5 | REQUIRED COMPONENTS 6 | roscpp 7 | baxter_core_msgs 8 | sensor_msgs 9 | tf 10 | tf_conversions 11 | kdl_parser 12 | gazebo_msgs 13 | ) 14 | 15 | find_package(cmake_modules REQUIRED) 16 | find_package(Eigen REQUIRED) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | CATKIN_DEPENDS 21 | std_msgs 22 | baxter_core_msgs 23 | gazebo_msgs 24 | sensor_msgs 25 | kdl_parser 26 | LIBRARIES ${PROJECT_NAME} 27 | ) 28 | 29 | include_directories(include 30 | ${catkin_INCLUDE_DIRS} 31 | ${EIGEN_INCLUDE_DIRS} 32 | ) 33 | 34 | add_library(${PROJECT_NAME} src/arm_kinematics.cpp) 35 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 36 | 37 | add_executable(kinematics src/position_kinematics.cpp) 38 | target_link_libraries(kinematics ${catkin_LIBRARIES} ${PROJECT_NAME}) 39 | 40 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 41 | add_dependencies(kinematics 42 | ${catkin_EXPORTED_TARGETS} 43 | baxter_core_msgs_gencpp 44 | ) 45 | 46 | install( 47 | TARGETS ${PROJECT_NAME} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | ) 50 | 51 | install( 52 | TARGETS kinematics 53 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 54 | ) 55 | 56 | install( 57 | DIRECTORY launch 58 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 59 | ) 60 | 61 | install(DIRECTORY include/${PROJECT_NAME}/ 62 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 63 | ) 64 | -------------------------------------------------------------------------------- /baxter_sim_kinematics/include/baxter_sim_kinematics/arm_kinematics.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc library that performs the calculations for the IK,FK and Gravity compensation using the KDL library 33 | */ 34 | 35 | #ifndef ARM_KINEMATICS_H_ 36 | #define ARM_KINEMATICS_H_ 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | namespace arm_kinematics { 58 | 59 | //Structures to pass the messages 60 | typedef struct INFO { 61 | std::vector link_names; 62 | std::vector joint_names; 63 | } KinematicSolverInfo; 64 | 65 | class Kinematics { 66 | public: 67 | Kinematics(); 68 | bool init_grav(); 69 | 70 | /* Initializes the solvers and the other variables required 71 | * @returns true is successful 72 | */ 73 | bool init(std::string tip_name, int &no_jts); 74 | typedef boost::shared_ptr Ptr; 75 | static Ptr create(std::string tip_name, int &no_jts) { 76 | Ptr parm_kinematics = Ptr(new Kinematics()); 77 | if (parm_kinematics->init(tip_name, no_jts)) { 78 | return parm_kinematics; 79 | } 80 | return Ptr(); 81 | } 82 | 83 | /* Method to calculate the IK for the required end pose 84 | * @returns true if successful 85 | */ 86 | bool getPositionIK(const geometry_msgs::PoseStamped &pose_stamp, 87 | const sensor_msgs::JointState &seed, 88 | sensor_msgs::JointState *result); 89 | 90 | /* Method to calculate the FK for the required joint configuration 91 | * @returns true if successful 92 | */ 93 | bool getPositionFK(std::string frame_id, 94 | const sensor_msgs::JointState &joint_configuration, 95 | geometry_msgs::PoseStamped &res); 96 | 97 | /* Method to calculate the torques required to apply at each of the joints for gravity compensation 98 | * @returns true is successful 99 | */ 100 | //bool getGravityTorques(const sensor_msgs::JointState &joint_configuration, 101 | //std::vector &torquesOut); 102 | bool getGravityTorques(const sensor_msgs::JointState joint_configuration, baxter_core_msgs::SEAJointState &left_gravity, baxter_core_msgs::SEAJointState &right_gravity, bool isEnabled); 103 | 104 | private: 105 | ros::NodeHandle nh, nh_private; 106 | std::string root_name, tip_name, grav_left_name, grav_right_name; 107 | KDL::JntArray joint_min, joint_max; 108 | KDL::Chain chain, grav_chain_l, grav_chain_r; 109 | unsigned int num_joints; 110 | 111 | KDL::ChainFkSolverPos_recursive* fk_solver; 112 | KDL::ChainIkSolverPos_NR_JL *ik_solver_pos; 113 | KDL::ChainIkSolverVel_pinv* ik_solver_vel; 114 | KDL::ChainIdSolver_RNE *gravity_solver_l, *gravity_solver_r; 115 | 116 | ros::ServiceServer ik_service, ik_solver_info_service; 117 | ros::ServiceServer fk_service, fk_solver_info_service; 118 | 119 | tf::TransformListener tf_listener; 120 | std::vector indd; 121 | std::vector left_joint, right_joint; 122 | KinematicSolverInfo info, grav_info_l, grav_info_r; 123 | 124 | /* Method to load all the values from the parameter server 125 | * @returns true is successful 126 | */ 127 | bool loadModel(const std::string xml); 128 | 129 | /* Method to read the URDF model and extract the joints 130 | * @returns true is successful 131 | */ 132 | bool readJoints(urdf::Model &robot_model); 133 | 134 | /* Method to calculate the Joint index of a particular joint from the KDL chain 135 | * @returns the index of the joint 136 | */ 137 | int getJointIndex(const std::string &name); 138 | 139 | /* Method to calculate the KDL segment index of a particular segment from the KDL chain 140 | * @returns the index of the segment 141 | */ 142 | int getKDLSegmentIndex(const std::string &name); 143 | }; 144 | 145 | } 146 | #endif 147 | -------------------------------------------------------------------------------- /baxter_sim_kinematics/include/baxter_sim_kinematics/position_kinematics.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Node to wrap/unwrap the messages and calculate the kinematics for the simulated Baxter 33 | */ 34 | 35 | #ifndef POSITION_KINEMATICS_H_ 36 | #define POSITION_KINEMATICS_H_ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | namespace kinematics { 50 | class position_kinematics { 51 | protected: 52 | position_kinematics() { 53 | } 54 | ; 55 | 56 | bool init(std::string side); 57 | 58 | public: 59 | 60 | //! return types of create() and createOnStack() 61 | typedef boost::shared_ptr poskin_ptr; 62 | 63 | /** 64 | * Factory method that creates a new instance of position_kinematics(), 65 | * calls init and returns initialized non-NULL pointer if init succeeds. 66 | * Returns an empty pointer when init fails. Design pattern expects that 67 | * we always check to see if a pointer is not NULL before assuming we can 68 | * use it. 69 | * 70 | * @return boost::shared_ptr 71 | */ 72 | static poskin_ptr create(std::string side) { 73 | poskin_ptr pk_ptr = poskin_ptr(new position_kinematics()); 74 | if (pk_ptr->init(side)) { 75 | return pk_ptr; 76 | } 77 | return poskin_ptr(); 78 | } 79 | 80 | /** 81 | * Method that serves as the main execution loop of the Node. This is called in the main() function to 'run' 82 | * and only returns after exit or ros::shutdown is called. 83 | */ 84 | void run() { 85 | //just do spin here (blocks until shutdown), remove while loop 86 | ros::spin(); 87 | 88 | //we have left the ros spin loop, clean up (if needed) then shutdown 89 | exit(); 90 | 91 | //attempt proper shutdown 92 | ros::shutdown(); 93 | } 94 | 95 | /** 96 | * Method that allows signals (from their main function) to trigger any 97 | * cleanup and manually exit the node's run loop. 98 | * This is usually triggered by capturing a SIGTERM, etc. 99 | */ 100 | void exit() { 101 | //Do anything to shut down cleanly 102 | //Note: Run loop will call shutdown before exiting 103 | 104 | m_ikService.shutdown(); 105 | } 106 | 107 | private: 108 | /** 109 | * Callback function that checks and sets the robot enabled flag 110 | */ 111 | void stateCB(const baxter_core_msgs::AssemblyState msg); 112 | 113 | /** 114 | * Method to pass the desired configuration of the joints and calculate the FK 115 | * @return calculated FK pose 116 | */ 117 | geometry_msgs::PoseStamped FKCalc(const sensor_msgs::JointState req); 118 | 119 | /** 120 | * Callback function for the IK service that responds with the appropriate joint configuration or error message if not found 121 | */ 122 | bool IKCallback(baxter_core_msgs::SolvePositionIK::Request &req, 123 | baxter_core_msgs::SolvePositionIK::Response &res); 124 | 125 | /** 126 | * Callback function for the FK subscriber that retrievs the appropriate FK from the Joint states and publishes it to the endpoint 127 | * topic 128 | */ 129 | void FKCallback(const sensor_msgs::JointState msg); 130 | 131 | /** 132 | * Method to Filter the names and positions of the initialized side from the remaining 133 | */ 134 | void FilterJointState(const sensor_msgs::JointState *msg, 135 | sensor_msgs::JointState &res); 136 | 137 | bool is_enabled; 138 | std::string m_limbName; 139 | ros::ServiceServer m_ikService; 140 | arm_kinematics::Kinematics::Ptr m_kinematicsModel; 141 | ros::Subscriber joint_states_sub, robot_state_sub; 142 | ros::Publisher end_pointstate_pub; 143 | sensor_msgs::JointState joint; 144 | ros::NodeHandle handle; 145 | std::string tip_name; 146 | std::vector joint_names; 147 | int no_jts; 148 | 149 | }; 150 | 151 | } 152 | 153 | #endif /* POSITION_KINEMATICS_H_ */ 154 | -------------------------------------------------------------------------------- /baxter_sim_kinematics/launch/baxter_sim_kinematics.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | [right_s0, right_s1, right_e0, right_e1, right_w0, right_w1, right_w2] 24 | 25 | 26 | [left_s0, left_s1, left_e0, left_e1, left_w0, left_w1, left_w2] 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /baxter_sim_kinematics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_sim_kinematics 4 | 1.2.12 5 | 6 | Baxter Kinematics for the FK, IK and the gravity compensation calculations 7 | 8 | 9 | 10 | Rethink Robotics Inc. 11 | 12 | BSD 13 | http://sdk.rethinkrobotics.com 14 | 15 | https://github.com/RethinkRobotics/baxter_simulator 16 | 17 | 18 | https://github.com/RethinkRobotics/baxter_simulator/issues 19 | 20 | Rethink Robotics Inc. 21 | Hariharasudan Malaichamee 22 | 23 | catkin 24 | baxter_core_msgs 25 | gazebo_msgs 26 | sensor_msgs 27 | kdl_parser 28 | tf 29 | tf_conversions 30 | cmake_modules 31 | 32 | moveit_ros_planning_interface 33 | std_msgs 34 | roscpp 35 | kdl_parser 36 | baxter_core_msgs 37 | gazebo_msgs 38 | sensor_msgs 39 | 40 | tf 41 | sensor_msgs 42 | tf_conversions 43 | 44 | kdl_parser 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /baxter_sim_kinematics/src/position_kinematics.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | # Copyright (c) 2013-2015, Rethink Robotics 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 3. Neither the name of the Rethink Robotics nor the names of its 14 | # contributors may be used to endorse or promote products derived from 15 | # this software without specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | *********************************************************************/ 29 | 30 | /** 31 | * \author Hariharasudan Malaichamee 32 | * \desc Node to wrap/unwrap the messages and calculate the kinematics for the Simulated Baxter 33 | */ 34 | 35 | #include 36 | #include 37 | 38 | namespace kinematics { 39 | 40 | static const std::string ref_frame_id = "base"; 41 | static const std::string JOINT_STATES = "/robot/joint_states"; 42 | static const std::string ROBOT_STATE = "/robot/state"; 43 | const int joint_id=6;//Modify this to get the FK of different joints 44 | 45 | /** 46 | * Method to initialize the publishing and subscribing topics, services and to acquire the resources required 47 | * @return true if succeeds. 48 | */ 49 | bool position_kinematics::init(std::string side) { 50 | //Robot would be disabled initially 51 | is_enabled = false; 52 | 53 | // capture the side we are working on 54 | m_limbName = side; 55 | 56 | //setup handle for the topics 57 | std::string node_path = "/ExternalTools/" + m_limbName 58 | + "/PositionKinematicsNode"; 59 | ros::NodeHandle handle1(node_path); 60 | 61 | static const std::string LIMB_ENDPOINT = "/robot/limb/" + side + "/endpoint_state"; 62 | 63 | //setup the service server for the Inverse Kinematics 64 | m_ikService = handle1.advertiseService("IKService", 65 | &position_kinematics::IKCallback, 66 | this); 67 | 68 | //Subscribe and advertise the subscribers and publishers accordingly for the Forward Kinematics 69 | joint_states_sub = handle.subscribe < sensor_msgs::JointState 70 | > (JOINT_STATES, 100, &position_kinematics::FKCallback, this); 71 | end_pointstate_pub = handle.advertise < baxter_core_msgs::EndpointState 72 | > (LIMB_ENDPOINT, 100); 73 | 74 | //Subscribe to the robot state 75 | robot_state_sub = handle.subscribe < baxter_core_msgs::AssemblyState 76 | > (ROBOT_STATE, 100, &position_kinematics::stateCB, this); 77 | 78 | //Initialize the Parameter server with the root_name and tip_name of the Kinematic Chain based on the side 79 | if (side == "right") { 80 | if (!handle.getParam("right_tip_name", tip_name)) { 81 | ROS_FATAL("GenericIK: No tip name for Right arm found on parameter server"); 82 | return false; 83 | } 84 | if (!handle.getParam("robot_config/right_config/joint_names", joint_names)) { 85 | ROS_FATAL("GenericIK: No Joint Names for the Right Arm found on parameter server"); 86 | return false; 87 | } 88 | } 89 | else { 90 | if (!handle.getParam("left_tip_name", tip_name)) { 91 | ROS_FATAL("GenericIK: No tip name for Right arm found on parameter server"); 92 | return false; 93 | } 94 | if (!handle.getParam("robot_config/left_config/joint_names", joint_names)) { 95 | ROS_FATAL("GenericIK: No Joint Names for the Left Arm found on parameter server"); 96 | return false; 97 | } 98 | } 99 | no_jts = joint_names.size(); 100 | m_kinematicsModel = arm_kinematics::Kinematics::create(tip_name, no_jts); 101 | return true; 102 | 103 | } 104 | 105 | /** 106 | * Callback function that checks and sets the robot enabled flag 107 | */ 108 | void position_kinematics::stateCB( 109 | const baxter_core_msgs::AssemblyState msg) { 110 | if (msg.enabled) 111 | is_enabled = true; 112 | else 113 | is_enabled = false; 114 | } 115 | 116 | /** 117 | * Callback function for the FK subscriber that retrievs the appropriate FK from the Joint states and publishes 118 | * to the endpoint_state topic 119 | */ 120 | void position_kinematics::FKCallback(const sensor_msgs::JointState msg) { 121 | baxter_core_msgs::EndpointState endpoint; 122 | 123 | sensor_msgs::JointState configuration; 124 | 125 | position_kinematics::FilterJointState(&msg, joint); 126 | //Copy the current Joint positions and names of the appropriate side to the configuration 127 | endpoint.pose = position_kinematics::FKCalc(joint).pose; 128 | 129 | //Fill out timestamp for endpoint 130 | endpoint.header.stamp = msg.header.stamp; 131 | 132 | //Publish the PoseStamp of the end effector 133 | end_pointstate_pub.publish(endpoint); 134 | } 135 | 136 | /** 137 | * Method to Filter the names and positions of the initialized side from the remaining 138 | */ 139 | void position_kinematics::FilterJointState( 140 | const sensor_msgs::JointState *msg, sensor_msgs::JointState &res) { 141 | // Resize the result to hold the names and positions of the 7 joints 142 | res.name.resize(joint_names.size()); 143 | res.position.resize(joint_names.size()); 144 | int i = 0; 145 | for (size_t ind = 0; ind < msg->name.size(); ind++) { 146 | // Retain the names and positions of the joints of the initialized arm 147 | if (std::find(joint_names.begin(), joint_names.end(), msg->name[ind]) != joint_names.end()) { 148 | res.name[i] = msg->name[ind]; 149 | res.position[i] = msg->position[ind]; 150 | i++; 151 | } 152 | } 153 | } 154 | 155 | /** 156 | * Method to pass the desired configuration of the joints and calculate the FK 157 | * @return calculated FK 158 | */ 159 | geometry_msgs::PoseStamped position_kinematics::FKCalc( 160 | const sensor_msgs::JointState configuration) { 161 | bool isV; 162 | geometry_msgs::PoseStamped fk_result; 163 | isV = m_kinematicsModel->getPositionFK(ref_frame_id, configuration, 164 | fk_result); 165 | return fk_result; 166 | } 167 | 168 | /** 169 | * Callback function for the IK service that responds with the appropriate joint configuration or error message if not found 170 | */ 171 | bool position_kinematics::IKCallback( 172 | baxter_core_msgs::SolvePositionIK::Request &req, 173 | baxter_core_msgs::SolvePositionIK::Response &res) { 174 | ros::Rate loop_rate(100); 175 | sensor_msgs::JointState joint_pose; 176 | res.joints.resize(req.pose_stamp.size()); 177 | res.isValid.resize(req.pose_stamp.size()); 178 | res.result_type.resize(req.pose_stamp.size()); 179 | for (size_t req_index = 0; req_index < req.pose_stamp.size(); req_index++) { 180 | 181 | res.isValid[req_index]=0; 182 | int valid_inp=0; 183 | 184 | if(!req.seed_angles.empty() && req.seed_mode != baxter_core_msgs::SolvePositionIKRequest::SEED_CURRENT) { 185 | res.isValid[req_index] = m_kinematicsModel->getPositionIK( 186 | req.pose_stamp[req_index], req.seed_angles[req_index], &joint_pose); 187 | res.joints[req_index].name.resize(joint_pose.name.size()); 188 | res.result_type[req_index]=baxter_core_msgs::SolvePositionIKRequest::SEED_USER; 189 | valid_inp=1; 190 | } 191 | 192 | if((!res.isValid[req_index]) && req.seed_mode != baxter_core_msgs::SolvePositionIKRequest::SEED_USER) { 193 | res.isValid[req_index] = m_kinematicsModel->getPositionIK( 194 | req.pose_stamp[req_index], joint, &joint_pose); 195 | res.joints[req_index].name.resize(joint_pose.name.size()); 196 | res.result_type[req_index]=baxter_core_msgs::SolvePositionIKRequest::SEED_CURRENT; 197 | valid_inp=1; 198 | } 199 | 200 | if(!valid_inp) { 201 | ROS_ERROR("Not a valid request message to the IK service"); 202 | return false; 203 | } 204 | 205 | if (res.isValid[req_index]) { 206 | res.joints[req_index].position.resize(joint_pose.position.size()); 207 | res.joints[req_index].name = joint_pose.name; 208 | res.joints[req_index].position = joint_pose.position; 209 | } 210 | else 211 | res.result_type[req_index]=baxter_core_msgs::SolvePositionIKResponse::RESULT_INVALID; 212 | } 213 | loop_rate.sleep(); 214 | } 215 | 216 | } //namespace 217 | /***************************************************************************************************/ 218 | 219 | //! global pointer to Node 220 | kinematics::position_kinematics::poskin_ptr g_pNode; 221 | 222 | //! Helper function for 223 | void quitRequested(int) { 224 | ROS_INFO("position_kinematics: Terminating program..."); 225 | if (g_pNode) { 226 | g_pNode->exit(); 227 | g_pNode.reset(); 228 | } 229 | } 230 | 231 | /** 232 | * Entry point for program. Sets up Node, parses 233 | * command line arguments, then control loop (calling run() on Node) 234 | */ 235 | int main(int argc, char* argv[]) { 236 | std::string side = argc > 1 ? argv[1] : ""; 237 | if (side != "left" && side != "right") { 238 | fprintf(stderr, "Usage: %s \n", argv[0]); 239 | return 1; 240 | } 241 | ros::init(argc, argv, "baxter_sim_kinematics_" + side); 242 | 243 | //capture signals and attempt to cleanup Node 244 | signal(SIGTERM, quitRequested); 245 | signal(SIGINT, quitRequested); 246 | signal(SIGHUP, quitRequested); 247 | 248 | g_pNode = kinematics::position_kinematics::create(side); 249 | 250 | //test to see if pointer is valid 251 | if (g_pNode) { 252 | g_pNode->run(); 253 | } 254 | 255 | //position_kinematics calls ros::shutdown upon exit, just return here 256 | return 0; 257 | } 258 | 259 | -------------------------------------------------------------------------------- /baxter_simulator.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: baxter_simulator 3 | uri: https://github.com/RethinkRobotics/baxter_simulator.git 4 | version: master 5 | - git: 6 | local-name: baxter 7 | uri: https://github.com/RethinkRobotics/baxter.git 8 | version: master 9 | - git: 10 | local-name: baxter_interface 11 | uri: https://github.com/RethinkRobotics/baxter_interface.git 12 | version: master 13 | - git: 14 | local-name: baxter_tools 15 | uri: https://github.com/RethinkRobotics/baxter_tools.git 16 | version: master 17 | - git: 18 | local-name: baxter_common 19 | uri: https://github.com/RethinkRobotics/baxter_common.git 20 | version: master 21 | - git: 22 | local-name: baxter_examples 23 | uri: https://github.com/RethinkRobotics/baxter_examples.git 24 | version: master 25 | -------------------------------------------------------------------------------- /baxter_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(baxter_simulator) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /baxter_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | baxter_simulator 4 | 1.2.12 5 | 6 | Metapackage - Baxter Research Robot Gazebo Simulation 7 | 8 | 9 | 10 | Rethink Robotics Inc. 11 | 12 | BSD 13 | http://sdk.rethinkrobotics.com 14 | 15 | https://github.com/RethinkRobotics/baxter_simultor 16 | 17 | 18 | https://github.com/RethinkRobotics/baxter_simulator/issues 19 | 20 | Rethink Robotics Inc. 21 | 22 | catkin 23 | baxter_gazebo 24 | baxter_sim_controllers 25 | baxter_sim_examples 26 | baxter_sim_hardware 27 | baxter_sim_io 28 | baxter_sim_kinematics 29 | 30 | 31 | 32 | 33 | 34 | 35 | --------------------------------------------------------------------------------