├── .gitignore ├── README.md ├── inmoov_bringup ├── CMakeLists.txt ├── LICENSE.txt ├── README.txt ├── config │ └── config.yaml ├── examples │ ├── blank.yaml │ ├── default.yaml │ ├── grey.yaml │ └── rosie.yaml ├── include │ ├── constants.py │ ├── export_yaml.py │ ├── load_config_from_param.py │ └── servos.py ├── launch │ └── bringup.launch ├── nodes │ ├── joint_command_dispatcher.py │ ├── joint_status_dispatcher.py │ └── motor_status_dispatcher.py ├── package.xml ├── scripts │ ├── generate_config_xacro.py │ └── generate_export_yaml_from_eeprom.py └── tools │ ├── enable_manager │ ├── enable_manager.py │ └── enable_manager.ui │ ├── rviz_manager │ ├── rviz_manager.py │ └── rviz_manager.ui │ └── trainer │ ├── trainer.py │ └── trainer.ui ├── inmoov_description ├── CMakeLists.txt ├── LICENSE.txt ├── launch │ ├── display.launch │ ├── gazebo.launch │ └── moveit-display.launch ├── launch_rviz.sh ├── package.xml ├── robots │ ├── inmoov.urdf.xacro │ ├── inmoov_face.urdf.xacro │ ├── inmoov_head.urdf.xacro │ └── inmoov_right_hand.urdf.xacro ├── urdf.rviz └── urdf │ ├── asmArm.urdf.xacro │ ├── asmBase.urdf.xacro │ ├── asmBicepServo.urdf.xacro │ ├── asmEye.urdf.xacro │ ├── asmFace.urdf.xacro │ ├── asmHand.urdf.xacro │ ├── asmHead.urdf.xacro │ ├── asmNeckFrontServo.urdf.xacro │ ├── asmNeckSideServo.urdf.xacro │ ├── asmTorso.urdf.xacro │ ├── asmTorsoServo.urdf.xacro │ ├── config.inertial.urdf.xacro │ ├── config.joints.urdf.xacro │ ├── inmoov.gazebo │ └── materials.urdf.xacro ├── inmoov_firmware ├── CMakeLists.txt ├── LICENSE.txt ├── TeensyServo.cpp ├── TeensyServo.h ├── configuration.h ├── eeprom.h ├── inmoov_firmware.ino └── package.xml ├── inmoov_meshes ├── CMakeLists.txt ├── LICENSE.txt ├── meshes │ ├── bicep.stl │ ├── bicepcover.stl │ ├── camera.stl │ ├── chest.stl │ ├── disk.stl │ ├── earleftv1.stl │ ├── earrightv1.stl │ ├── eye.stl │ ├── eyesupport.stl │ ├── face.stl │ ├── head.stl │ ├── head_base.stl │ ├── index3_1.stl │ ├── index3_2.stl │ ├── index3_3.stl │ ├── iris.stl │ ├── jaw.stl │ ├── kinectone.stl │ ├── l_cover_hand.stl │ ├── l_cover_handpinky.stl │ ├── l_cover_handring.stl │ ├── l_cover_index.stl │ ├── l_cover_middle.stl │ ├── l_cover_pinky.stl │ ├── l_cover_ring.stl │ ├── l_cover_thumb.stl │ ├── l_eyesupport.stl │ ├── l_forearm.stl │ ├── l_hand.stl │ ├── l_pinky3_1.stl │ ├── l_ring3_1.stl │ ├── l_shoulder.stl │ ├── l_shoulder_base.stl │ ├── l_thumb5_1.stl │ ├── launch_rviz.sh │ ├── mid_stomach.stl │ ├── middle3_1.stl │ ├── middle3_2.stl │ ├── middle3_3.stl │ ├── pinky3_2.stl │ ├── pinky3_3.stl │ ├── pinky3_4.stl │ ├── r_cover_hand.stl │ ├── r_cover_handpinky.stl │ ├── r_cover_handring.stl │ ├── r_cover_index.stl │ ├── r_cover_middle.stl │ ├── r_cover_pinky.stl │ ├── r_cover_ring.stl │ ├── r_cover_thumb.stl │ ├── r_eyesupport.stl │ ├── r_forearm.stl │ ├── r_hand.stl │ ├── r_pinky3_1.stl │ ├── r_ring3_1.stl │ ├── r_shoulder.stl │ ├── r_shoulder_base.stl │ ├── r_thumb5_1.stl │ ├── ring3_2.stl │ ├── ring3_3.stl │ ├── ring3_4.stl │ ├── skull.stl │ ├── thumb5_2.stl │ ├── thumb5_3.stl │ ├── top_stomach.stl │ ├── torso.stl │ └── virtual.stl └── package.xml ├── inmoov_msgs ├── CMakeLists.txt ├── LICENSE.txt ├── msg │ ├── MotorCommand.msg │ ├── MotorStatus.msg │ └── SmartServoStatus.msg ├── package.xml └── srv │ └── MotorParameter.srv └── inmoov_tools ├── CMakeLists.txt ├── LICENSE.txt ├── headtracker ├── head_pointer_gui.py ├── point_head.py ├── point_head_1.py ├── point_head_2.py ├── point_head_3.py ├── pub_3d_target.py ├── publish_point_target.py ├── room_watcher.py └── rviz_goal.py ├── launch ├── demo.launch ├── dev.launch ├── kinect-demo.launch ├── kinect-full-demo.launch ├── makercon-demo.launch ├── params.yaml ├── righteye.launch ├── sergei-help.launch ├── servobus.launch ├── trainer-stack.launch ├── videoserver.launch └── webcams.launch ├── look ├── look.py └── look.ui └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | /CMakeLists.txt 2 | ~* 3 | *~ 4 | .o 5 | .pyc 6 | 7 | .vscode/ 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | **The xenial-kinetic branch is currently being restructured to make it easier to adapt to custom InMoov designs.** 2 | 3 | Please check the xenial-kinetic milestones to see the direction we're heading and our status 4 | https://github.com/alansrobotlab/inmoov_ros/milestones/xenial_kinetic 5 | 6 | These are massive changes to the framework, but once it's all in, it will be alot easier to set up ROS with your specific robot. Stay tuned! 7 | 8 | Until then, once you have the packages installed, the following commands should work: 9 | - roslaunch inmoov_description display.launch (to pull up the rviz urdf model) 10 | - (the rest is currently hard coded to one robot, we're working on that) 11 | 12 | ## Alan's InMoov ROS Introduction 13 | ![enter image description here](http://i.imgur.com/bweApZH.png) 14 | 15 | 16 | --------- 17 | 18 | ### What Is It? 19 | This is a ROS software stack that connects a dedicated PC to an InMoov robot. 20 | It currently implements the following: 21 | 22 | - a fully articulated URDF model of InMoov 23 | - inmoov firmware that integrates rossserial_arduino for communication 24 | - inmoov_msgs that define communication between host pc and arduino 25 | - trainer module to set arduino eeprom values and calibrate each servo 26 | 27 | ### What You Need To Get Started 28 | > This works with a specific technology stack. It can be run natively on a PC, or in VMWare Player. Everything is tied to compatibility with MoveIt!. 29 | > 30 | > MoveIt! is currently available for ROS Indigo, and Indigo is tied to Ubuntu 14.04 LTS, so everything fits together from there. 31 | > 32 | > As soon as they release MoveIt! packages for ROS Kinetic, the stack will be updated to Ubuntu 16.04 + ROS Kinetic + MoveIt! 33 | 34 | #### What you'll need: 35 | 36 | - VMWare Player 37 | (https://my.vmware.com/en/web/vmware/free#desktop_end_user_computing/vmware_workstation_player/12_0) 38 | - Ubuntu 14.04 LTS (http://www.ubuntu.com/download/alternative-downloads) 39 | - ROS Indigo (http://wiki.ros.org/indigo/Installation/Ubuntu) 40 | - MoveIt! (http://moveit.ros.org/install/) 41 | 42 | ### How to Install It: 43 | Copy these packages into your {inmoov_ros}/src folder 44 | Run the following commands from the root of your {inmoov_ros} folder: 45 | 46 | catkin_make #build/rebuild all projects 47 | source devel/setup.bash #let ROS know about all of your new packages 48 | 49 | ### How to use it: 50 | Run the following commands: 51 | 52 | rosrun inmoov_tools set_parameters.py #loads parameters into server 53 | roslaunch inmoov_tools servobus.launch #launches arduino interfaces 54 | roslaunch inmoov_description display.launch #launches rviz 55 | rosrun inmoov_tools trainer.py #launches trainer module 56 | 57 | 58 | ### Next Steps (todo list) 59 | - urdf: fix right shoulder out rotation 60 | - urdf: fix right wrist rotation 61 | - trainer: move all arduino communication to service calls 62 | - trainer: only publish joint commands to joint_command topic 63 | - node: write node that sends joint commands to arduino through service calls 64 | - pose: migrate pose module to pyqt4 65 | - headdemo: migrate headdemo module to pyqt4 66 | -------------------------------------------------------------------------------- /inmoov_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(inmoov_bringup) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES inmoov_bringup 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | 116 | ## Declare a C++ library 117 | # add_library(${PROJECT_NAME} 118 | # src/${PROJECT_NAME}/inmoov_bringup.cpp 119 | # ) 120 | 121 | ## Add cmake target dependencies of the library 122 | ## as an example, code may need to be generated before libraries 123 | ## either from message generation or dynamic reconfigure 124 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 125 | 126 | ## Declare a C++ executable 127 | ## With catkin_make all packages are built within a single CMake context 128 | ## The recommended prefix ensures that target names across packages don't collide 129 | # add_executable(${PROJECT_NAME}_node src/inmoov_bringup_node.cpp) 130 | 131 | ## Rename C++ executable without prefix 132 | ## The above recommended prefix causes long target names, the following renames the 133 | ## target back to the shorter version for ease of user use 134 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 135 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 136 | 137 | ## Add cmake target dependencies of the executable 138 | ## same as for the library above 139 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Specify libraries to link a library or executable target against 142 | # target_link_libraries(${PROJECT_NAME}_node 143 | # ${catkin_LIBRARIES} 144 | # ) 145 | 146 | ############# 147 | ## Install ## 148 | ############# 149 | 150 | # all install targets should use catkin DESTINATION variables 151 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 152 | 153 | ## Mark executable scripts (Python etc.) for installation 154 | ## in contrast to setup.py, you can choose the destination 155 | # install(PROGRAMS 156 | # scripts/my_python_script 157 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 158 | # ) 159 | 160 | ## Mark executables and/or libraries for installation 161 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 162 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark cpp header files for installation 168 | # install(DIRECTORY include/${PROJECT_NAME}/ 169 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 170 | # FILES_MATCHING PATTERN "*.h" 171 | # PATTERN ".svn" EXCLUDE 172 | # ) 173 | 174 | ## Mark other files for installation (e.g. launch and bag files, etc.) 175 | # install(FILES 176 | # # myfile1 177 | # # myfile2 178 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 179 | # ) 180 | 181 | ############# 182 | ## Testing ## 183 | ############# 184 | 185 | ## Add gtest based cpp test target and link libraries 186 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_bringup.cpp) 187 | # if(TARGET ${PROJECT_NAME}-test) 188 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 189 | # endif() 190 | 191 | ## Add folders to be run by python nosetests 192 | # catkin_add_nosetests(test) 193 | -------------------------------------------------------------------------------- /inmoov_bringup/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Alan Timm (alansrobotlab) 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /inmoov_bringup/README.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_bringup/README.txt -------------------------------------------------------------------------------- /inmoov_bringup/include/constants.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed as BSD-3 3 | 4 | from enum import Enum 5 | 6 | class PROTOCOL(): 7 | #Protocol Constants 8 | 9 | ENABLE = 0x18 10 | GOAL = 0x1E 11 | 12 | SERVOPIN = 0x0A 13 | SENSORPIN = 0x0B 14 | 15 | MINGOAL = 0x06 16 | MAXGOAL = 0x08 17 | REST = 0x09 18 | MINPULSE = 0x14 19 | MAXPULSE = 0x16 20 | MINSENSOR = 0xA2 21 | MAXSENSOR = 0xA4 22 | MAXSPEED = 0x0C 23 | 24 | GOALSPEED = 0x20 25 | CALIBRATED = 0xA0 26 | LED = 0x19 27 | SMOOTHING = 0xA6 28 | 29 | PRESENTPOSITION = 0x24 30 | PRESENTSPEED = 0x26 31 | MOVING = 0x2E 32 | SENSORRAW = 0xA8 33 | POWER = 0x2A 34 | -------------------------------------------------------------------------------- /inmoov_bringup/include/export_yaml.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed as BSD-3 3 | 4 | import sys 5 | import rospy 6 | import yaml 7 | import os 8 | from os.path import dirname, abspath 9 | 10 | #hacky way to add include directory to sys path 11 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 12 | 13 | from constants import PROTOCOL 14 | from servos import Servo 15 | 16 | def export_yaml(filename): 17 | 18 | outfile = os.path.join(dirname(dirname(abspath(__file__))),'config',filename) 19 | 20 | print outfile 21 | 22 | with open(outfile, 'w') as export: 23 | export.write('bringup:\n') 24 | export.write(str(' angles:').ljust(20) + rospy.get_param('/bringup/angles')+ '\n') 25 | export.write(str(' hz:').ljust(20) + str(rospy.get_param('/bringup/hz')) +'\n') 26 | export.write('\n') 27 | export.write('joints:\n') 28 | export.write('\n') 29 | 30 | for name in rospy.get_param('/joints'): 31 | print "updating yaml for: " + name 32 | key = '/joints/' + name + '/' 33 | 34 | export.write(str(' ' + name + ':').ljust(20) + '\n') 35 | val = rospy.get_param(key + 'bus') 36 | export.write(str(' bus:').ljust(20) + str(int(val)) + '\n') 37 | val = rospy.get_param(key + 'servoPin') 38 | export.write(str(' servoPin:').ljust(20) + str(int(val)) + '\n') 39 | val = rospy.get_param(key + 'minPulse') 40 | export.write(str(' minPulse:').ljust(20) + str(int(val)) + '\n') 41 | val = rospy.get_param(key + 'maxPulse') 42 | export.write(str(' maxPulse:').ljust(20) + str(int(val)) + '\n') 43 | val = rospy.get_param(key + 'minGoal') 44 | export.write(str(' minGoal:').ljust(20) + str(val) + '\n') 45 | val = rospy.get_param(key + 'maxGoal') 46 | export.write(str(' maxGoal:').ljust(20) + str(val) + '\n') 47 | val = rospy.get_param(key + 'rest') 48 | export.write(str(' rest:').ljust(20) + str(val) + '\n') 49 | val = rospy.get_param(key + 'maxSpeed') 50 | export.write(str(' maxSpeed:').ljust(20) + str(val) + '\n') 51 | val = rospy.get_param(key + 'smoothing') 52 | export.write(str(' smoothing:').ljust(20) + str(int(val)) + '\n') 53 | val = rospy.get_param(key + 'sensorPin') 54 | export.write(str(' sensorPin:').ljust(20) + str(int(val)) + '\n') 55 | val = rospy.get_param(key + 'minSensor') 56 | export.write(str(' minSensor:').ljust(20) + str(int(val)) + '\n') 57 | val = rospy.get_param(key + 'maxSensor') 58 | export.write(str(' maxSensor:').ljust(20) + str(int(val)) + '\n') 59 | 60 | export.write('\n') 61 | 62 | export.close() 63 | 64 | print "DONE" -------------------------------------------------------------------------------- /inmoov_bringup/include/load_config_from_param.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed under BSD-3 3 | 4 | import sys 5 | import rospy 6 | import yaml 7 | import os 8 | from os.path import dirname, abspath 9 | 10 | 11 | #hacky way to add include directory to sys path 12 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 13 | 14 | from constants import PROTOCOL 15 | from servos import Servo 16 | 17 | servos = {} 18 | 19 | def load_config_from_param(): 20 | 21 | # first, make sure parameter server is even loaded 22 | while not rospy.search_param("/joints"): 23 | rospy.loginfo("waiting for parameter server to load with joint definitions") 24 | rospy.sleep(1) 25 | 26 | rospy.sleep(1) 27 | 28 | joints = rospy.get_param('/joints') 29 | for name in joints: 30 | rospy.loginfo( "found: " + name ) 31 | 32 | s = Servo() 33 | 34 | key = '/joints/' + name + '/' 35 | 36 | s.bus = rospy.get_param(key + 'bus') 37 | 38 | s.servoPin = rospy.get_param(key + 'servoPin') 39 | s.minPulse = rospy.get_param(key + 'minPulse') 40 | s.maxPulse = rospy.get_param(key + 'maxPulse') 41 | s.minGoal = rospy.get_param(key + 'minGoal') 42 | s.maxGoal = rospy.get_param(key + 'maxGoal') 43 | s.rest = rospy.get_param(key + 'rest') 44 | s.maxSpeed = rospy.get_param(key + 'maxSpeed') 45 | s.smoothing = rospy.get_param(key + 'smoothing') 46 | 47 | s.sensorpin = rospy.get_param(key + 'sensorPin') 48 | s.minSensor = rospy.get_param(key + 'minSensor') 49 | s.maxSensor = rospy.get_param(key + 'maxSensor') 50 | 51 | servos[name] = s 52 | 53 | return servos 54 | -------------------------------------------------------------------------------- /inmoov_bringup/include/servos.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed as BSD-3 3 | 4 | class Servo: 5 | key = -1 6 | name = "" 7 | bus = -1 8 | 9 | goal = -1.0 10 | 11 | servoPin = -1 12 | 13 | minPulse = -1.0 14 | maxPulse = -1.0 15 | minGoal = -1.0 16 | maxGoal = -1.0 17 | rest = -1.0 18 | smoothing = -1 19 | maxSpeed = -1.0 20 | 21 | sensorPin = -1 22 | minSensor = -1.0 23 | maxSensor = -1.0 -------------------------------------------------------------------------------- /inmoov_bringup/launch/bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /inmoov_bringup/nodes/joint_command_dispatcher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed under BSD-3 3 | 4 | 5 | import rospy 6 | 7 | from inmoov_msgs.msg import MotorStatus 8 | from inmoov_msgs.msg import MotorCommand 9 | from inmoov_msgs.srv import MotorParameter 10 | from sensor_msgs.msg import JointState 11 | from std_msgs.msg import Header 12 | 13 | import os 14 | import sys 15 | from os.path import dirname, abspath 16 | 17 | #hacky way to add include directory to sys path 18 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 19 | 20 | from constants import PROTOCOL 21 | from servos import Servo 22 | from load_config_from_param import load_config_from_param 23 | 24 | servos = {} # servo configuration data for robot 25 | joints = {} # dict of joint names and position values 26 | 27 | bus = {} # dict of motorcommand busses indexed by ordinal 28 | 29 | def init(): 30 | 31 | rospy.init_node('joint_command_dispatcher', anonymous=False) 32 | rate = rospy.Rate(20) # 40hz 33 | 34 | rospy.Subscriber("joint_command", JointState, dispatcher) 35 | 36 | servos = load_config_from_param() 37 | 38 | for j,b in rospy.get_param('/joints').items(): 39 | 40 | number = rospy.get_param('/joints/' + j + '/bus') 41 | busname = '/servobus/' + str(number).zfill(2) + '/motorcommand' 42 | 43 | if not bus.has_key(number): 44 | bus[number] = rospy.Publisher(busname, MotorCommand, queue_size=10) 45 | rospy.loginfo('adding: ' + busname) 46 | 47 | while not rospy.is_shutdown(): 48 | 49 | #iterate through joints and publish 50 | for j,p in joints.items(): 51 | 52 | try: 53 | motorcommand = MotorCommand() 54 | motorcommand.id = int(servos[j].servoPin) 55 | motorcommand.parameter = PROTOCOL.GOAL 56 | motorcommand.value = p 57 | 58 | bus[servos[j].bus].publish(motorcommand) 59 | except: 60 | rospy.logwarn('joint_command_dispatcher: unknown joint:' + j) 61 | 62 | 63 | 64 | #clear joints cache 65 | joints.clear() 66 | rate.sleep() 67 | 68 | rospy.spin() 69 | 70 | def dispatcher(js): 71 | #print "OHAI!" 72 | 73 | # iterate through array and stuff name + position into dict object 74 | for x in range(0, len(js.name)): 75 | joints[js.name[x]] = js.position[x] 76 | print"YATZEE" 77 | 78 | 79 | 80 | 81 | if __name__ == '__main__': 82 | try: 83 | init() 84 | except rospy.ROSInterruptException: 85 | pass -------------------------------------------------------------------------------- /inmoov_bringup/nodes/joint_status_dispatcher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed under BSD-3 3 | 4 | 5 | import rospy 6 | 7 | from inmoov_msgs.msg import MotorStatus 8 | from inmoov_msgs.msg import MotorCommand 9 | from inmoov_msgs.srv import MotorParameter 10 | from sensor_msgs.msg import JointState 11 | from std_msgs.msg import Header 12 | 13 | import os 14 | import sys 15 | from os.path import dirname, abspath 16 | 17 | #hacky way to add include directory to sys path 18 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 19 | 20 | from constants import PROTOCOL 21 | from servos import Servo 22 | from load_config_from_param import load_config_from_param 23 | 24 | servos = {} 25 | lookup = {} 26 | 27 | joints = {} 28 | bus = {} 29 | 30 | state = {} 31 | 32 | jointstatus = JointState() 33 | 34 | def init(): 35 | 36 | rospy.init_node('joint_status_dispatcher', anonymous=False) 37 | rate = rospy.Rate(40) # 40hz 38 | 39 | servos = load_config_from_param() 40 | 41 | #now, load lookup name by (bus*255+servo id) 42 | for n,s in servos.items(): 43 | key = ((int(s.bus)*255)+int(s.servoPin)) 44 | lookup[key] = n 45 | print 'key: ' + str(key) 46 | 47 | publisher = rospy.Publisher("joint_status", JointState, queue_size=10) 48 | 49 | for j,b in rospy.get_param('/joints').items(): 50 | 51 | #create motorstatus bus name 52 | number = rospy.get_param('/joints/' + j + '/bus') 53 | busname = '/servobus/' + str(number).zfill(2) + '/motorstatus' 54 | 55 | # and if it's not already in the bus{}, then add it 56 | # (not sure if the check is required) 57 | if not bus.has_key(number): 58 | bus[number] = rospy.Subscriber(busname, MotorStatus, dispatcher, (number)) 59 | rospy.loginfo('adding: ' + busname) 60 | 61 | while not rospy.is_shutdown(): 62 | 63 | #jointstatus = JointState() 64 | jointstatus.header = Header() 65 | jointstatus.header.stamp = rospy.Time.now() 66 | jointstatus.name = [] 67 | jointstatus.position = [] 68 | jointstatus.velocity = [] 69 | jointstatus.effort = [] 70 | 71 | 72 | for j,p in joints.items(): 73 | jointstatus.name.append(j) 74 | jointstatus.position.append(p) 75 | 76 | if jointstatus.name.count > 0: 77 | publisher.publish(jointstatus) 78 | 79 | joints.clear() 80 | rate.sleep() 81 | 82 | rospy.spin() 83 | 84 | def dispatcher(data, bus): 85 | try: 86 | #print "OHAI! bus:" + str(bus) + " servo:" + str(data.id) 87 | key = lookup[((int(bus)*255)+int(data.id))] 88 | joints[key] = data.position 89 | except: 90 | rospy.logwarn('joint_status_dispatcher: unknown servo at bus:'+str(bus)+' servo:'+str(data.id)) 91 | 92 | 93 | 94 | 95 | if __name__ == '__main__': 96 | try: 97 | init() 98 | except rospy.ROSInterruptException: 99 | pass -------------------------------------------------------------------------------- /inmoov_bringup/nodes/motor_status_dispatcher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed under BSD-3 3 | 4 | 5 | import rospy 6 | 7 | from inmoov_msgs.msg import MotorStatus 8 | 9 | from std_msgs.msg import Header 10 | 11 | import os 12 | import sys 13 | from os.path import dirname, abspath 14 | 15 | #hacky way to add include directory to sys path 16 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 17 | 18 | from constants import PROTOCOL 19 | from servos import Servo 20 | from load_config_from_param import load_config_from_param 21 | 22 | servos = {} 23 | lookup = {} 24 | 25 | joints = {} 26 | bus = {} 27 | 28 | state = {} 29 | 30 | publisher = rospy.Publisher("motor_status", MotorStatus, queue_size=10) 31 | 32 | def init(): 33 | 34 | rospy.init_node('motor_status_dispatcher', anonymous=False) 35 | rate = rospy.Rate(40) # 40hz 36 | 37 | servos = load_config_from_param() 38 | 39 | #now, load lookup name by (bus*255+servo id) 40 | for n,s in servos.items(): 41 | try: 42 | key = ((int(s.bus)*255)+int(s.servoPin)) 43 | except: 44 | rospy.logwarn('motor_status_dispatcher: unknown servo at bus:'+str(s.bus)+' servo:'+str(s.servoPin)) 45 | 46 | lookup[key] = n 47 | print 'key: ' + str(key) 48 | 49 | #publisher = rospy.Publisher("motor_status", MotorStatus, queue_size=10) 50 | 51 | for j,b in rospy.get_param('/joints').items(): 52 | 53 | #create motorstatus bus name 54 | number = rospy.get_param('/joints/' + j + '/bus') 55 | busname = '/servobus/' + str(number).zfill(2) + '/motorstatus' 56 | 57 | # and if it's not already in the bus{}, then add it 58 | # (not sure if the check is required) 59 | if not bus.has_key(number): 60 | bus[number] = rospy.Subscriber(busname, MotorStatus, dispatcher, (number)) 61 | rospy.loginfo('adding: ' + busname) 62 | 63 | rospy.spin() 64 | 65 | def dispatcher(data, bus): 66 | 67 | try: 68 | jointname = lookup[((int(bus)*255)+int(data.id))] 69 | data.joint = jointname 70 | data.bus = bus 71 | publisher.publish(data) 72 | except: 73 | rospy.logwarn('motor_status_dispatcher: unknown servo at bus:'+str(bus)+' servo:'+str(data.id)) 74 | 75 | 76 | if __name__ == '__main__': 77 | try: 78 | init() 79 | except rospy.ROSInterruptException: 80 | pass -------------------------------------------------------------------------------- /inmoov_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inmoov_bringup 4 | 0.0.0 5 | The inmoov_bringup package 6 | 7 | 8 | 9 | 10 | grey 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /inmoov_bringup/scripts/generate_config_xacro.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed as BSD-3 3 | 4 | import sys 5 | import rospy 6 | import yaml 7 | import os 8 | from os.path import dirname, abspath 9 | 10 | #hacky way to add include directory to sys path 11 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 12 | 13 | from constants import PROTOCOL 14 | from servos import Servo 15 | 16 | from inmoov_msgs.srv import MotorParameter 17 | 18 | servos = {} 19 | 20 | def exporter(): 21 | 22 | rospy.init_node("generate_config_xacro", log_level=rospy.INFO) 23 | 24 | load_config_from_param() 25 | 26 | check_config() 27 | 28 | update_config_from_eeprom() 29 | 30 | emit_config_xacro() 31 | 32 | print "DONE!" 33 | 34 | 35 | 36 | def load_config_from_param(): 37 | 38 | # first, make sure parameter server is even loaded 39 | while not rospy.search_param("/joints"): 40 | rospy.loginfo("waiting for parameter server to load with joint definitions") 41 | rospy.sleep(1) 42 | 43 | rospy.sleep(1) 44 | 45 | joints = rospy.get_param('/joints') 46 | for name in joints: 47 | print "found: " + name 48 | 49 | s = Servo() 50 | 51 | key = '/joints/' + name + '/' 52 | 53 | s.bus = rospy.get_param(key + 'bus') 54 | s.servo = rospy.get_param(key + 'servo') 55 | s.flip = rospy.get_param(key + 'flip') 56 | 57 | s.servopin = rospy.get_param(key + 'servopin') 58 | s.sensorpin = rospy.get_param(key + 'sensorpin') 59 | s.minpulse = rospy.get_param(key + 'minpulse') 60 | s.maxpulse = rospy.get_param(key + 'maxpulse') 61 | s.minangle = rospy.get_param(key + 'minangle') 62 | s.maxangle = rospy.get_param(key + 'maxangle') 63 | s.minsensor = rospy.get_param(key + 'minsensor') 64 | s.maxsensor = rospy.get_param(key + 'maxsensor') 65 | s.maxspeed = rospy.get_param(key + 'maxspeed') 66 | s.smoothing = rospy.get_param(key + 'smoothing') 67 | 68 | servos[name] = s 69 | 70 | 71 | print "DONE" 72 | 73 | def check_config(): 74 | print "DONE" 75 | 76 | def update_config_from_eeprom(): 77 | 78 | for name in servos: 79 | s = servos[name] 80 | print "interrogating eeprom for: " + name 81 | 82 | rospy.wait_for_service("servobus/" + str(s.bus).zfill(2) + "/motorparameter") 83 | 84 | motorparameter = rospy.ServiceProxy("servobus/" + str(s.bus).zfill(2) + "/motorparameter", MotorParameter) 85 | 86 | s.servopin = motorparameter(int(s.servo), PROTOCOL.SERVOPIN).data 87 | s.sensorpin = motorparameter(int(s.servo), PROTOCOL.SENSORPIN).data 88 | s.minpulse = motorparameter(int(s.servo), PROTOCOL.MINPULSE).data 89 | s.maxpulse = motorparameter(int(s.servo), PROTOCOL.MAXPULSE).data 90 | s.minangle = motorparameter(int(s.servo), PROTOCOL.MINANGLE).data 91 | s.maxangle = motorparameter(int(s.servo), PROTOCOL.MAXANGLE).data 92 | s.minsensor = motorparameter(int(s.servo), PROTOCOL.MINSENSOR).data 93 | s.maxsensor = motorparameter(int(s.servo), PROTOCOL.MAXSENSOR).data 94 | s.maxspeed = motorparameter(int(s.servo), PROTOCOL.MAXSPEED).data 95 | s.smoothing = motorparameter(int(s.servo), PROTOCOL.SMOOTH).data 96 | 97 | print "DONE" 98 | 99 | def emit_config_xacro(): 100 | 101 | # this is the fastest way to do it, but doesn't preserve the original yaml format 102 | # wouldn't be hard to rewrite the function to bigbang the yaml file 103 | # in the exact format of config.yaml 104 | 105 | #infile = os.path.join(dirname(dirname(abspath(__file__))),'config','config.yaml') 106 | outfile = os.path.join(dirname(dirname(abspath(__file__))),'config','config.xacro') 107 | #print infile 108 | print outfile 109 | 110 | #y = yaml.load(open(infile, 'r')) 111 | 112 | with open(outfile, 'w') as xacro: 113 | xacro.write('' + '\n') 114 | xacro.write('\n') 115 | xacro.write('' + '\n') 116 | xacro.write('\n') 117 | 118 | for name in servos: 119 | s = servos[name] 120 | print "updating yaml for: " + name 121 | 122 | xacro.write(' \n') 123 | xacro.write(' \n') 124 | xacro.write(' \n') 125 | 126 | xacro.write('\n') 127 | 128 | xacro.write('') 129 | 130 | xacro.close() 131 | 132 | print "DONE" 133 | 134 | 135 | if __name__ == '__main__': 136 | try: 137 | exporter() 138 | except rospy.ROSInterruptException: 139 | pass -------------------------------------------------------------------------------- /inmoov_bringup/scripts/generate_export_yaml_from_eeprom.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed as BSD-3 3 | 4 | import sys 5 | import rospy 6 | import yaml 7 | import os 8 | from os.path import dirname, abspath 9 | 10 | #hacky way to add include directory to sys path 11 | sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) 12 | 13 | from constants import PROTOCOL 14 | from servos import Servo 15 | 16 | from export_yaml import export_yaml 17 | from load_config_from_param import load_config_from_param 18 | 19 | from inmoov_msgs.srv import MotorParameter 20 | 21 | servos = {} 22 | 23 | def exporter(): 24 | 25 | rospy.init_node("generate_export_yaml", log_level=rospy.INFO) 26 | 27 | servos = load_config_from_param() 28 | 29 | check_config() 30 | 31 | update_config_from_eeprom(servos) 32 | 33 | export_yaml('export.yaml') 34 | 35 | print "DONE!" 36 | 37 | def check_config(): 38 | print "DONE" 39 | 40 | def update_config_from_eeprom(servos): 41 | 42 | for name in servos: 43 | s = servos[name] 44 | print "interrogating eeprom for: " + name 45 | 46 | rospy.wait_for_service("servobus/" + str(s.bus).zfill(2) + "/motorparameter") 47 | 48 | motorparameter = rospy.ServiceProxy("servobus/" + str(s.bus).zfill(2) + "/motorparameter", MotorParameter) 49 | 50 | key = '/joints/' + name + '/' 51 | 52 | val = motorparameter(int(s.servo), PROTOCOL.SERVOPIN).data 53 | rospy.set_param(key + 'servoPin', val) 54 | val = motorparameter(int(s.servo), PROTOCOL.SENSORPIN).data 55 | rospy.set_param(key + 'sensorPin', val) 56 | val = motorparameter(int(s.servo), PROTOCOL.MINPULSE).data 57 | rospy.set_param(key + 'minPulse', val) 58 | val = motorparameter(int(s.servo), PROTOCOL.MAXPULSE).data 59 | rospy.set_param(key + 'maxPulse', val) 60 | val = motorparameter(int(s.servo), PROTOCOL.MINGOAL).data 61 | rospy.set_param(key + 'minGoal', val) 62 | val = motorparameter(int(s.servo), PROTOCOL.MAXGOAL).data 63 | rospy.set_param(key + 'maxGoal', val) 64 | val = motorparameter(int(s.servo), PROTOCOL.REST).data 65 | rospy.set_param(key + 'rest', val) 66 | val = motorparameter(int(s.servo), PROTOCOL.MINSENSOR).data 67 | rospy.set_param(key + 'minSensor', val) 68 | val = motorparameter(int(s.servo), PROTOCOL.MAXSENSOR).data 69 | rospy.set_param(key + 'maxSensor', val) 70 | val = motorparameter(int(s.servo), PROTOCOL.MAXSPEED).data 71 | rospy.set_param(key + 'maxSpeed', val) 72 | 73 | 74 | print "DONE" 75 | 76 | 77 | if __name__ == '__main__': 78 | try: 79 | exporter() 80 | except rospy.ROSInterruptException: 81 | pass -------------------------------------------------------------------------------- /inmoov_bringup/tools/enable_manager/enable_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import os 5 | import rospy 6 | import rospkg 7 | 8 | import random 9 | 10 | from threading import Thread 11 | import thread 12 | import atexit 13 | 14 | from python_qt_binding import loadUi 15 | from python_qt_binding import QtGui 16 | from python_qt_binding.QtWidgets import QWidget 17 | 18 | from PyQt5 import QtWidgets, QtCore, uic 19 | from PyQt5.QtWidgets import QWidget, QCheckBox, QApplication, QVBoxLayout 20 | 21 | #from lookgui import Ui_MainWindow 22 | 23 | from inmoov_msgs.msg import MotorStatus 24 | from inmoov_msgs.msg import MotorCommand 25 | from inmoov_msgs.srv import MotorParameter 26 | from sensor_msgs.msg import JointState 27 | from std_msgs.msg import Header 28 | 29 | import os 30 | import sys 31 | from os.path import dirname, abspath 32 | 33 | #hacky way to add include directory to sys path 34 | sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))),'include')) 35 | 36 | from constants import PROTOCOL 37 | from servos import Servo 38 | from load_config_from_param import load_config_from_param 39 | 40 | from time import sleep 41 | 42 | # https://github.com/ColinDuquesnoy/QDarkStyleSheet 43 | # import qdarkstyle 44 | 45 | # https://www.safaribooksonline.com/blog/2014/01/22/create-basic-gui-using-pyqt/ 46 | gui = os.path.join(os.path.dirname(__file__), 'enable_manager.ui') 47 | form_class = uic.loadUiType(gui)[0] 48 | 49 | # https://nikolak.com/pyqt-qt-designer-getting-started/ 50 | #class ExampleApp(QtGui.QMainWindow, Ui_MainWindow): 51 | class ExampleApp(QtWidgets.QMainWindow, form_class): 52 | def __init__(self): 53 | # Explaining super is out of the scope of this article 54 | # So please google it if you're not familar with it 55 | # Simple reason why we use it here is that it allows us to 56 | # access variables, methods etc in the design.py file 57 | super(self.__class__, self).__init__() 58 | self.setupUi(self) # This is defined in design.py file automatically 59 | # It sets up layout and widgets that are defined 60 | 61 | self.servos = load_config_from_param() 62 | 63 | self.bus = {} 64 | 65 | self.checkboxes = {} 66 | 67 | self.motorcommand = MotorCommand() 68 | self.jointcommand = JointState() 69 | 70 | self.jointNames = [] 71 | 72 | rospy.init_node('enable_manager', anonymous=True) 73 | 74 | print("INITIALIZED") 75 | 76 | for j,b in rospy.get_param('/joints').items(): 77 | 78 | number = rospy.get_param('/joints/' + j + '/bus') 79 | busname = '/servobus/' + str(number).zfill(2) + '/motorcommand' 80 | 81 | if not self.bus.has_key(number): 82 | self.bus[number] = rospy.Publisher(busname, MotorCommand, queue_size=40) 83 | rospy.loginfo('adding: ' + busname) 84 | 85 | self.jointPublisher = rospy.Publisher("joint_command", JointState, queue_size=40) 86 | 87 | self.statusSubscriber = rospy.Subscriber("motor_status", MotorStatus, self.statusListener) 88 | 89 | self.btnEnableAll.clicked.connect(self.setEnableAll) 90 | self.btnDisableAll.clicked.connect(self.setDisableAll) 91 | 92 | print("INIT COMPLETE") 93 | 94 | joints = rospy.get_param('/joints') 95 | for name, s in self.servos.items(): 96 | print name 97 | chk = QCheckBox(name) 98 | chk.setText(name) 99 | chk.setStyleSheet(checkboxstylesheet) 100 | #chk.setEnabled(False) 101 | chk.stateChanged.connect(self.checkChanged) 102 | self.layout.addWidget(chk) 103 | self.checkboxes[name] = chk 104 | 105 | def statusListener(self, s): 106 | j = s.joint 107 | chk = self.checkboxes[j] 108 | chk.blockSignals(True) 109 | chk.setChecked(s.enabled) 110 | chk.blockSignals(False) 111 | return 112 | 113 | def checkChanged(self): 114 | sender = self.sender() 115 | print sender.text() 116 | 117 | s = self.servos[sender.text()] 118 | chk = self.checkboxes[sender.text()] 119 | 120 | motorcommand = MotorCommand() 121 | motorcommand.id = int(s.servoPin) 122 | motorcommand.parameter = PROTOCOL.ENABLE 123 | motorcommand.value = sender.isChecked() 124 | #if chk.isChecked(): 125 | # motorcommand.value = True 126 | ##else: 127 | # motorcommand.value = False 128 | #motorcommand.value = float(chk.isChecked()) 129 | 130 | print 'checkChanged: ' + sender.text() + 'to: ' + str(sender.isChecked()) 131 | 132 | self.bus[s.bus].publish(motorcommand) 133 | 134 | def setEnableAll(self): 135 | 136 | #self.statusSubscriber.unregister() 137 | 138 | #disconnect events 139 | #for j,chk in self.checkboxes.items(): 140 | # chk.disconnect() 141 | 142 | for j,s in self.servos.items(): 143 | 144 | 145 | motorcommand = MotorCommand() 146 | motorcommand.id = int(s.servoPin) 147 | motorcommand.parameter = PROTOCOL.ENABLE 148 | motorcommand.value = 1 149 | 150 | #print str(j) 151 | 152 | self.bus[s.bus].publish(motorcommand) 153 | #rospy.sleep(0.1) 154 | 155 | #self.statusSubscriber = rospy.Subscriber("motor_status", MotorStatus, self.statusListener) 156 | 157 | def setDisableAll(self): 158 | for j,s in self.servos.items(): 159 | 160 | motorcommand = MotorCommand() 161 | motorcommand.id = int(s.servoPin) 162 | motorcommand.parameter = PROTOCOL.ENABLE 163 | motorcommand.value = 0 164 | 165 | print j 166 | 167 | self.bus[s.bus].publish(motorcommand) 168 | #rospy.sleep(0.1) 169 | 170 | 171 | def closeEvent(self, event): 172 | self.enabled = False 173 | self.random = False 174 | print "GOODBYE!" 175 | 176 | checkboxstylesheet = \ 177 | 'QCheckBox::indicator {' + \ 178 | 'width: 12px;' + \ 179 | 'height: 12px;' + \ 180 | 'border-style: outset;' + \ 181 | 'border-width: 1px;' + \ 182 | 'border-radius: 4px;' + \ 183 | 'border-color: rgb(125, 125, 125);' + \ 184 | 'border: 1px solid rgb(0,0,0);' + \ 185 | 'background-color: rgb(130, 7, 7);' + \ 186 | '}' + \ 187 | 'QCheckBox::indicator:unchecked {background-color: rgb(130, 7, 7);}' + \ 188 | 'QCheckBox::indicator:checked {background-color: rgb(11, 145, 1);}' 189 | 190 | def main(): 191 | app = QtWidgets.QApplication(sys.argv) # A new instance of QApplication 192 | # app.setStyleSheet(qdarkstyle.load_stylesheet_pyqt5()) 193 | form = ExampleApp() # We set the form to be our ExampleApp (design) 194 | form.show() # Show the form 195 | app.exec_() # and execute the app 196 | 197 | if __name__ == '__main__': # if we're running file directly and not importing it 198 | main() 199 | 200 | -------------------------------------------------------------------------------- /inmoov_bringup/tools/enable_manager/enable_manager.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 222 10 | 776 11 | 12 | 13 | 14 | Enable Manager 15 | 16 | 17 | false 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 10 27 | 90 28 | 201 29 | 661 30 | 31 | 32 | 33 | QFrame::Box 34 | 35 | 36 | QFrame::Raised 37 | 38 | 39 | 2 40 | 41 | 42 | 43 | 44 | 9 45 | 9 46 | 181 47 | 641 48 | 49 | 50 | 51 | 52 | QLayout::SetNoConstraint 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 10 61 | 10 62 | 201 63 | 71 64 | 65 | 66 | 67 | QFrame::WinPanel 68 | 69 | 70 | QFrame::Raised 71 | 72 | 73 | 74 | 75 | 10 76 | 10 77 | 184 78 | 21 79 | 80 | 81 | 82 | QPushButton { 83 | background-color: rgb(0, 120, 0); 84 | border-style: outset; 85 | border-width: 1px; 86 | border-radius: 4px; 87 | border-color: rgb(125, 125, 125); 88 | min-width: 10em; 89 | padding: 3px; 90 | } 91 | 92 | 93 | Enable All 94 | 95 | 96 | false 97 | 98 | 99 | false 100 | 101 | 102 | false 103 | 104 | 105 | 106 | 107 | 108 | 10 109 | 40 110 | 184 111 | 21 112 | 113 | 114 | 115 | QPushButton { 116 | background-color: rgb(120, 0, 0); 117 | border-style: outset; 118 | border-width: 1px; 119 | border-radius: 4px; 120 | border-color: rgb(125, 125, 125); 121 | min-width: 10em; 122 | padding: 3px; 123 | } 124 | 125 | 126 | Disable All 127 | 128 | 129 | false 130 | 131 | 132 | false 133 | 134 | 135 | false 136 | 137 | 138 | 139 | 140 | 141 | 142 | false 143 | 144 | 145 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /inmoov_bringup/tools/rviz_manager/rviz_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # licensed under BSD-3 3 | 4 | 5 | import rospy 6 | 7 | from inmoov_msgs.msg import MotorStatus 8 | from inmoov_msgs.msg import MotorCommand 9 | from inmoov_msgs.srv import MotorParameter 10 | from sensor_msgs.msg import JointState 11 | from std_msgs.msg import Header 12 | 13 | import os 14 | import sys 15 | from os.path import dirname, abspath 16 | 17 | from PyQt5 import QtWidgets, QtCore, uic 18 | from PyQt5.QtWidgets import QPushButton, QMessageBox 19 | 20 | from threading import Thread 21 | 22 | from python_qt_binding import loadUi 23 | from python_qt_binding import QtGui 24 | from python_qt_binding.QtWidgets import QWidget 25 | 26 | #hacky way to add include directory to sys path 27 | sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))),'include')) 28 | 29 | from constants import PROTOCOL 30 | from servos import Servo 31 | from load_config_from_param import load_config_from_param 32 | 33 | servos = {} # servo configuration data for robot 34 | joints = {} # dict of joint names and position values 35 | 36 | bus = {} # dict of motorcommand busses indexed by ordinal 37 | 38 | PI = 3.1415926539 39 | 40 | # https://github.com/ColinDuquesnoy/QDarkStyleSheet 41 | # import qdarkstyle 42 | 43 | # https://www.safaribooksonline.com/blog/2014/01/22/create-basic-gui-using-pyqt/ 44 | gui = os.path.join(os.path.dirname(__file__), 'rviz_manager.ui') 45 | form_class = uic.loadUiType(gui)[0] 46 | 47 | def commandDispatcher(data): 48 | 49 | # if robot set up with degrees, then convert to radians 50 | if rospy.get_param('/bringup/angles') == 'degrees': 51 | p = list(data.position) 52 | for x in range(0, len(p)): 53 | p[x] = PI * p[x] / 180.0 54 | 55 | data.position = tuple(p) 56 | 57 | publisher.publish(data) 58 | 59 | def stateDispatcher(data): 60 | 61 | # if robot set up with degrees, then convert to radians 62 | if rospy.get_param('/bringup/angles') == 'degrees': 63 | p = list(data.position) 64 | for x in range(0, len(p)): 65 | p[x] = PI * p[x] / 180.0 66 | 67 | data.position = tuple(p) 68 | 69 | publisher.publish(data) 70 | 71 | subscriber1 = rospy.Subscriber("joint_command", JointState, commandDispatcher) 72 | subscriber2 = rospy.Subscriber("joint_state", JointState, stateDispatcher) 73 | publisher = rospy.Publisher('rviz_command',JointState, queue_size=10) 74 | 75 | def init(): 76 | 77 | rospy.init_node('rviz_command_dispatcher', anonymous=False) 78 | #rate = rospy.Rate(20) # 20hz 79 | 80 | load_config_from_param() 81 | 82 | #for j,b in rospy.get_param('/joints').items(): 83 | # 84 | # number = rospy.get_param('/joints/' + j + '/bus') 85 | # busname = '/servobus/' + str(number).zfill(2) + '/motorcommand' 86 | 87 | # if not bus.has_key(number): 88 | # bus[number] = rospy.Publisher(busname, MotorCommand, queue_size=10) 89 | # rospy.loginfo('adding: ' + busname) 90 | 91 | 92 | rospy.spin() 93 | 94 | 95 | 96 | if __name__ == '__main__': 97 | try: 98 | init() 99 | except rospy.ROSInterruptException: 100 | pass -------------------------------------------------------------------------------- /inmoov_bringup/tools/rviz_manager/rviz_manager.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 222 10 | 129 11 | 12 | 13 | 14 | rviz_manager 15 | 16 | 17 | false 18 | 19 | 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 10 27 | 10 28 | 201 29 | 91 30 | 31 | 32 | 33 | QFrame::WinPanel 34 | 35 | 36 | QFrame::Raised 37 | 38 | 39 | 40 | 41 | 20 42 | 40 43 | 141 44 | 22 45 | 46 | 47 | 48 | joint_command 49 | 50 | 51 | true 52 | 53 | 54 | 55 | 56 | 57 | 20 58 | 60 59 | 117 60 | 22 61 | 62 | 63 | 64 | joint_status 65 | 66 | 67 | 68 | 69 | 70 | 40 71 | 10 72 | 111 73 | 20 74 | 75 | 76 | 77 | Qt::LeftToRight 78 | 79 | 80 | rviz joint source: 81 | 82 | 83 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 84 | 85 | 86 | 87 | 88 | 89 | 90 | false 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /inmoov_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(inmoov_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES inmoov_description 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(inmoov_description 115 | # src/${PROJECT_NAME}/inmoov_description.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(inmoov_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(inmoov_description_node src/inmoov_description_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(inmoov_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(inmoov_description_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS inmoov_description inmoov_description_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_description.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /inmoov_description/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Alan Timm (alansrobotlab) 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /inmoov_description/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | ["rviz_command"] 8 | 20 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /inmoov_description/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /inmoov_description/launch/moveit-display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /inmoov_description/launch_rviz.sh: -------------------------------------------------------------------------------- 1 | roslaunch urdf_tutorial display.launch model:=robots/inmoov.urdf gui:=True 2 | 3 | -------------------------------------------------------------------------------- /inmoov_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inmoov_description 4 | 0.0.0 5 | The inmoov_description package 6 | 7 | 8 | 9 | 10 | grey 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /inmoov_description/robots/inmoov.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | -------------------------------------------------------------------------------- /inmoov_description/robots/inmoov_face.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /inmoov_description/robots/inmoov_head.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /inmoov_description/robots/inmoov_right_hand.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmArm.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmBase.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmBicepServo.urdf.xacro: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_description/urdf/asmBicepServo.urdf.xacro -------------------------------------------------------------------------------- /inmoov_description/urdf/asmEye.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmFace.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmHead.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmNeckFrontServo.urdf.xacro: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_description/urdf/asmNeckFrontServo.urdf.xacro -------------------------------------------------------------------------------- /inmoov_description/urdf/asmNeckSideServo.urdf.xacro: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_description/urdf/asmNeckSideServo.urdf.xacro -------------------------------------------------------------------------------- /inmoov_description/urdf/asmTorso.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /inmoov_description/urdf/asmTorsoServo.urdf.xacro: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_description/urdf/asmTorsoServo.urdf.xacro -------------------------------------------------------------------------------- /inmoov_description/urdf/materials.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /inmoov_firmware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(inmoov_firmware) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES inmoov_firmware 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(inmoov_firmware 115 | # src/${PROJECT_NAME}/inmoov_firmware.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(inmoov_firmware ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(inmoov_firmware_node src/inmoov_firmware_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(inmoov_firmware_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(inmoov_firmware_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS inmoov_firmware inmoov_firmware_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_firmware.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /inmoov_firmware/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Alan Timm (alansrobotlab) 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /inmoov_firmware/TeensyServo.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "eeprom.h" 4 | #include "configuration.h" 5 | #include 6 | 7 | class TeensyServo { 8 | private: 9 | Eeprom e; 10 | 11 | void readEeprom(int); 12 | void writeEeprom(); 13 | int generateEepromChecksum(); 14 | 15 | float goalAngle; 16 | 17 | public: 18 | int servoPin; 19 | int sensorPin; 20 | 21 | int debugInt = 0; 22 | float debugFloat = 0; 23 | 24 | 25 | bool moving = false; 26 | bool power = false; 27 | int r; 28 | int deltaPulse; 29 | int velocityArc; 30 | int moveDuration; 31 | int enabled = 0; 32 | 33 | int startMillis; 34 | int startPulse; 35 | int commandPulse; 36 | int currentPulse; 37 | 38 | int deltaMillis; 39 | int ticksPerSecond = 1500; 40 | 41 | int position = 0; 42 | int sampleCount = 0; 43 | int sampleBucket = 0; 44 | int sampleStartMillis = 0; 45 | int sampleDuration = 0; 46 | 47 | bool receivedCommand = false; 48 | 49 | int i, j; 50 | 51 | Servo servo; 52 | 53 | TeensyServo(int, int); 54 | 55 | void setGoal(float); 56 | float getGoal(); 57 | 58 | void moveToMicroseconds(int); 59 | short readPositionRaw(); 60 | float readPositionAngle(); 61 | short readPositionPulse(); 62 | 63 | void update(); 64 | 65 | void calibrate(); 66 | 67 | void setupADC(); 68 | 69 | void setMinPulse(short); 70 | short getMinPulse(); 71 | void setMaxPulse(short); 72 | short getMaxPulse(); 73 | 74 | void setMinAngle(float); 75 | float getMinAngle(); 76 | void setMaxAngle(float); 77 | float getMaxAngle(); 78 | 79 | void setMinSensor(int); 80 | int getMinSensor(); 81 | void setMaxSensor(int); 82 | int getMaxSensor(); 83 | 84 | void setEnabled(bool); 85 | bool getEnabled(); 86 | 87 | bool getCalibrated(); 88 | void setCalibrated(bool); 89 | 90 | 91 | void setMaxSpeed(float); 92 | float getMaxSpeed(); 93 | 94 | void setSmooth(byte); 95 | byte getSmooth(); 96 | 97 | bool getPower(); 98 | 99 | bool getMoving(); 100 | 101 | float readPresentSpeed(); 102 | 103 | void updatePosition(); 104 | 105 | }; 106 | -------------------------------------------------------------------------------- /inmoov_firmware/configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef configuration_h 2 | #define configuration_h 3 | 4 | #define NUMSERVOS 12 5 | 6 | #define MINPULSE 550 7 | #define MAXPULSE 2450 8 | 9 | #define MINSENSOR 550 10 | #define MAXSENSOR 3800 11 | 12 | #define MINANGLE 0 13 | #define MAXANGLE 180 14 | 15 | #define SMOOTH 0 16 | #define MAXSPEED 100 17 | 18 | #define ID_ADDRESS 512 19 | 20 | #define EEPROM_RECORD_SIZE 64 21 | 22 | #define EEPROM_START 0 23 | 24 | #define UPDATEPERIOD 250 25 | 26 | //Protocol Constants 27 | #define P_WRITE 0x03 28 | #define P_READ 0x02 29 | 30 | #define P_GOALPOSITION 0x1E 31 | #define P_ENABLE 0x18 32 | #define P_MINANGLE 0x06 33 | #define P_MAXANGLE 0x08 34 | #define P_MINPULSE 0x14 35 | #define P_MAXPULSE 0x16 36 | #define P_MINSENSOR 0xA2 37 | #define P_MAXSENSOR 0xA4 38 | #define P_GOALSPEED 0x20 39 | #define P_CALIBRATED 0xA0 40 | #define P_LED 0x19 41 | #define P_SMOOTH 0xA6 42 | 43 | #define P_PRESENTPOSITION 0x24 44 | #define P_PRESENTSPEED 0x26 45 | #define P_MOVING 0x2E 46 | #define P_SENSORRAW 0xA8 47 | #define P_POWER 0x2A 48 | 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /inmoov_firmware/eeprom.h: -------------------------------------------------------------------------------- 1 | #ifndef eeprom_h 2 | #define eeprom_h 3 | 4 | struct Eeprom { 5 | 6 | byte id; 7 | 8 | byte checksum; 9 | 10 | float minAngle; 11 | float maxAngle; 12 | 13 | short minPulse; //defined as pulse for 0 degrees 14 | short maxPulse; //defined as pulse for 180 degrees 15 | 16 | short minSensor; //defined as sensor reading for minangle 17 | short maxSensor; //defined as sensor reading for maxangle 18 | 19 | short maxSpeed; //rpm? 20 | 21 | byte smooth; // smothing algorythm setting 22 | // 0 == instant 23 | // 1 == max speed 24 | // 2 == linear 25 | // 3 == cos 26 | // 4 == ??? (I forgot) 27 | 28 | byte calibrated; // 1 is calibrated, 0 is not calibrated 29 | 30 | float p; // PID P value * 100 31 | float i; // PID I value * 100 32 | float d; // PID D value * 100 33 | }; 34 | 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /inmoov_firmware/inmoov_firmware.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "TeensyServo.h" 4 | #include "configuration.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define LED 13 12 | 13 | int i, j; 14 | int updateMillis; 15 | int commands; 16 | 17 | TeensyServo *tServo[12]; 18 | 19 | int commandAngle; 20 | int startMillis; 21 | 22 | ros::NodeHandle nh; 23 | 24 | inmoov_msgs::MotorCommand command_msg; 25 | inmoov_msgs::MotorParameter parameter_msg; 26 | inmoov_msgs::MotorStatus status_msg; 27 | 28 | const bool heartbeats[] = {1, 0, 1, 0, 0, 0, 0, 0}; 29 | 30 | void getParameter(const inmoov_msgs::MotorParameter::Request & req, inmoov_msgs::MotorParameter::Response & res) { 31 | byte id = req.id; 32 | byte parameter = req.parameter; 33 | float value = 0.0; 34 | 35 | switch (parameter) { 36 | 37 | case P_GOALPOSITION: 38 | value = tServo[id]->getGoal(); 39 | break; 40 | 41 | case P_MINANGLE: 42 | value = tServo[id]->getMinAngle(); 43 | break; 44 | 45 | case P_MAXANGLE: 46 | value = tServo[id]->getMaxAngle(); 47 | break; 48 | 49 | case P_MINPULSE: 50 | value = (float)tServo[id]->getMinPulse(); 51 | break; 52 | 53 | case P_MAXPULSE: 54 | value = (float)tServo[id]->getMaxPulse(); 55 | break; 56 | 57 | case P_MINSENSOR: 58 | value = (float)tServo[id]->getMinSensor(); 59 | break; 60 | 61 | case P_MAXSENSOR: 62 | value = (float)tServo[id]->getMaxSensor(); 63 | break; 64 | 65 | case P_CALIBRATED: 66 | value = tServo[id]->getCalibrated(); 67 | break; 68 | 69 | case P_PRESENTPOSITION: 70 | value = tServo[id]->readPositionAngle(); 71 | break; 72 | 73 | case P_SENSORRAW: 74 | value = (float)tServo[id]->readPositionRaw(); 75 | break; 76 | 77 | case P_MOVING: 78 | value = (float)tServo[id]->getMoving(); 79 | break; 80 | 81 | case P_PRESENTSPEED: 82 | value = tServo[id]->readPresentSpeed(); 83 | break; 84 | 85 | case P_SMOOTH: 86 | value = (float)tServo[id]->getSmooth(); 87 | break; 88 | 89 | case P_GOALSPEED: 90 | value = tServo[id]->getMaxSpeed(); 91 | break; 92 | 93 | case P_ENABLE: 94 | value = (float)tServo[id]->getEnabled(); 95 | break; 96 | 97 | case P_POWER: 98 | value = (float)tServo[id]->getPower(); 99 | break; 100 | } 101 | 102 | res.data = value; 103 | 104 | } 105 | 106 | void commandCb( const inmoov_msgs::MotorCommand& command_msg) { 107 | 108 | byte id = command_msg.id; 109 | byte parameter = command_msg.parameter; 110 | float value = command_msg.value; 111 | 112 | switch (parameter) { 113 | 114 | case P_GOALPOSITION: 115 | tServo[id]->setGoal(value); 116 | //String string = "Goal Position = " + String(tServo[id]->commandPulse); 117 | //nh.loginfo(String(tServo[id]->commandPulse)); 118 | break; 119 | 120 | case P_MINANGLE: 121 | tServo[id]->setMinAngle(value); 122 | break; 123 | 124 | case P_MAXANGLE: 125 | tServo[id]->setMaxAngle(value); 126 | break; 127 | 128 | case P_MINPULSE: 129 | tServo[id]->setMinPulse(value); 130 | break; 131 | 132 | case P_MAXPULSE: 133 | tServo[id]->setMaxPulse(value); 134 | break; 135 | 136 | case P_ENABLE: 137 | tServo[id]->setEnabled(value); 138 | break; 139 | 140 | case P_CALIBRATED: 141 | tServo[id]->setCalibrated(value); 142 | break; 143 | 144 | case P_MINSENSOR: 145 | tServo[id]->setMinSensor(value); 146 | break; 147 | 148 | case P_MAXSENSOR: 149 | tServo[id]->setMaxSensor(value); 150 | break; 151 | 152 | case P_SMOOTH: 153 | tServo[id]->setSmooth(value); 154 | break; 155 | 156 | case P_GOALSPEED: 157 | tServo[id]->setMaxSpeed(value); 158 | 159 | } 160 | 161 | 162 | } 163 | 164 | 165 | ros::Publisher motorstatus("motorstatus", &status_msg); 166 | 167 | ros::Subscriber motorcommand("motorcommand", &commandCb); 168 | 169 | ros::ServiceServer server("motorparameter", &getParameter); 170 | 171 | 172 | void setupADC() { 173 | analogReadResolution(12); 174 | analogReference(EXTERNAL); 175 | } 176 | 177 | 178 | void updateServos() { 179 | for (i = 0; i < NUMSERVOS; i++) { 180 | tServo[i]->update(); 181 | } 182 | } 183 | 184 | void setupServos() { 185 | tServo[0] = new TeensyServo(0, A8); 186 | tServo[1] = new TeensyServo(1, A9); 187 | tServo[2] = new TeensyServo(2, A10); 188 | tServo[3] = new TeensyServo(3, A11); 189 | tServo[4] = new TeensyServo(4, A7); 190 | tServo[5] = new TeensyServo(5, A6); 191 | tServo[6] = new TeensyServo(6, A5); 192 | tServo[7] = new TeensyServo(7, A4); 193 | tServo[8] = new TeensyServo(8, A3); 194 | tServo[9] = new TeensyServo(9, A2); 195 | tServo[10] = new TeensyServo(10, A1); 196 | tServo[11] = new TeensyServo(11, A0); 197 | 198 | } 199 | 200 | byte generateChecksum() { 201 | return 0; 202 | } 203 | 204 | void setup() { 205 | pinMode(LED, OUTPUT); 206 | digitalWrite(LED, 1); 207 | 208 | nh.initNode(); 209 | nh.advertise(motorstatus); 210 | nh.subscribe(motorcommand); 211 | nh.advertiseService(server); 212 | 213 | while (!nh.connected() ) { 214 | nh.spinOnce(); 215 | } 216 | 217 | setupADC(); 218 | delay(1); 219 | 220 | setupServos(); 221 | 222 | Serial.begin(115200); 223 | 224 | startMillis = millis(); 225 | updateMillis = millis(); 226 | commands = 0; 227 | 228 | 229 | 230 | nh.loginfo("Setup Complete!!!"); 231 | 232 | } 233 | 234 | int pushmotorstatus = 0; // which motorstatus to update 235 | 236 | char joint[2] = " "; 237 | byte bus = 0; 238 | 239 | void loop() { 240 | 241 | updateServos(); 242 | 243 | digitalWrite(LED, heartbeats[((millis() >> 7) & 7)]); 244 | /* 245 | if ((millis() - updateMillis) >= (UPDATEPERIOD / NUMSERVOS)) { 246 | status_msg.id = pushmotorstatus; 247 | status_msg.goal = tServo[pushmotorstatus]->getGoal(); 248 | status_msg.position = tServo[pushmotorstatus]->readPositionAngle(); 249 | status_msg.presentspeed = tServo[pushmotorstatus]->readPresentSpeed(); 250 | status_msg.moving = tServo[pushmotorstatus]->getMoving(); 251 | //status_msg.posraw = tServo[servo]->sampleDuration; 252 | status_msg.posraw = tServo[pushmotorstatus]->readPositionRaw(); 253 | status_msg.enabled = tServo[pushmotorstatus]->getEnabled(); 254 | status_msg.power = tServo[pushmotorstatus]->getPower(); 255 | 256 | motorstatus.publish( &status_msg); 257 | 258 | pushmotorstatus++; 259 | if(pushmotorstatus == NUMSERVOS) { 260 | pushmotorstatus = 0; 261 | } 262 | 263 | updateMillis = millis(); 264 | nh.spinOnce(); 265 | 266 | 267 | } 268 | */ 269 | 270 | 271 | if ((millis() - updateMillis) >= UPDATEPERIOD) { 272 | for (int servo = 0; servo < NUMSERVOS; servo++) { 273 | 274 | status_msg.joint = joint; 275 | status_msg.bus = bus; 276 | status_msg.id = servo; 277 | status_msg.goal = tServo[servo]->getGoal(); 278 | status_msg.position = tServo[servo]->readPositionAngle(); 279 | status_msg.presentspeed = tServo[servo]->readPresentSpeed(); 280 | status_msg.moving = tServo[servo]->getMoving(); 281 | //status_msg.posraw = tServo[servo]->sampleDuration; 282 | status_msg.posraw = tServo[servo]->readPositionRaw(); 283 | status_msg.enabled = tServo[servo]->getEnabled(); 284 | status_msg.power = tServo[servo]->getPower(); 285 | 286 | nh.spinOnce(); 287 | 288 | motorstatus.publish( &status_msg); 289 | 290 | nh.spinOnce(); 291 | } 292 | 293 | updateMillis = millis(); 294 | nh.spinOnce(); 295 | 296 | commands = 0; 297 | 298 | } 299 | 300 | nh.spinOnce(); 301 | 302 | } 303 | 304 | 305 | 306 | -------------------------------------------------------------------------------- /inmoov_firmware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inmoov_firmware 4 | 0.0.0 5 | The inmoov_firmware package 6 | 7 | 8 | 9 | 10 | grey 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /inmoov_meshes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(inmoov_meshes) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES inmoov_meshes 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | 116 | ## Declare a C++ library 117 | # add_library(${PROJECT_NAME} 118 | # src/${PROJECT_NAME}/inmoov_meshes.cpp 119 | # ) 120 | 121 | ## Add cmake target dependencies of the library 122 | ## as an example, code may need to be generated before libraries 123 | ## either from message generation or dynamic reconfigure 124 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 125 | 126 | ## Declare a C++ executable 127 | ## With catkin_make all packages are built within a single CMake context 128 | ## The recommended prefix ensures that target names across packages don't collide 129 | # add_executable(${PROJECT_NAME}_node src/inmoov_meshes_node.cpp) 130 | 131 | ## Rename C++ executable without prefix 132 | ## The above recommended prefix causes long target names, the following renames the 133 | ## target back to the shorter version for ease of user use 134 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 135 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 136 | 137 | ## Add cmake target dependencies of the executable 138 | ## same as for the library above 139 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 140 | 141 | ## Specify libraries to link a library or executable target against 142 | # target_link_libraries(${PROJECT_NAME}_node 143 | # ${catkin_LIBRARIES} 144 | # ) 145 | 146 | ############# 147 | ## Install ## 148 | ############# 149 | 150 | # all install targets should use catkin DESTINATION variables 151 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 152 | 153 | ## Mark executable scripts (Python etc.) for installation 154 | ## in contrast to setup.py, you can choose the destination 155 | # install(PROGRAMS 156 | # scripts/my_python_script 157 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 158 | # ) 159 | 160 | ## Mark executables and/or libraries for installation 161 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 162 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 163 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 164 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark cpp header files for installation 168 | # install(DIRECTORY include/${PROJECT_NAME}/ 169 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 170 | # FILES_MATCHING PATTERN "*.h" 171 | # PATTERN ".svn" EXCLUDE 172 | # ) 173 | 174 | ## Mark other files for installation (e.g. launch and bag files, etc.) 175 | # install(FILES 176 | # # myfile1 177 | # # myfile2 178 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 179 | # ) 180 | 181 | ############# 182 | ## Testing ## 183 | ############# 184 | 185 | ## Add gtest based cpp test target and link libraries 186 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_meshes.cpp) 187 | # if(TARGET ${PROJECT_NAME}-test) 188 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 189 | # endif() 190 | 191 | ## Add folders to be run by python nosetests 192 | # catkin_add_nosetests(test) 193 | -------------------------------------------------------------------------------- /inmoov_meshes/meshes/bicep.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/bicep.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/bicepcover.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/bicepcover.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/camera.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/camera.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/chest.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/chest.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/disk.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/disk.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/earleftv1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/earleftv1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/earrightv1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/earrightv1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/eye.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/eye.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/eyesupport.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/eyesupport.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/face.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/face.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/head.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/head.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/head_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/head_base.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/index3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/index3_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/index3_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/index3_2.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/index3_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/index3_3.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/iris.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/iris.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/jaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/jaw.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/kinectone.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/kinectone.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_hand.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_handpinky.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_handpinky.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_handring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_handring.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_index.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_index.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_middle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_middle.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_pinky.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_pinky.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_ring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_ring.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_cover_thumb.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_cover_thumb.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_eyesupport.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_eyesupport.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_forearm.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_hand.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_pinky3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_pinky3_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_ring3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_ring3_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_shoulder.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_shoulder_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_shoulder_base.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/l_thumb5_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/l_thumb5_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/launch_rviz.sh: -------------------------------------------------------------------------------- 1 | roslaunch urdf_tutorial display.launch model:=inmoov_grey.urdf gui:=True 2 | 3 | -------------------------------------------------------------------------------- /inmoov_meshes/meshes/mid_stomach.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/mid_stomach.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/middle3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/middle3_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/middle3_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/middle3_2.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/middle3_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/middle3_3.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/pinky3_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/pinky3_2.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/pinky3_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/pinky3_3.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/pinky3_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/pinky3_4.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_hand.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_handpinky.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_handpinky.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_handring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_handring.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_index.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_index.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_middle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_middle.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_pinky.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_pinky.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_ring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_ring.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_cover_thumb.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_cover_thumb.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_eyesupport.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_eyesupport.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_forearm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_forearm.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_hand.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_hand.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_pinky3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_pinky3_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_ring3_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_ring3_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_shoulder.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_shoulder_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_shoulder_base.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/r_thumb5_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/r_thumb5_1.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/ring3_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/ring3_2.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/ring3_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/ring3_3.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/ring3_4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/ring3_4.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/skull.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/skull.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/thumb5_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/thumb5_2.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/thumb5_3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/thumb5_3.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/top_stomach.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/top_stomach.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/torso.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/torso.stl -------------------------------------------------------------------------------- /inmoov_meshes/meshes/virtual.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_meshes/meshes/virtual.stl -------------------------------------------------------------------------------- /inmoov_meshes/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inmoov_meshes 4 | 0.0.0 5 | The inmoov_meshes package 6 | 7 | 8 | 9 | 10 | grey 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /inmoov_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(inmoov_msgs) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS message_generation std_msgs) 6 | 7 | add_message_files( 8 | FILES 9 | MotorCommand.msg 10 | MotorStatus.msg 11 | SmartServoStatus.msg 12 | ) 13 | 14 | add_service_files( 15 | FILES 16 | MotorParameter.srv 17 | ) 18 | 19 | generate_messages( 20 | DEPENDENCIES std_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS message_runtime std_msgs 25 | ) 26 | -------------------------------------------------------------------------------- /inmoov_msgs/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Alan Timm (alansrobotlab) 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /inmoov_msgs/msg/MotorCommand.msg: -------------------------------------------------------------------------------- 1 | uint8 id # motor id 2 | uint8 parameter # parameter 3 | float32 value # value 4 | 5 | -------------------------------------------------------------------------------- /inmoov_msgs/msg/MotorStatus.msg: -------------------------------------------------------------------------------- 1 | string joint # joint name for status frame (populated in motor_status_dispatcher) 2 | uint8 bus # bus source for status frame (populated in motor_status_dispatcher) 3 | uint8 id # motor id 0-11 4 | float32 goal # command position 5 | float32 position # current sensed position 6 | float32 presentspeed # calculated rotational speed 7 | bool moving # is servo moving? 8 | uint16 posraw # raw position sensor value 9 | bool enabled # is servo enabled? 10 | bool power # does servo have power? 11 | -------------------------------------------------------------------------------- /inmoov_msgs/msg/SmartServoStatus.msg: -------------------------------------------------------------------------------- 1 | string joint # joint name for status frame (populated in motor_status_dispatcher) 2 | uint8 bus # bus source for status frame (populated in motor_status_dispatcher) 3 | uint8 id # motor id 0-11 4 | float32 goal # command position 5 | float32 position # current sensed position 6 | float32 presentspeed # calculated rotational speed 7 | bool moving # is servo moving? 8 | uint16 posraw # raw position sensor value 9 | bool enabled # is servo enabled? 10 | bool power # does servo have power? 11 | uint8 temp # current sensed temperature 12 | uint8 error # error flags 13 | int16 value1 # custom value 14 | int16 value2 # custom value 15 | int16 value3 # custom value 16 | int16 value4 # custom value 17 | int16 value5 # custom value -------------------------------------------------------------------------------- /inmoov_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | inmoov_msgs 3 | 0.1.0 4 | 5 | Common messages used throughout inmoov_motor stack. 6 | 7 | Alan Timm 8 | 9 | BSD 10 | 11 | https://www.facebook.com/alansrobotlab 12 | https://github.com/alansrobotlab/inmoov_ros 13 | 14 | Alan Timm 15 | 16 | catkin 17 | 18 | message_generation 19 | std_msgs 20 | message_runtime 21 | std_msgs 22 | 23 | 24 | -------------------------------------------------------------------------------- /inmoov_msgs/srv/MotorParameter.srv: -------------------------------------------------------------------------------- 1 | uint8 id # motor id 2 | uint8 parameter # parameter 3 | --- 4 | float32 data 5 | -------------------------------------------------------------------------------- /inmoov_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(inmoov_tools) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES inmoov_tools 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(inmoov_tools 115 | # src/${PROJECT_NAME}/inmoov_tools.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(inmoov_tools ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(inmoov_tools_node src/inmoov_tools_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(inmoov_tools_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(inmoov_tools_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS inmoov_tools inmoov_tools_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_tools.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /inmoov_tools/LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2017, Alan Timm (alansrobotlab) 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/head_pointer_gui.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | A simple Controller GUI to drive robots and pose heads. 5 | Copyright (c) 2008-2011 Michael E. Ferguson. All right reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * 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 | * Neither the name of Vanadium Labs LLC nor the names of its 15 | contributors may be used to endorse or promote products derived 16 | from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 23 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 24 | OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 25 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 26 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 27 | ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | Modifications by Patrick Goebel for the Pi Robot Project 30 | """ 31 | 32 | import roslib; #roslib.load_manifest('pi_head_tracking_3d_part1') 33 | import rospy 34 | import wx 35 | 36 | import tf 37 | from geometry_msgs.msg import PointStamped 38 | from geometry_msgs.msg import Twist 39 | from std_msgs.msg import Float64 40 | 41 | width = 300 42 | height = 200 43 | 44 | class PointHeadGUI(wx.Frame): 45 | TIMER_ID = 100 46 | 47 | def __init__(self, parent, debug = False): 48 | wx.Frame.__init__(self, parent, -1, "Point Head GUI", style = wx.DEFAULT_FRAME_STYLE & ~ (wx.RESIZE_BORDER | wx.MAXIMIZE_BOX)) 49 | 50 | # Intialize the transform listener 51 | self.tf = tf.TransformListener() 52 | 53 | # Make sure at least the head_pan and head_tilt frames are visible 54 | #self.tf.waitForTransform('head_pan_link', 'head_tilt_link', rospy.Time(), rospy.Duration(5.0)) 55 | 56 | # Get the list of all frames 57 | self.frames = self.tf.getFrameStrings() 58 | 59 | # Initialize the target point 60 | self.target_point = PointStamped() 61 | 62 | # Create the target point publisher 63 | self.targetPub = rospy.Publisher('/target_point', PointStamped) 64 | 65 | sizer = wx.GridBagSizer(15,10) 66 | 67 | # Select Target Reference Frame 68 | selectFrame = wx.StaticBox(self, -1, 'Select Frame') 69 | selectFrame.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD)) 70 | selectFrameBox = wx.StaticBoxSizer(selectFrame, orient=wx.VERTICAL) 71 | frameSizer = wx.GridBagSizer(3, 10) 72 | self.frameComboBox = wx.ComboBox(self, -1, style=wx.CB_READONLY) 73 | self.Bind(wx.EVT_COMBOBOX, self.on_combobox, self.frameComboBox) 74 | self.update_combobox() 75 | frameSizer.Add(self.frameComboBox, (0, 0)) 76 | selectFrameBox.Add(frameSizer) 77 | sizer.Add(selectFrameBox,(0,0),wx.GBSpan(3,10),wx.EXPAND|wx.TOP|wx.RIGHT|wx.LEFT,5) 78 | 79 | # Select x y z target coordinates relative to the frame selected above 80 | selectPoint = wx.StaticBox(self, -1, 'Set Target Point') 81 | selectPoint.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD)) 82 | selectPointBox = wx.StaticBoxSizer(selectPoint, orient=wx.VERTICAL) 83 | headSizer = wx.GridBagSizer(3, 10) 84 | headSizer.Add(wx.StaticText(self, -1, "x:"),(0,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL) 85 | self.target_point_x = wx.TextCtrl(self,-1,value=u"0.0") 86 | headSizer.Add(self.target_point_x, (0,1)) 87 | headSizer.Add(wx.StaticText(self, -1, "y:"),(1,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL) 88 | self.target_point_y = wx.TextCtrl(self,-1,value=u"0.0") 89 | headSizer.Add(self.target_point_y, (1,1)) 90 | headSizer.Add(wx.StaticText(self, -1, "z:"),(2,0), wx.GBSpan(1,1),wx.ALIGN_CENTER_VERTICAL) 91 | self.target_point_z = wx.TextCtrl(self,-1,value=u"0.0") 92 | headSizer.Add(self.target_point_z, (2,1)) 93 | selectPointBox.Add(headSizer) 94 | sizer.Add(selectPointBox, (3,0), wx.GBSpan(3,10), wx.EXPAND|wx.BOTTOM|wx.RIGHT|wx.LEFT,5) 95 | 96 | # Point head button 97 | pointSizer = wx.GridBagSizer(1, 10) 98 | self.pointButton = wx.Button(self, -1, label="Point Head!") 99 | self.Bind(wx.EVT_BUTTON, self.on_point_button, self.pointButton) 100 | pointSizer.Add(self.pointButton, (0, 1)) 101 | 102 | # Reset head button 103 | self.resetButton = wx.Button(self, -1, label="Reset Position") 104 | self.Bind(wx.EVT_BUTTON, self.on_reset_button, self.resetButton) 105 | pointSizer.Add(self.resetButton, (0, 2)) 106 | sizer.Add(pointSizer, (6,0), wx.GBSpan(2,10), wx.EXPAND|wx.BOTTOM|wx.RIGHT|wx.LEFT,5) 107 | 108 | # timer for output 109 | self.timer = wx.Timer(self, self.TIMER_ID) 110 | self.timer.Start(50) 111 | wx.EVT_CLOSE(self, self.onClose) 112 | wx.EVT_TIMER(self, self.TIMER_ID, self.onTimer) 113 | 114 | # bind the panel to the paint event 115 | wx.EVT_PAINT(self, self.onPaint) 116 | self.dirty = 1 117 | self.onPaint() 118 | 119 | self.SetSizerAndFit(sizer) 120 | self.Show(True) 121 | 122 | def update_combobox(self): 123 | self.frameComboBox.SetItems(self.frames) 124 | selected_frame = self.get_selected_frame() 125 | 126 | def get_selected_frame(self): 127 | return str(self.frameComboBox.GetValue()) 128 | 129 | def on_combobox(self, event): 130 | self.target_point.header.frame_id = self.get_selected_frame().rstrip() 131 | 132 | def on_point_button(self, event): 133 | try: 134 | self.target_point.header.frame_id = 'kinect2_link' 135 | self.target_point.point.x = float(self.target_point_x.GetValue().rstrip()) 136 | self.target_point.point.y = float(self.target_point_y.GetValue().rstrip()) 137 | self.target_point.point.z = float(self.target_point_z.GetValue().rstrip()) 138 | rospy.loginfo("Publishing target point:\n" + str(self.target_point)) 139 | self.targetPub.publish(self.target_point) 140 | except: 141 | rospy.loginfo("Invalid floating point coordinates. Please try again.") 142 | 143 | def on_reset_button(self, event): 144 | # Set the head level and straight ahead but setting a far away target point relative to the base_link frame. 145 | try: 146 | self.target_point.point.x = 1000. 147 | self.target_point.point.y = 0.0 148 | self.target_point.point.z = 0.0 149 | self.target_point.header.frame_id = '/base_link' 150 | rospy.loginfo("Publishing target point:\n" + str(self.target_point)) 151 | self.targetPub.publish(self.target_point) 152 | except: 153 | rospy.loginfo("Invalid floating point coordinates. Please try again.") 154 | 155 | def onClose(self, event): 156 | self.timer.Stop() 157 | self.Destroy() 158 | 159 | def onPaint(self, event=None): 160 | pass 161 | 162 | def onTimer(self, event): 163 | if rospy.is_shutdown(): 164 | self.Close(True) 165 | self.Refresh() 166 | 167 | def onExit(self, e): 168 | self.Close(True) 169 | self.Refresh() 170 | 171 | def onError(self): 172 | self.Raise() 173 | 174 | if __name__ == '__main__': 175 | # initialize GUI 176 | rospy.init_node('point_head_gui') 177 | app = wx.PySimpleApp() 178 | frame = PointHeadGUI(None, True) 179 | app.MainLoop() 180 | 181 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/point_head.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona 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 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * 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 | # * Neither the name of the Willow Garage, Inc. 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 | # Author: Anh Tran 31 | 32 | # Based on the wubble_head_action.py script by Anh Tran. Modifications made by Patrick Goebel 33 | # for the Pi Robot Project. 34 | 35 | import roslib; #roslib.load_manifest('pi_head_tracking_3d_part1') 36 | import rospy 37 | import tf 38 | from geometry_msgs.msg import PointStamped 39 | #from std_msgs.msg import Float64 40 | from inmoov_msgs.msg import MotorCommand 41 | from sensor_msgs.msg import JointState 42 | from std_msgs.msg import Header 43 | 44 | import math 45 | 46 | class PointHeadNode(): 47 | 48 | def __init__(self): 49 | # Initialize new node 50 | rospy.init_node('point_head_node', anonymous=True) 51 | 52 | #dynamixel_namespace = rospy.get_namespace() 53 | rate = rospy.get_param('~rate', 40) 54 | r = rospy.Rate(rate) 55 | 56 | # Initialize the target point 57 | self.target_point = PointStamped() 58 | self.last_target_point = PointStamped() 59 | 60 | # Subscribe to the target_point topic 61 | #rospy.Subscriber('/clicked_point', PointStamped, self.update_target_point) 62 | rospy.Subscriber('/target_point', PointStamped, self.update_target_point) 63 | 64 | # Initialize publisher 65 | self.head_pan_frame = 'head' 66 | self.head_tilt_frame = 'head_base' 67 | self.head_yaw_frame = 'head_tilt' 68 | self.reference_frame = 'kinect2_link' 69 | self.head_pub = rospy.Publisher('/servobus/torso/motorcommand', MotorCommand, queue_size=10) 70 | self.jointPublisher = rospy.Publisher("joint_command", JointState, queue_size=10) 71 | 72 | 73 | # Initialize publisher for the tilt servo 74 | #self.head_tilt_frame = 'head_tilt_link' 75 | #self.head_tilt_pub = rospy.Publisher(dynamixel_namespace + 'head_tilt_controller/command', Float64) 76 | 77 | # Initialize tf listener 78 | self.tf = tf.TransformListener() 79 | 80 | self.motorcommand = MotorCommand() 81 | self.jointcommand = JointState() 82 | 83 | # Make sure we can see at least the pan and tilt frames 84 | #self.tf.waitForTransform(self.head_pan_frame, self.head_tilt_frame, rospy.Time(), rospy.Duration(5.0)) 85 | 86 | # Reset the head position to neutral 87 | rospy.sleep(1) 88 | self.center_head() 89 | 90 | rospy.loginfo("Ready to accept target point") 91 | 92 | while not rospy.is_shutdown(): 93 | rospy.wait_for_message('/target_point', PointStamped) 94 | if self.target_point == self.last_target_point: 95 | continue 96 | try: 97 | target_angles = self.transform_target_point(self.target_point) 98 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 99 | rospy.logerr("tf Failure") 100 | continue 101 | 102 | self.head_pan(target_angles[0]) 103 | self.head_tilt(target_angles[1]) 104 | #self.head_pan_pub.publish(target_angles[0]) 105 | #self.head_tilt_pub.publish(target_angles[1]) 106 | 107 | self.last_target_point = self.target_point 108 | rospy.loginfo("Setting Target Point:\n" + str(self.target_point)) 109 | 110 | r.sleep() 111 | 112 | def update_target_point(self, msg): 113 | self.target_point = msg 114 | 115 | def center_head(self): 116 | self.head_pan(0.0) 117 | self.head_tilt(0.0) 118 | #self.head_pan_pub.publish(0.0) 119 | #self.head_tilt_pub.publish(0.0) 120 | rospy.sleep(3) 121 | 122 | def transform_target_point(self, target): 123 | # Set the pan and tilt reference frames to the head_pan_frame and head_tilt_frame defined above 124 | #pan_ref_frame = self.head_pan_frame 125 | #tilt_ref_frame = self.head_tilt_frame 126 | pan_ref_frame = self.reference_frame 127 | tilt_ref_frame = self.reference_frame 128 | 129 | # Wait for tf info (time-out in 5 seconds) 130 | self.tf.waitForTransform(pan_ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0)) 131 | self.tf.waitForTransform(tilt_ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0)) 132 | 133 | # Transform target point to pan reference frame & retrieve the pan angle 134 | pan_target = self.tf.transformPoint(pan_ref_frame, target) 135 | pan_angle = math.atan2(pan_target.point.y, pan_target.point.x) 136 | 137 | # Transform target point to tilt reference frame & retrieve the tilt angle 138 | tilt_target = self.tf.transformPoint(tilt_ref_frame, target) 139 | tilt_angle = math.atan2(tilt_target.point.z, 140 | math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2))) 141 | 142 | return [pan_angle, tilt_angle] 143 | 144 | def rad_to_degrees (self, rad): 145 | degrees = (180.0 * (rad / 3.1415928539)) 146 | rospy.loginfo(str(rad) + " Rad to " + str(degrees) + " Degrees: \n" ) 147 | 148 | return (180.0 * (rad / 3.1415928539)) 149 | 150 | def tilt_to_tilt_yaw (self, rad): 151 | return [0,0] 152 | 153 | def head_pan(self, rad): 154 | self.motorcommand.id = 3 155 | self.motorcommand.parameter = 0x1E 156 | self.motorcommand.value = self.rad_to_degrees(rad) 157 | self.head_pub.publish(self.motorcommand) 158 | 159 | self.jointcommand.header = Header() 160 | self.jointcommand.header.stamp = rospy.Time.now() 161 | self.jointcommand.name = ['head_leftright'] 162 | self.jointcommand.position = [rad] 163 | self.jointcommand.velocity = [] 164 | self.jointcommand.effort = [] 165 | self.jointPublisher.publish(self.jointcommand) 166 | 167 | def head_tilt(self, rad): 168 | self.motorcommand.id = 4 169 | self.motorcommand.parameter = 0x1E 170 | self.motorcommand.value = -self.rad_to_degrees(rad) 171 | self.head_pub.publish(self.motorcommand) 172 | 173 | self.jointcommand.header = Header() 174 | self.jointcommand.header.stamp = rospy.Time.now() 175 | self.jointcommand.name = ['head_updown'] 176 | self.jointcommand.position = [rad] 177 | self.jointcommand.velocity = [] 178 | self.jointcommand.effort = [] 179 | self.jointPublisher.publish(self.jointcommand) 180 | 181 | if __name__ == '__main__': 182 | try: 183 | point_head = PointHeadNode() 184 | rospy.spin() 185 | except rospy.ROSInterruptException: 186 | pass 187 | 188 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/point_head_1.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # Based on the wubble_head_action.py script by Anh Tran. Modifications made by Patrick Goebel 4 | # for the Pi Robot Project. 5 | 6 | 7 | import roslib; 8 | import rospy 9 | import tf 10 | from geometry_msgs.msg import PointStamped 11 | from inmoov_msgs.msg import MotorCommand 12 | from sensor_msgs.msg import JointState 13 | from std_msgs.msg import Header 14 | 15 | import math 16 | import time 17 | 18 | class PointHeadNode(): 19 | 20 | def __init__(self): 21 | # Initialize new node 22 | rospy.init_node('point_head_node', anonymous=True) 23 | 24 | self.rate = 0.2 25 | self.headMoveDuration = 1000.0 #time in milliseconds 26 | 27 | rate = rospy.get_param('~rate', self.rate) 28 | r = rospy.Rate(rate) 29 | 30 | # Subscribe to the target_point topic 31 | rospy.Subscriber('/target_point', PointStamped, self.update_target_point) 32 | 33 | # Initialize publisher 34 | self.head_pan_frame = 'head' 35 | self.head_tilt_frame = 'head' 36 | self.head_yaw_frame = 'head_tilt' 37 | self.reference_frame = 'kinect2_link' 38 | self.head_pub = rospy.Publisher('/servobus/torso/motorcommand', MotorCommand, queue_size=10) 39 | self.jointPublisher = rospy.Publisher("joint_command", JointState, queue_size=10) 40 | 41 | # Initialize the target point 42 | self.target_point = PointStamped() 43 | self.target_point.point.x = 100.0 44 | self.target_point.point.y = 0.0 45 | self.target_point.point.z = 0.0 46 | self.target_point.header.stamp = rospy.Time.now() 47 | self.target_point.header.frame_id = self.reference_frame 48 | 49 | self.last_target_point = PointStamped() 50 | 51 | # Initialize tf listener 52 | self.tf = tf.TransformListener() 53 | 54 | self.motorcommand = MotorCommand() 55 | self.jointcommand = JointState() 56 | 57 | self.center_head() 58 | 59 | # Make sure we can see at least the pan and tilt frames 60 | self.tf.waitForTransform(self.head_pan_frame, self.head_tilt_frame, rospy.Time(), rospy.Duration(5.0)) 61 | 62 | rospy.sleep(1) 63 | 64 | rospy.loginfo("Ready to accept target point") 65 | 66 | self.center_head() 67 | 68 | while not rospy.is_shutdown(): 69 | 70 | r.sleep() 71 | 72 | def update_target_point(self, msg): 73 | 74 | try: 75 | target_angles = self.transform_target_point(msg) 76 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 77 | rospy.logerr("tf Failure") 78 | 79 | self.head_pan(target_angles[0]) 80 | self.head_tilt(target_angles[1]) 81 | 82 | 83 | 84 | def center_head(self): 85 | self.head_pan(0.0) 86 | self.head_tilt(0.0) 87 | rospy.sleep(3) 88 | 89 | def transform_target_point(self, target): 90 | # Set the pan and tilt reference frames to the head_pan_frame and head_tilt_frame defined above 91 | ref_frame = 'base_link' 92 | 93 | target.point.z -= 1.35 94 | 95 | rospy.loginfo("OG Point=" + str(target)) 96 | 97 | target.header.stamp = rospy.Time(0) 98 | 99 | # Wait for tf info (time-out in 5 seconds) 100 | self.tf.waitForTransform(ref_frame, target.header.frame_id, rospy.Time(), rospy.Duration(5.0)) 101 | 102 | # Transform target point to pan reference frame & retrieve the pan angle 103 | pan_target = self.tf.transformPoint(ref_frame, target) 104 | pan_angle = math.atan2(pan_target.point.y, pan_target.point.x) 105 | 106 | # Transform target point to tilt reference frame & retrieve the tilt angle 107 | tilt_target = self.tf.transformPoint(ref_frame, target) 108 | tilt_angle = -math.atan2(tilt_target.point.z, 109 | math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2))) 110 | 111 | rospy.loginfo("New Point=" + str(pan_target)) 112 | 113 | rospy.loginfo(":Pan Angle=" + str(self.rad_to_degrees(pan_angle)) + " Tilt Angle=" + str(self.rad_to_degrees(tilt_angle))) 114 | 115 | return [pan_angle, tilt_angle] 116 | 117 | def rad_to_degrees (self, rad): 118 | degrees = (180.0 * (rad / 3.1415928539)) 119 | #rospy.loginfo(str(rad) + " Rad to " + str(degrees) + " Degrees: \n" ) 120 | 121 | return (180.0 * (rad / 3.1415928539)) 122 | 123 | def tilt_to_tilt_yaw (self, rad): 124 | return [0,0] 125 | 126 | def nudge(self): 127 | 128 | m = self.getMillis()-self.startMillis 129 | 130 | self.curPanAngle = self.targetPanAngle 131 | self.curTiltAngle = self.targetTiltAngle 132 | 133 | self.head_pan(self.curPanAngle) 134 | self.head_tilt(self.curTiltAngle) 135 | 136 | 137 | def head_pan(self, rad): 138 | self.motorcommand.id = 3 139 | self.motorcommand.parameter = 0x1E 140 | self.motorcommand.value = self.rad_to_degrees(rad) 141 | self.head_pub.publish(self.motorcommand) 142 | 143 | self.jointcommand.header = Header() 144 | self.jointcommand.header.stamp = rospy.Time.now() 145 | self.jointcommand.name = ['head_leftright'] 146 | self.jointcommand.position = [rad] 147 | self.jointcommand.velocity = [] 148 | self.jointcommand.effort = [] 149 | self.jointPublisher.publish(self.jointcommand) 150 | 151 | def head_tilt(self, rad): 152 | self.motorcommand.id = 4 153 | self.motorcommand.parameter = 0x1E 154 | self.motorcommand.value = self.rad_to_degrees(rad) 155 | self.head_pub.publish(self.motorcommand) 156 | 157 | self.jointcommand.header = Header() 158 | self.jointcommand.header.stamp = rospy.Time.now() 159 | self.jointcommand.name = ['head_updown'] 160 | self.jointcommand.position = [rad] 161 | self.jointcommand.velocity = [] 162 | self.jointcommand.effort = [] 163 | self.jointPublisher.publish(self.jointcommand) 164 | 165 | def head_roll(self, rad): 166 | self.motorcommand.id = 5 167 | self.motorcommand.parameter = 0x1E 168 | self.motorcommand.value = self.rad_to_degrees(rad) 169 | self.head_pub.publish(self.motorcommand) 170 | 171 | self.jointcommand.header = Header() 172 | self.jointcommand.header.stamp = rospy.Time.now() 173 | self.jointcommand.name = ['head_tilt'] 174 | self.jointcommand.position = [rad] 175 | self.jointcommand.velocity = [] 176 | self.jointcommand.effort = [] 177 | self.jointPublisher.publish(self.jointcommand) 178 | 179 | def eyes_pan(self, rad): 180 | self.motorcommand.id = 0 181 | self.motorcommand.parameter = 0x1E 182 | self.motorcommand.value = self.rad_to_degrees(rad) 183 | self.head_pub.publish(self.motorcommand) 184 | 185 | self.jointcommand.header = Header() 186 | self.jointcommand.header.stamp = rospy.Time.now() 187 | self.jointcommand.name = ['eye_leftright'] 188 | self.jointcommand.position = [rad] 189 | self.jointcommand.velocity = [] 190 | self.jointcommand.effort = [] 191 | self.jointPublisher.publish(self.jointcommand) 192 | 193 | def eyes_tilt(self, rad): 194 | self.motorcommand.id = 1 195 | self.motorcommand.parameter = 0x1E 196 | self.motorcommand.value = self.rad_to_degrees(rad) 197 | self.head_pub.publish(self.motorcommand) 198 | 199 | self.jointcommand.header = Header() 200 | self.jointcommand.header.stamp = rospy.Time.now() 201 | self.jointcommand.name = ['eyes_updown'] 202 | self.jointcommand.position = [rad] 203 | self.jointcommand.velocity = [] 204 | self.jointcommand.effort = [] 205 | self.jointPublisher.publish(self.jointcommand) 206 | 207 | def getMillis(self): 208 | #return time.time()*1000 209 | m = int(round(time.time()*1000)) 210 | return m 211 | 212 | if __name__ == '__main__': 213 | try: 214 | point_head = PointHeadNode() 215 | rospy.spin() 216 | except rospy.ROSInterruptException: 217 | pass 218 | 219 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/pub_3d_target.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #""" 4 | # pub_3d_target.py - Version 1.0 2011-08-17 5 | # 6 | ## Publish a target point relative to a given reference frame. 7 | # 8 | # Created for the Pi Robot Project: http://www.pirobot.org 9 | # Copyright (c) 2010 Patrick Goebel. All rights reserved. 10 | # 11 | # This program is free software; you can redistribute it and/or modify 12 | # it under the terms of the GNU General Public License as published by 13 | # the Free Software Foundation; either version 2 of the License, or 14 | # (at your option) any later version. 15 | # 16 | # This program is distributed in the hope that it will be useful, 17 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | # GNU General Public License for more details at: 20 | # 21 | # http://www.gnu.org/licenses/gpl.html 22 | #""" 23 | 24 | import roslib; #roslib.load_manifest('pi_head_tracking_3d_part2') 25 | import rospy 26 | import tf 27 | import sys 28 | from visualization_msgs.msg import Marker 29 | from geometry_msgs.msg import PointStamped, Point 30 | import math 31 | import random 32 | 33 | class Pub3DTarget(): 34 | def __init__(self): 35 | rospy.init_node('pub_3d_target') 36 | 37 | rate = rospy.get_param('~rate', 40) 38 | r = rospy.Rate(rate) 39 | 40 | # Remap this frame in the launch file or command line if necessary 41 | target_frame = 'kinect2_link' 42 | target_topic = 'target_point' 43 | #target_topic = 'clicked_point' 44 | 45 | # Parameters for publishing the RViz target marker 46 | marker_scale = rospy.get_param('~marker_scale', 0.05) 47 | marker_lifetime = rospy.get_param('~marker_lifetime', 1) # 0 is forever 48 | marker_ns = rospy.get_param('~marker_ns', 'target_point') 49 | marker_id = rospy.get_param('~marker_id', 0) 50 | marker_color = rospy.get_param('~marker_color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}) 51 | 52 | # Define a marker publisher 53 | marker_pub = rospy.Publisher('target_point_marker', Marker) 54 | 55 | # Initialize the marker 56 | marker = Marker() 57 | marker.ns = marker_ns 58 | marker.id = marker_id 59 | marker.type = Marker.SPHERE 60 | marker.action = Marker.ADD 61 | marker.lifetime = rospy.Duration(marker_lifetime) 62 | marker.scale.x = marker_scale 63 | marker.scale.y = marker_scale 64 | marker.scale.z = marker_scale 65 | marker.color.r = marker_color['r'] 66 | marker.color.g = marker_color['g'] 67 | marker.color.b = marker_color['b'] 68 | marker.color.a = marker_color['a'] 69 | 70 | # Define the target as a PointStamped message 71 | target = PointStamped() 72 | target.header.frame_id = target_frame 73 | 74 | # Define the target publisher 75 | target_pub = rospy.Publisher(target_topic, PointStamped) 76 | 77 | # Initialize tf listener 78 | tf_listener = tf.TransformListener() 79 | 80 | # Give the tf buffer a chance to fill up 81 | rospy.sleep(5.0) 82 | 83 | rospy.loginfo("Publishing target point on frame " + str(target_frame)) 84 | 85 | theta = 0.0 86 | rand_num = random.Random() 87 | 88 | while not rospy.is_shutdown(): 89 | target.point.x = 0.5 + 0.25 * math.sin(theta) 90 | target.point.y = 0.5 * math.sin(theta) 91 | target.point.z = -0.1 + 0.5 * abs(math.sin(theta)) 92 | theta += 0.025 93 | target.header.stamp = rospy.Time.now() 94 | target_pub.publish(target) 95 | 96 | marker.header.stamp = target.header.stamp 97 | marker.header.frame_id = 'base_link' 98 | marker.pose.position = target.point 99 | marker.id += 1 100 | marker.id %= 100 101 | marker_pub.publish(marker) 102 | 103 | r.sleep() 104 | 105 | if __name__ == '__main__': 106 | try: 107 | target = Pub3DTarget() 108 | rospy.spin() 109 | except rospy.ROSInterruptException: 110 | rospy.loginfo("Target publisher is shut down.") 111 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/publish_point_target.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #""" 4 | # pub_3d_target.py - Version 1.0 2011-08-17 5 | # 6 | ## Publish a target point relative to a given reference frame. 7 | # 8 | # Created for the Pi Robot Project: http://www.pirobot.org 9 | # Copyright (c) 2010 Patrick Goebel. All rights reserved. 10 | # 11 | # This program is free software; you can redistribute it and/or modify 12 | # it under the terms of the GNU General Public License as published by 13 | # the Free Software Foundation; either version 2 of the License, or 14 | # (at your option) any later version. 15 | # 16 | # This program is distributed in the hope that it will be useful, 17 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | # GNU General Public License for more details at: 20 | # 21 | # http://www.gnu.org/licenses/gpl.html 22 | #""" 23 | 24 | import roslib; #roslib.load_manifest('pi_head_tracking_3d_part2') 25 | import rospy 26 | import tf 27 | import sys 28 | from visualization_msgs.msg import Marker 29 | from geometry_msgs.msg import PointStamped, Point 30 | import math 31 | import random 32 | 33 | class Pub3DTarget(): 34 | def __init__(self): 35 | rospy.init_node('pub_3d_target') 36 | 37 | rate = rospy.get_param('~rate', 10) 38 | r = rospy.Rate(rate) 39 | 40 | # Remap this frame in the launch file or command line if necessary 41 | #target_frame = 'base_link' 42 | #target_frame = 'kinect2_link' 43 | target_topic = 'target_point' 44 | #target_topic = 'clicked_point' 45 | 46 | # Parameters for publishing the RViz target marker 47 | marker_scale = rospy.get_param('~marker_scale', 0.1) 48 | marker_lifetime = rospy.get_param('~marker_lifetime', 1) # 0 is forever 49 | marker_ns = rospy.get_param('~marker_ns', 'target_point') 50 | marker_id = rospy.get_param('~marker_id', 0) 51 | marker_color = rospy.get_param('~marker_color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}) 52 | 53 | # Define a marker publisher 54 | self.marker_pub = rospy.Publisher('target_point_marker', Marker, queue_size=10) 55 | 56 | self.clicked_sub = rospy.Subscriber('clicked_point', PointStamped, self.clicked_callback) 57 | 58 | #target_pub = rospy.Publisher(target_topic, PointStamped, queue_size=10) 59 | 60 | # Initialize the marker 61 | self.marker = Marker() 62 | self.marker.ns = marker_ns 63 | self.marker.id = marker_id 64 | self.marker.type = Marker.SPHERE 65 | self.marker.action = Marker.ADD 66 | self.marker.lifetime = rospy.Duration(marker_lifetime) 67 | self.marker.scale.x = marker_scale 68 | self.marker.scale.y = marker_scale 69 | self.marker.scale.z = marker_scale 70 | self.marker.color.r = marker_color['r'] 71 | self.marker.color.g = marker_color['g'] 72 | self.marker.color.b = marker_color['b'] 73 | self.marker.color.a = marker_color['a'] 74 | 75 | # Define the target as a PointStamped message 76 | self.target = PointStamped() 77 | self.target.point.x = 100.0 78 | self.target.point.y = 0.0 79 | self.target.point.z = 0.0 80 | self.target.header.stamp = rospy.Time.now() 81 | self.target.header.frame_id = 'base_link' 82 | 83 | # Define the target publisher 84 | self.target_pub = rospy.Publisher(target_topic, PointStamped, queue_size=10) 85 | 86 | # Initialize tf listener 87 | self.tf = tf.TransformListener() 88 | 89 | # Give the tf buffer a chance to fill up 90 | rospy.sleep(5.0) 91 | 92 | rospy.loginfo("Publishing target point on frame ")# + str(target_frame)) 93 | 94 | while not rospy.is_shutdown(): 95 | 96 | r.sleep() 97 | 98 | 99 | def clicked_callback(self, data): 100 | #i dont know why yet, but clicked_point has x and y reversed 101 | 102 | #self.target.point = self.tf.transformPoint('kinect2_link', data) 103 | #self.target.point.x = data.point.x 104 | #self.target.point.y = data.point.y 105 | #self.target.point.z = data.point.z 106 | #self.target.header.frame_id = data.header.frame_id 107 | 108 | #self.target.header.stamp = rospy.Time.now() 109 | self.target = data 110 | self.target_pub.publish(self.target) 111 | 112 | self.marker.header.stamp = self.target.header.stamp 113 | self.marker.header.frame_id = self.target.header.frame_id 114 | self.marker.pose.position = self.target.point 115 | self.marker.id += 1 116 | self.marker.id %= 100 117 | self.marker_pub.publish(self.marker) 118 | 119 | 120 | if __name__ == '__main__': 121 | try: 122 | target = Pub3DTarget() 123 | rospy.spin() 124 | except rospy.ROSInterruptException: 125 | rospy.loginfo("Target publisher is shut down.") 126 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/room_watcher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #""" 4 | # pub_3d_target.py - Version 1.0 2011-08-17 5 | # 6 | ## Publish a target point relative to a given reference frame. 7 | # 8 | # Created for the Pi Robot Project: http://www.pirobot.org 9 | # Copyright (c) 2010 Patrick Goebel. All rights reserved. 10 | # 11 | # This program is free software; you can redistribute it and/or modify 12 | # it under the terms of the GNU General Public License as published by 13 | # the Free Software Foundation; either version 2 of the License, or 14 | # (at your option) any later version. 15 | # 16 | # This program is distributed in the hope that it will be useful, 17 | ## but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | # GNU General Public License for more details at: 20 | # 21 | # http://www.gnu.org/licenses/gpl.html 22 | #""" 23 | 24 | import roslib; #roslib.load_manifest('pi_head_tracking_3d_part2') 25 | import rospy 26 | import tf 27 | import sys 28 | from visualization_msgs.msg import Marker 29 | from geometry_msgs.msg import PointStamped, Point 30 | import math 31 | from random import randint 32 | 33 | class Pub3DTarget(): 34 | def __init__(self): 35 | rospy.init_node('pub_3d_target') 36 | 37 | rate = rospy.get_param('~rate', 10) 38 | r = rospy.Rate(rate) 39 | 40 | # Remap this frame in the launch file or command line if necessary 41 | #target_frame = 'base_link' 42 | target_frame = 'base_link' 43 | target_topic = 'target_point' 44 | #target_topic = 'clicked_point' 45 | 46 | # Parameters for publishing the RViz target marker 47 | marker_scale = rospy.get_param('~marker_scale', 0.05) 48 | marker_lifetime = rospy.get_param('~marker_lifetime', 1) # 0 is forever 49 | marker_ns = rospy.get_param('~marker_ns', 'target_point') 50 | marker_id = rospy.get_param('~marker_id', 0) 51 | marker_color = rospy.get_param('~marker_color', {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}) 52 | 53 | # Define a marker publisher 54 | marker_pub = rospy.Publisher('target_point_marker', Marker, queue_size=10) 55 | 56 | #clicked_sub = rospy.Subscriber('clicked_point', PointStamped, self.clicked_callback) 57 | 58 | #target_pub = rospy.Publisher(target_topic, PointStamped, queue_size=10) 59 | 60 | # Initialize the marker 61 | marker = Marker() 62 | marker.ns = marker_ns 63 | marker.id = marker_id 64 | marker.type = Marker.SPHERE 65 | marker.action = Marker.ADD 66 | marker.lifetime = rospy.Duration(marker_lifetime) 67 | marker.scale.x = marker_scale 68 | marker.scale.y = marker_scale 69 | marker.scale.z = marker_scale 70 | marker.color.r = marker_color['r'] 71 | marker.color.g = marker_color['g'] 72 | marker.color.b = marker_color['b'] 73 | marker.color.a = marker_color['a'] 74 | 75 | # Define the target as a PointStamped message 76 | self.target = PointStamped() 77 | self.target.point.x = 100.0 78 | self.target.point.y = 0.0 79 | self.target.point.z = 0.0 80 | self.target.header.stamp = rospy.Time.now() 81 | self.target.header.frame_id = target_frame 82 | 83 | # Define the target publisher 84 | self.target_pub = rospy.Publisher(target_topic, PointStamped, queue_size=10) 85 | 86 | # Initialize tf listener 87 | self.tf = tf.TransformListener() 88 | 89 | # Give the tf buffer a chance to fill up 90 | rospy.sleep(5.0) 91 | 92 | rospy.loginfo("Publishing target point on frame " + str(target_frame)) 93 | 94 | while not rospy.is_shutdown(): 95 | 96 | row = randint(0,10) 97 | seat = randint(0,10) 98 | 99 | pause = randint(1,8) 100 | 101 | self.target.point.x = (row*2) + 5 102 | self.target.point.y = (seat-5) * 1.0 103 | self.target.point.z = 1.35 104 | 105 | self.target.header.stamp = rospy.Time.now() 106 | self.target_pub.publish(self.target) 107 | 108 | rospy.loginfo("Publishing target point" + str(self.target)) 109 | 110 | rospy.sleep(pause) 111 | 112 | 113 | def clicked_callback(self, data): 114 | #i dont know why yet, but clicked_point has x and y reversed 115 | 116 | #self.target.point = self.tf.transformPoint('kinect2_link', data) 117 | self.target.point.x = data.point.y 118 | self.target.point.y = -data.point.x 119 | self.target.point.z = data.point.z - 1.35 120 | 121 | self.target.header.stamp = rospy.Time.now() 122 | self.target_pub.publish(self.target) 123 | 124 | 125 | if __name__ == '__main__': 126 | try: 127 | target = Pub3DTarget() 128 | rospy.spin() 129 | except rospy.ROSInterruptException: 130 | rospy.loginfo("Target publisher is shut down.") 131 | -------------------------------------------------------------------------------- /inmoov_tools/headtracker/rviz_goal.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alansrobotlab/inmoov_ros/f2ff0082071dc9efa58f649b9491a0c03822ad46/inmoov_tools/headtracker/rviz_goal.py -------------------------------------------------------------------------------- /inmoov_tools/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /inmoov_tools/launch/dev.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /inmoov_tools/launch/kinect-demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /inmoov_tools/launch/kinect-full-demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /inmoov_tools/launch/makercon-demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /inmoov_tools/launch/params.yaml: -------------------------------------------------------------------------------- 1 | servobus: 2 | leftarm: 3 | name: servoBus/leftarm 4 | port: /dev/ttyACM0 5 | servomap: 6 | '0': {name: left_pinky} 7 | '1': {name: left_ring} 8 | '10': {name: leftarm-nc-10} 9 | '11': {name: leftarm-nc-11} 10 | '2': {name: left_middle} 11 | '3': {name: left_index} 12 | '4': {name: left_thumb} 13 | '5': {name: left_hand} 14 | '6': {name: left_bicep} 15 | '7': {name: left_bicep_rotate} 16 | '8': {name: left_shoulder_side} 17 | '9': {name: left_shoulder_up} 18 | servonode: {baud: 115200, port: /dev/ttyACM2} 19 | rightarm: 20 | name: servoBus/rightarm 21 | port: /dev/ttyACM1 22 | servomap: 23 | '0': {name: right_pinky} 24 | '1': {name: right_ring} 25 | '10': {name: rightarm-nc-10} 26 | '11': {name: rightarm-nc-11} 27 | '2': {name: right_middle} 28 | '3': {name: right_index} 29 | '4': {name: right_thumb} 30 | '5': {name: right_hand} 31 | '6': {name: right_bicep} 32 | '7': {name: right_bicep_rotate} 33 | '8': {name: right_shoulder_side} 34 | '9': {name: right_shoulder_up} 35 | servonode: {baud: 115200, port: /dev/ttyACM0} 36 | torso: 37 | name: servoBus/torso 38 | port: /dev/ttyACM2 39 | servomap: 40 | '0': {name: eye_leftright} 41 | '1': {name: eyes_updown} 42 | '10': {name: torso-nc-10} 43 | '11': {name: torso-nc-11} 44 | '2': {name: jaw} 45 | '3': {name: head_leftright} 46 | '4': {name: head_updown} 47 | '5': {name: head_tilt} 48 | '6': {name: waist_lean} 49 | '7': {name: waist_rotate} 50 | '8': {name: torso-nc-8} 51 | '9': {name: torso-nc-9} 52 | servonode: {baud: 115200, port: /dev/ttyACM1} 53 | -------------------------------------------------------------------------------- /inmoov_tools/launch/righteye.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /inmoov_tools/launch/sergei-help.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /inmoov_tools/launch/servobus.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /inmoov_tools/launch/trainer-stack.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /inmoov_tools/launch/videoserver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /inmoov_tools/launch/webcams.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /inmoov_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | inmoov_tools 4 | 0.0.0 5 | The inmoov_tools package 6 | 7 | 8 | 9 | 10 | grey 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | --------------------------------------------------------------------------------