diff --git a/Readme.md b/Readme.md index 9505f86b..91cbf79e 100644 --- a/Readme.md +++ b/Readme.md @@ -27,7 +27,7 @@ Interested in contributing? Check out the [development guidelines](Development.m ## Installation -Check out [installation instructions](INSTALL.md). +Check out [installation instructions](docs/install.md). We are working on experimental install scripts: - [ROS Indigo/Ubuntu 14.04 LTS](install_indigo.sh) diff --git a/costar_bringup/launch/iiwa14_s_model.launch b/costar_bringup/launch/iiwa14_s_model.launch index dcfd9488..edbe6694 100644 --- a/costar_bringup/launch/iiwa14_s_model.launch +++ b/costar_bringup/launch/iiwa14_s_model.launch @@ -40,7 +40,7 @@ - + @@ -72,7 +72,7 @@ args="$(arg gripper_ip_address)"/> - + diff --git a/costar_bringup/launch/s_model.launch b/costar_bringup/launch/s_model.launch index f8e1ea28..d6b8b713 100644 --- a/costar_bringup/launch/s_model.launch +++ b/costar_bringup/launch/s_model.launch @@ -9,15 +9,15 @@ - - + + - - + + diff --git a/costar_bringup/launch/ur5_c_model.launch b/costar_bringup/launch/ur5_c_model.launch index 0c9445c6..a116a337 100644 --- a/costar_bringup/launch/ur5_c_model.launch +++ b/costar_bringup/launch/ur5_c_model.launch @@ -44,7 +44,7 @@ - + diff --git a/costar_bringup/package.xml b/costar_bringup/package.xml index b200751e..bf1ed67d 100644 --- a/costar_bringup/package.xml +++ b/costar_bringup/package.xml @@ -41,10 +41,10 @@ catkin - costar_gripper_manager + gripper_manager costar_robot_manager costar_waypoint_manager - costar_gripper_manager + gripper_manager costar_robot_manager costar_waypoint_manager diff --git a/costar_gripper/costar_gripper_manager/CMakeLists.txt b/costar_gripper/gripper_manager/CMakeLists.txt similarity index 99% rename from costar_gripper/costar_gripper_manager/CMakeLists.txt rename to costar_gripper/gripper_manager/CMakeLists.txt index d61f5ce4..0212becf 100644 --- a/costar_gripper/costar_gripper_manager/CMakeLists.txt +++ b/costar_gripper/gripper_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(costar_gripper_manager) +project(gripper_manager) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/costar_gripper/costar_gripper_manager/LICENSE b/costar_gripper/gripper_manager/LICENSE similarity index 100% rename from costar_gripper/costar_gripper_manager/LICENSE rename to costar_gripper/gripper_manager/LICENSE diff --git a/costar_gripper/costar_gripper_manager/Readme.md b/costar_gripper/gripper_manager/Readme.md similarity index 100% rename from costar_gripper/costar_gripper_manager/Readme.md rename to costar_gripper/gripper_manager/Readme.md diff --git a/costar_gripper/costar_gripper_manager/package.xml b/costar_gripper/gripper_manager/package.xml similarity index 98% rename from costar_gripper/costar_gripper_manager/package.xml rename to costar_gripper/gripper_manager/package.xml index 7cc8324c..7365d883 100644 --- a/costar_gripper/costar_gripper_manager/package.xml +++ b/costar_gripper/gripper_manager/package.xml @@ -1,6 +1,6 @@ - costar_gripper_manager + gripper_manager 0.0.0 The costar_gripper_manager package diff --git a/costar_gripper/costar_gripper_manager/setup.py b/costar_gripper/gripper_manager/setup.py similarity index 88% rename from costar_gripper/costar_gripper_manager/setup.py rename to costar_gripper/gripper_manager/setup.py index 1a3b85b9..80811c24 100644 --- a/costar_gripper/costar_gripper_manager/setup.py +++ b/costar_gripper/gripper_manager/setup.py @@ -5,7 +5,7 @@ d = generate_distutils_setup( ## don't do this unless you want a globally visible script - packages=['costar_gripper'], + packages=['gripper_manager'], package_dir={'': 'src'}, ) diff --git a/costar_gripper/gripper_manager/src/gripper_manager/__init__.py b/costar_gripper/gripper_manager/src/gripper_manager/__init__.py new file mode 100644 index 00000000..afd33378 --- /dev/null +++ b/costar_gripper/gripper_manager/src/gripper_manager/__init__.py @@ -0,0 +1,4 @@ +# This is the equivalent of the CostarArm component. +from costar_gripper import CostarGripper + +__all__ = ["CostarGripper"] diff --git a/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py new file mode 100644 index 00000000..5673d6c3 --- /dev/null +++ b/costar_gripper/gripper_manager/src/gripper_manager/costar_gripper.py @@ -0,0 +1,77 @@ +from costar_component import CostarComponent +from os.path import join +import rospy +from std_srvs.srv import Empty + +# The CoSTAR Gripper component contains a lot less special logic than the +# CoSTAR Arm does. It mostly just has a few tools. +class CostarGripper(CostarComponent): + def __init__(self, + name, #name of the gripper + input_topic, # topic on which we receive messages from the gripper + output_topic, # topic on which we send messages to the gripper + InputMsgType, # type of input message + OutputMsgType, # tpye of output message + GripperPredicatorType, # construct a predicator node to send status info + ns, # operating namespace + verbose, # verbose or not + *args, **kwargs): + + self.verbose = verbose + self.predicator = GripperPredicatorType( + start_subscriber=False, + publish_predicates=True, + gripper_name=name) + + self.sub = rospy.Subscriber(input_topic, InputMsgType, self.status_cb) + self.pub = rospy.Publisher(output_topic, OutputMsgType, queue_size = 100) + self.open = rospy.Service(join(ns,"open"), Empty, self.open_gripper) + self.close = rospy.Service(join(ns,"close"), Empty, self.close_gripper) + self.wide_mode_srv = rospy.Service(join(ns,"wide_mode"), Empty, self.wide_mode) + self.pinch_mode_srv = rospy.Service(join(ns,"pinch_mode"), Empty, self.pinch_mode) + self.basic_mode_srv = rospy.Service(join(ns,"basic_mode"), Empty, self.basic_mode) + self.scissor_mode_srv = rospy.Service(join(ns,"scissor_mode"), Empty, self.scissor_mode) + self.reactivate_srv = rospy.Service(join(ns,"activate"), Empty, self.activate) + self.reset_srv = rospy.Service(join(ns,"reset"), Empty, self.reset) + self.command = self.getDefaultMsg() + + self.activated = True; + + self.activate() + + def getDefaultMsg(self): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def activate(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def reset(self, msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def open_gripper(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def close_gripper(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def wide_mode(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def pinch_mode(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def basic_mode(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def scissor_mode(self,msg=None): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def statusInterpreter(self,status): + raise NotImplementedError('not implemented in CostarGripper base class!') + + def status_cb(self,msg): + if self.verbose: + rospy.loginfo(self.statusInterpreter(msg)) + self.predicator.handle(msg) + + diff --git a/costar_gripper/costar_gripper_robotiq/CMakeLists.txt b/costar_gripper/gripper_robotiq/CMakeLists.txt similarity index 87% rename from costar_gripper/costar_gripper_robotiq/CMakeLists.txt rename to costar_gripper/gripper_robotiq/CMakeLists.txt index 582ef744..f8369dae 100644 --- a/costar_gripper/costar_gripper_robotiq/CMakeLists.txt +++ b/costar_gripper/gripper_robotiq/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 2.8.3) -project(costar_gripper_robotiq) +project(gripper_robotiq) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - costar_gripper_manager + gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control @@ -18,7 +18,7 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -102,8 +102,8 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES costar_gripper_robotiq -# CATKIN_DEPENDS costar_gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control +# LIBRARIES gripper_robotiq +# CATKIN_DEPENDS gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control # DEPENDS system_lib ) @@ -119,24 +119,24 @@ include_directories( ) ## Declare a C++ library -# add_library(costar_gripper_robotiq -# src/${PROJECT_NAME}/costar_gripper_robotiq.cpp +# add_library(gripper_robotiq +# src/${PROJECT_NAME}/gripper_robotiq.cpp # ) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -# add_dependencies(costar_gripper_robotiq ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(gripper_robotiq ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable -# add_executable(costar_gripper_robotiq_node src/costar_gripper_robotiq_node.cpp) +# add_executable(gripper_robotiq_node src/gripper_robotiq_node.cpp) ## Add cmake target dependencies of the executable ## same as for the library above -# add_dependencies(costar_gripper_robotiq_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(gripper_robotiq_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -# target_link_libraries(costar_gripper_robotiq_node +# target_link_libraries(gripper_robotiq_node # ${catkin_LIBRARIES} # ) @@ -155,7 +155,7 @@ include_directories( # ) ## Mark executables and/or libraries for installation -# install(TARGETS costar_gripper_robotiq costar_gripper_robotiq_node +# install(TARGETS gripper_robotiq gripper_robotiq_node # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -180,7 +180,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_costar_gripper_robotiq.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_gripper_robotiq.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/costar_gripper/costar_gripper_manager/launch/c_model.launch b/costar_gripper/gripper_robotiq/launch/c_model.launch similarity index 63% rename from costar_gripper/costar_gripper_manager/launch/c_model.launch rename to costar_gripper/gripper_robotiq/launch/c_model.launch index 523a9939..5530a300 100644 --- a/costar_gripper/costar_gripper_manager/launch/c_model.launch +++ b/costar_gripper/gripper_robotiq/launch/c_model.launch @@ -2,12 +2,12 @@ - + diff --git a/costar_gripper/costar_gripper_manager/launch/robotiq_c_model_endpoint.launch b/costar_gripper/gripper_robotiq/launch/robotiq_c_model_endpoint.launch similarity index 100% rename from costar_gripper/costar_gripper_manager/launch/robotiq_c_model_endpoint.launch rename to costar_gripper/gripper_robotiq/launch/robotiq_c_model_endpoint.launch diff --git a/costar_gripper/costar_gripper_manager/launch/robotiq_s_model_endpoint.launch b/costar_gripper/gripper_robotiq/launch/robotiq_s_model_endpoint.launch similarity index 100% rename from costar_gripper/costar_gripper_manager/launch/robotiq_s_model_endpoint.launch rename to costar_gripper/gripper_robotiq/launch/robotiq_s_model_endpoint.launch diff --git a/costar_gripper/costar_gripper_manager/launch/s_model.launch b/costar_gripper/gripper_robotiq/launch/s_model.launch similarity index 86% rename from costar_gripper/costar_gripper_manager/launch/s_model.launch rename to costar_gripper/gripper_robotiq/launch/s_model.launch index 125ad6cb..ef2c8755 100644 --- a/costar_gripper/costar_gripper_manager/launch/s_model.launch +++ b/costar_gripper/gripper_robotiq/launch/s_model.launch @@ -4,7 +4,7 @@ - + @@ -23,6 +23,6 @@ - + diff --git a/costar_gripper/costar_gripper_robotiq/package.xml b/costar_gripper/gripper_robotiq/package.xml similarity index 87% rename from costar_gripper/costar_gripper_robotiq/package.xml rename to costar_gripper/gripper_robotiq/package.xml index cb4963ce..4580b39e 100644 --- a/costar_gripper/costar_gripper_robotiq/package.xml +++ b/costar_gripper/gripper_robotiq/package.xml @@ -1,8 +1,8 @@ - costar_gripper_robotiq + gripper_robotiq 0.0.0 - The costar_gripper_robotiq package + The gripper_robotiq package @@ -19,7 +19,7 @@ - + @@ -40,11 +40,11 @@ catkin - costar_gripper_manager + gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control - costar_gripper_manager + gripper_manager predicator_robotiq robotiq_c_model_control robotiq_s_model_control @@ -55,4 +55,4 @@ - \ No newline at end of file + diff --git a/costar_gripper/costar_gripper_manager/scripts/c_model.py b/costar_gripper/gripper_robotiq/scripts/c_model.py similarity index 85% rename from costar_gripper/costar_gripper_manager/scripts/c_model.py rename to costar_gripper/gripper_robotiq/scripts/c_model.py index 49423a67..5966acee 100755 --- a/costar_gripper/costar_gripper_manager/scripts/c_model.py +++ b/costar_gripper/gripper_robotiq/scripts/c_model.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import rospy -from costar_gripper import SimpleCModelServer +from gripper_robotiq import SimpleCModelServer rospy.init_node("simple_c_model_server") verbose = rospy.get_param('~verbose',False) diff --git a/costar_gripper/costar_gripper_manager/scripts/s_model.py b/costar_gripper/gripper_robotiq/scripts/s_model.py similarity index 79% rename from costar_gripper/costar_gripper_manager/scripts/s_model.py rename to costar_gripper/gripper_robotiq/scripts/s_model.py index 0d928a16..ca0ef649 100755 --- a/costar_gripper/costar_gripper_manager/scripts/s_model.py +++ b/costar_gripper/gripper_robotiq/scripts/s_model.py @@ -1,7 +1,7 @@ #!/usr/bin/env python import rospy -from costar_gripper import SimpleSModelServer +from gripper_robotiq import SimpleSModelServer rospy.init_node("simple_s_model_server") server = SimpleSModelServer("/costar/gripper") diff --git a/costar_gripper/gripper_robotiq/setup.py b/costar_gripper/gripper_robotiq/setup.py new file mode 100644 index 00000000..f20f9c37 --- /dev/null +++ b/costar_gripper/gripper_robotiq/setup.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + ## don't do this unless you want a globally visible script + packages=['gripper_robotiq'], + package_dir={'': 'src'}, +) + +setup(**d) + diff --git a/costar_gripper/costar_gripper_manager/src/costar_gripper/__init__.py b/costar_gripper/gripper_robotiq/src/gripper_robotiq/__init__.py similarity index 100% rename from costar_gripper/costar_gripper_manager/src/costar_gripper/__init__.py rename to costar_gripper/gripper_robotiq/src/gripper_robotiq/__init__.py diff --git a/costar_gripper/costar_gripper_manager/src/costar_gripper/c_model_server.py b/costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py similarity index 76% rename from costar_gripper/costar_gripper_manager/src/costar_gripper/c_model_server.py rename to costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py index 44996cbe..4a9e1f72 100644 --- a/costar_gripper/costar_gripper_manager/src/costar_gripper/c_model_server.py +++ b/costar_gripper/gripper_robotiq/src/gripper_robotiq/c_model_server.py @@ -34,47 +34,36 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from gripper_manager import CostarGripper + import rospy from robotiq_c_model_control.msg import _CModel_robot_output as outputMsg from robotiq_c_model_control.msg import _CModel_robot_input as inputMsg -from os.path import join -from std_srvs.srv import Empty from predicator_robotiq import CModelPredicator -def getDefaultMsg(): - command = outputMsg.CModel_robot_output(); - command.rACT = 1 - command.rGTO = 1 - command.rSP = 255 - command.rFR = 150 - return command - -class SimpleCModelServer: +class SimpleCModelServer(CostarGripper): def __init__(self,ns="/costar/gripper",verbose=False): - - self.verbose = verbose - self.predicator = CModelPredicator(start_subscriber=False,publish_predicates=True, - gripper_name="c_model") - - self.sub = rospy.Subscriber("CModelRobotInput", inputMsg.CModel_robot_input, self.status_cb) - self.pub = rospy.Publisher('CModelRobotOutput', outputMsg.CModel_robot_output, queue_size = 100) - self.open = rospy.Service(join(ns,"open"), Empty, self.open_gripper) - self.close = rospy.Service(join(ns,"close"), Empty, self.close_gripper) - self.wide_mode_srv = rospy.Service(join(ns,"wide_mode"), Empty, self.wide_mode) - self.pinch_mode_srv = rospy.Service(join(ns,"pinch_mode"), Empty, self.pinch_mode) - self.basic_mode_srv = rospy.Service(join(ns,"basic_mode"), Empty, self.basic_mode) - self.scissor_mode_srv = rospy.Service(join(ns,"scissor_mode"), Empty, self.scissor_mode) - self.reactivate_srv = rospy.Service(join(ns,"activate"), Empty, self.activate) - self.reset_srv = rospy.Service(join(ns,"reset"), Empty, self.reset) - self.command = getDefaultMsg() - - self.activated = True; - - self.activate() + super(SimpleCModelServer, self).__init__( + "c_model", + input_topic="CModelRobotInput", + output_topic="CModelRobotOutput", + InputMsgType=inputMsg.CModel_robot_input, + OutputMsgType=outputMsg.CModel_robot_output, + GripperPredicatorType=CModelPredicator, + ns=ns, + verbose=verbose) + + def getDefaultMsg(cls): + command = outputMsg.CModel_robot_output(); + command.rACT = 1 + command.rGTO = 1 + command.rSP = 255 + command.rFR = 150 + return command def activate(self,msg=None): - self.command = getDefaultMsg() + self.command = self.getDefaultMsg() self.pub.publish(self.command) return [] @@ -114,11 +103,6 @@ def basic_mode(self,msg=None): def scissor_mode(self,msg=None): return [] - def status_cb(self,msg): - if self.verbose: - rospy.loginfo(self.statusInterpreter(msg)) - self.predicator.handle(msg) - def statusInterpreter(self,status): """Generate a string according to the current value of the status variables.""" diff --git a/costar_gripper/costar_gripper_manager/src/costar_gripper/s_model_server.py b/costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py similarity index 85% rename from costar_gripper/costar_gripper_manager/src/costar_gripper/s_model_server.py rename to costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py index 9dbff2b3..00a52a08 100644 --- a/costar_gripper/costar_gripper_manager/src/costar_gripper/s_model_server.py +++ b/costar_gripper/gripper_robotiq/src/gripper_robotiq/s_model_server.py @@ -34,44 +34,36 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +from gripper_manager import CostarGripper + import rospy from robotiq_s_model_control.msg import _SModel_robot_output as outputMsg from robotiq_s_model_control.msg import _SModel_robot_input as inputMsg -from os.path import join -from std_srvs.srv import Empty from predicator_robotiq import SModelPredicator -def getDefaultMsg(): - command = outputMsg.SModel_robot_output(); - command.rACT = 1 - command.rGTO = 1 - command.rSPA = 255 - command.rFRA = 150 - return command - -class SimpleSModelServer: - - def __init__(self,ns="/costar/gripper"): - self.sub = rospy.Subscriber("SModelRobotInput", inputMsg.SModel_robot_input, self.status_cb) - self.pub = rospy.Publisher('SModelRobotOutput', outputMsg.SModel_robot_output) - self.open = rospy.Service(join(ns,"open"), Empty, self.open_gripper) - self.close = rospy.Service(join(ns,"close"), Empty, self.close_gripper) - self.wide_mode_srv = rospy.Service(join(ns,"wide_mode"), Empty, self.wide_mode) - self.pinch_mode_srv = rospy.Service(join(ns,"pinch_mode"), Empty, self.pinch_mode) - self.basic_mode_srv = rospy.Service(join(ns,"basic_mode"), Empty, self.basic_mode) - self.scissor_mode_srv = rospy.Service(join(ns,"scissor_mode"), Empty, self.scissor_mode) - self.reactivate_srv = rospy.Service(join(ns,"activate"), Empty, self.activate) - self.reset_srv = rospy.Service(join(ns,"reset"), Empty, self.reset) - self.command = getDefaultMsg() - - self.predicator = SModelPredicator(start_subscriber=False,publish_predicates=True,gripper_name="s_model") - - self.activated = True; - - self.activate() +class SimpleSModelServer(CostarGripper): + + def __init__(self, ns="/costar/gripper", verbose=False): + super(SimpleSModelServer, self).__init__( + "s_model", + input_topic="SModelRobotInput", + output_topic="SModelRobotOutput", + InputMsgType=inputMsg.SModel_robot_input, + OutputMsgType=outputMsg.SModel_robot_output, + GripperPredicatorType=SModelPredicator, + ns=ns, + verbose=verbose) + + def getDefaultMsg(cls): + command = outputMsg.SModel_robot_output(); + command.rACT = 1 + command.rGTO = 1 + command.rSPA = 255 + command.rFRA = 150 + return command def activate(self,msg=None): - self.command = getDefaultMsg() + self.command = self.getDefaultMsg() self.pub.publish(self.command) return [] @@ -124,10 +116,6 @@ def scissor_mode(self,msg=None): rospy.sleep(0.5) return [] - def status_cb(self,msg): - rospy.loginfo(self.statusInterpreter(msg)) - self.predicator.handle(msg) - def statusInterpreter(self,status): """Generate a string according to the current value of the status variables.""" diff --git a/costar_robot/costar_robot_manager/src/costar_robot/__init__.py b/costar_robot/costar_robot_manager/src/costar_robot/__init__.py index fe993e72..5426a5cb 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/__init__.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/__init__.py @@ -34,3 +34,9 @@ def rospack_exists(module_name, module_exist_list, module_missing_list): else: print 'Missing robot driver: ', missing_robot_driver raise ImportError('No ros robot driver is installed') + + +# UR5 +from inverseKinematicsUR5 import InverseKinematicsUR5 +from ur_driver import CostarUR5Driver +__all__ = ['CostarArm','SimplePlanning','InverseKinematicsUR5','CostarUR5Driver'] diff --git a/costar_robot/costar_robot_manager/src/costar_robot/inverseKinematicsUR5.py b/costar_robot/costar_robot_manager/src/costar_robot/inverseKinematicsUR5.py index a27d9398..ac1c1fde 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/inverseKinematicsUR5.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/inverseKinematicsUR5.py @@ -1,3 +1,8 @@ +# Inverse Kinematics UR5 +# By Felix Jonathan +# (c) 2017 The Johns Hopkins University +# See license for more details + # import PyKDL # import tf_conversions.posemath from math import * @@ -284,7 +289,7 @@ def solveIK(self,forward_kinematics): index += 1 if self.debug: - print 'Number of solution: ', number_of_solution + print 'Number of solutions: ', number_of_solution print Q return Q diff --git a/costar_robot/costar_robot_manager/src/costar_robot/planning.py b/costar_robot/costar_robot_manager/src/costar_robot/planning.py index e9c6f864..db0f8c2d 100644 --- a/costar_robot/costar_robot_manager/src/costar_robot/planning.py +++ b/costar_robot/costar_robot_manager/src/costar_robot/planning.py @@ -1,6 +1,6 @@ -''' -(c) 2016 Chris Paxton -''' +# By Chris Paxton and Felix Jonathan +# (c) 2016-2017 The Johns Hopkins University +# See license for more details import rospy @@ -24,11 +24,22 @@ ModeJoints = 'joints' ModeCart = 'cartesian' +# SIMPLE PLANNING +# This class is really just a wrapper for a bunch of parameters. It exposes +# the various functions that we use to call MoveIt and acts as an interface of +# sorts to generate some nice trajectories. +# +# You could think of this as a sort of sub-component of CoSTAR, if you'd like, +# but it's very closely tied with the way that Arm works in order to produce +# the various behaviors that we are interested in. class SimplePlanning: skip_tol = 1e-6 - def __init__(self,robot,base_link,end_link,group, + # How you set these options will determine how we do planning: + # what inverse kinematics are used for queries, etc. Most of these are + # meant to be directly inherited/provided by the CoSTAR Arm class. + def __init__(self, robot, base_link, end_link, group, move_group_ns="move_group", planning_scene_topic="planning_scene", robot_ns="", @@ -57,9 +68,9 @@ def __init__(self,robot,base_link,end_link,group, self.verbose = verbose self.closed_form_IK_solver = closed_form_IK_solver - ''' - ik: handles calls to KDL inverse kinematics - ''' + # Basic ik() function call. + # It handles calls to KDL inverse kinematics or to the closed form ik + # solver that you provided. def ik(self, T, q0, dist=0.5): q = None if self.closed_form_IK_solver is not None: @@ -68,26 +79,30 @@ def ik(self, T, q0, dist=0.5): else: q = self.kdl_kin.inverse(T,q0) - # NOTE: this results in unsafe behavior; do not use without checks - #if q is None: - # q = self.kdl_kin.inverse(T) return q + # Compute parameters for a nice trapezoidal motion. This will let us create + # movements with the basic move() operation that actually look pretty nice. def calculateAccelerationProfileParameters(self, - dq_to_target, - base_steps, - steps_per_meter, - steps_per_radians, + dq_to_target, # joint space offset to target + base_steps, # number of trajectory points to create + steps_per_meter, # number of extra traj pts to make + steps_per_radians, # as above delta_translation, time_multiplier, percent_acc): + # We compute a number of steps to take along our trapezoidal trajectory + # curve. This gives us a nice, relatively dense trajectory that we can + # introspect on later -- we can use it to compute cost functions, to + # detect collisions, etc. delta_q_norm = np.linalg.norm(dq_to_target) - steps = base_steps + delta_translation * steps_per_meter + delta_q_norm * steps_per_radians + steps = base_steps + delta_translation * steps_per_meter + delta_q_norm \ + * steps_per_radians + # Number of steps must be an int. steps = int(np.round(steps)) - print " -- Computing %d steps"%steps - # the time needed for constant velocity at 100% to reach the goal + # This is the time needed for constant velocity at 100% to reach the goal. t_v_constant = delta_translation + delta_q_norm ts = (t_v_constant / steps ) * time_multiplier @@ -100,17 +115,26 @@ def calculateAccelerationProfileParameters(self, # j_acceleration = np.array(dq_target) / t_v_constant * percent_acc t_v_setting_max = v_setting_max / acceleration - steps_to_max_speed = 0.5 * acceleration * t_v_setting_max **2 / (dq_max_target / steps) + # Compute the number of trajectory points we want to make before we will + # get up to max speed. + steps_to_max_speed = 0.5 * acceleration * t_v_setting_max **2 \ + / (dq_max_target / steps) if steps_to_max_speed * 2 > steps: - print "-- Cannot reach the maximum velocity setting, steps required %.1f > total number of steps %d"%(steps_to_max_speed * 2,steps) + rospy.logwarn("Cannot reach the maximum velocity setting, steps " + "required %.1f > total number of steps %d"%(steps_to_max_speed * 2,steps)) t_v_setting_max = np.sqrt(0.5 * dq_max_target / acceleration) steps_to_max_speed = (0.5 * steps) v_setting_max = t_v_setting_max * acceleration - print "-- Acceleration number of steps is set to %.1f and time elapsed to reach max velocity is %.3fs"%(steps_to_max_speed,t_v_setting_max) + + rospy.loginfo("Acceleration number of steps is set to %.1f and time " + "elapsed to reach max velocity is %.3fs"%(steps_to_max_speed, + t_v_setting_max)) + const_velocity_max_step = np.max([steps - 2 * steps_to_max_speed, 0]) return steps, ts, t_v_setting_max, steps_to_max_speed, const_velocity_max_step + # This is where we compute what time we want for each trajectory point. def calculateTimeOfTrajectoryStep(self, step_index, steps_to_max_speed, @@ -130,6 +154,8 @@ def calculateTimeOfTrajectoryStep(self, return acceleration_time + const_vel_time + deceleration_time + # Compute a nice joint trajectory. This is useful for checking collisions, + # and ensuring that we have nice, well defined behavior. def getJointMove(self, q_goal, q0, @@ -212,6 +238,7 @@ def getJointMove(self, traj.joint_names = self.joint_names return traj + # Compute a simple trajectory. def getCartesianMove(self, frame, q0, base_steps=1000, steps_per_meter=1000, diff --git a/costar_tools/smart_waypoint_manager/CMakeLists.txt b/costar_robot/smart_waypoint_manager/CMakeLists.txt similarity index 100% rename from costar_tools/smart_waypoint_manager/CMakeLists.txt rename to costar_robot/smart_waypoint_manager/CMakeLists.txt diff --git a/costar_tools/smart_waypoint_manager/package.xml b/costar_robot/smart_waypoint_manager/package.xml similarity index 100% rename from costar_tools/smart_waypoint_manager/package.xml rename to costar_robot/smart_waypoint_manager/package.xml diff --git a/costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py b/costar_robot/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py similarity index 100% rename from costar_tools/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py rename to costar_robot/smart_waypoint_manager/scripts/test_smart_waypoint_manager.py diff --git a/costar_tools/smart_waypoint_manager/setup.py b/costar_robot/smart_waypoint_manager/setup.py similarity index 100% rename from costar_tools/smart_waypoint_manager/setup.py rename to costar_robot/smart_waypoint_manager/setup.py diff --git a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py b/costar_robot/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py similarity index 100% rename from costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py rename to costar_robot/smart_waypoint_manager/src/smart_waypoint_manager/__init__.py diff --git a/costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py b/costar_robot/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py similarity index 100% rename from costar_tools/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py rename to costar_robot/smart_waypoint_manager/src/smart_waypoint_manager/smart_waypoint_manager.py diff --git a/docs/design.md b/docs/design.md new file mode 100644 index 00000000..7966f6ef --- /dev/null +++ b/docs/design.md @@ -0,0 +1,29 @@ + +# Architecture Design + +CoSTAR takes a page from most ROS system design. + +### Arm + +This component is extended to implement functionality for specific robots: + - IIWA + - UR5 + - DVRK + +The `Arm` component has a few associated helper classes, created to isolate complex functionality: + - `SimplePlanning`, which wraps a MoveIt trajectory interface and creates a few other things. + - `InverseKinematicsUR5`, which as its name implies does inverse kinemetics for our UR5. It is a closed-form IK solver, and is fairly specific to our robot as a result. + +### Gripper + +The gripper is configured so that we can set it into different modes, depending on what gripper is currently available. + + +## Predicator + + +## Operations + +## Instructor + +Instructor is our BT-based UI. It is closely tied with [beetree](external/beetree/README.md). diff --git a/docs/gazebo.md b/docs/gazebo.md new file mode 100644 index 00000000..4c6b0dec --- /dev/null +++ b/docs/gazebo.md @@ -0,0 +1,51 @@ +# Gazebo + +In general, you should be able to install Gazebo from binary for your ROS distro. These are some rough notes for you if that does not work. + +## 14.04 + +Dependencies: + +``` +sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' +``` + +Setup keys and update: + +``` +wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - +sudo apt-get update +``` + +(From OSRF) Now install prerequisites. A clean Ubuntu system will need the following: + +``` +wget https://bitbucket.org/osrf/release-tools/raw/default/jenkins-scripts/lib/dependencies_archive.sh -O /tmp/dependencies.sh +ROS_DISTRO=indigo GAZEBO_VERSION=7 . /tmp/dependencies.sh +sudo apt-get install $(sed 's:\\ ::g' <<< $BASE_DEPENDENCIES) $(sed 's:\\ ::g' <<< $GAZEBO_BASE_DEPENDENCIES) +``` + +We need 7 because that is the last one that supports 14.04. + +Install these other dependencies: + +``` +# Only needed on Trusty. Ubuntu packages since Utopic. +sudo apt-add-repository ppa:libccd-debs +sudo apt-add-repository ppa:fcl-debs + +# Main repository +sudo apt-add-repository ppa:dartsim +sudo apt-get update +sudo apt-get install libdart-core5-dev libccd-dev +``` + +Alright, that should at least let you compile Gazebo. + +## 16.04 + +Not yet supported. Hope the binaries work. + +## References + + - [Installing Gazebo from source](http://gazebosim.org/tutorials?tut=install_from_source) diff --git a/INSTALL.md b/docs/install.md similarity index 91% rename from INSTALL.md rename to docs/install.md index 08d37287..524c216e 100644 --- a/INSTALL.md +++ b/docs/install.md @@ -1,8 +1,6 @@ # How to Install CoSTAR -Instructions by Baichuan Jiang - -Note: CoSTAR installation has only been tested on ROS Indigo on Ubuntu 14.04 machine. For instructions on Indigo installation, please see [here](http://wiki.ros.org/indigo/Installation/Ubuntu). There is a prototype install script available [here](install_indigo.sh) that you can try out as well. +Note: CoSTAR installation has only been tested on ROS Indigo (Ubuntu 14.04 LTS). For instructions on Indigo installation, please see [here](http://wiki.ros.org/indigo/Installation/Ubuntu). There is a prototype install script available [here](install_indigo.sh) that you can try out as well. ## Prerequisites @@ -91,3 +89,7 @@ The top should say "Robot Mode: Idle." If you installed the sample workspace, op CoSTAR is currently set up to launch our two testbed systems: a KUKA LBR iiwa 14 with a 3-finger Robotiq gripper and a Universal Robots UR5 with a 2-finger Robotiq gripper. We plan to add some funcitonality to support additional platforms. If you are interested in supporting another platform or run into other issues trying to run this code, please contact Chris Paxton (cpaxton@jhu.edu). + +## Other Information + + - [Notes on installing Gazebo](docs/gazebo.md)