Merge branch 'devel'

CHANGELOG.md

@@ -1,6 +1,21 @@
 Change log
 ==========
 
+1.3.0 (2016-01-08)
+==================
+
+* API changes:
+  * Python: now import as a package using `import dvrk_python.robot`
+* Deprecated features:
+  * None
+* New features:
+  * Potentiometer calibration script: `dvrk_robot/scripts/dvrk_calibrate_potentiometers.py`
+  * dvrk_robot: added low level IO data colelction for pots/joints/actuators (see potentiometer calibration wiki)
+  * dvrk_robot: added set_wrench topics
+  * dvrk_robot: console supports kinematic simulation with RViz (optional)
+* Bug fixes:
+  * None
+
 1.1.1 (2015-10-18)
 ==================
 

dvrk_python/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required (VERSION 2.8.3)
+project (dvrk_python)
+
+## 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
+              rospy
+              )
+
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package (
+#  INCLUDE_DIRS include
+#  LIBRARIES my_pkg
+   CATKIN_DEPENDS rospy
+#  DEPENDS system_lib
+)
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+catkin_python_setup ()

dvrk_python/README.md

@@ -0,0 +1,39 @@
+## Introduction
+
+This directory contains:
+* dvrk_python module.  This modules defines the class `robot` which uses the dVRK ROS topics to communicate with the `dvrk_console_json` application included in the `dvrk_robot` ROS package.
+* tutorial examples.  Python scripts using the `dvrk_python` module.
+
+## Requirements
+
+You will need to build the dVRK software stack using the ROS catkin build tools, see https://github.com/jhu-dvrk/sawIntuitiveResearchKit/wiki/CatkinBuild
+
+The `dvrk_python` package is included in the `dvrk_ros` git repository.  Please checkout the whole `dvrk_ros` repository and build it using the catkin build tools (don't use `catkin_make`).
+
+You will also need some extra Python packages:
+```sh
+   sudo apt-get install ros-hydro-python-orocos-kdl ros-hydro-orocos-kinematics-dynamics ros-hydro-tf
+```
+
+## Runtime
+
+You first need to start the dVRK console application:
+```sh
+  rosrun dvrk_robot dvrk_console_json -j your_console_config_file.json
+```
+
+Once the console is running, you can check which arms are available using the `rostopic` command:
+```sh
+  rostopic list
+```
+
+You should see one namespace per arm under `/dvrk`, e.g. `/dvrk/PSM1`, `/dvrk/MTML` and/or `/dvrk/ECM` based on your console configuration.
+
+Then in Python:
+```python
+from dvrk_python.robot import *
+mtml = robot('MTML')
+mtml.home()
+mtml.get_current_joint_position()
+...
+```

dvrk_python/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package>
+  <name>dvrk_python</name>
+  <version>1.3.0</version>
+  <description>The dVRK Python package</description>
+
+  <maintainer email="[email protected]">Anton Deguet</maintainer>
+  <license>CISST</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>rospy</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+  </export>
+
+</package>

dvrk_python/setup.py

@@ -0,0 +1,10 @@
+ ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(packages=['dvrk_python'],
+                                      package_dir={'': 'src'},
+                                      )
+
+setup(**setup_args)

dvrk_python/src/dvrk_python/__init__.py

@@ -0,0 +1 @@
+# __all__ = ["robot.py"]

dvrk_python/src/robot.py → dvrk_python/src/dvrk_python/robot.py

@@ -77,6 +77,7 @@
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Vector3
 from geometry_msgs.msg import Quaternion
+from geometry_msgs.msg import Wrench
 from sensor_msgs.msg import JointState
 
 from code import InteractiveConsole
@@ -128,18 +129,22 @@ def __init__(self, robot_name, ros_namespace = '/dvrk/'):
         # publishers
         frame = Frame()
         full_ros_namespace = self.__ros_namespace + self.__robot_name
-        self.set_robot_state = rospy.Publisher(full_ros_namespace + '/set_robot_state',
-                                               String, latch=True, queue_size=1)
-        self.set_position_joint = rospy.Publisher(full_ros_namespace + '/set_position_joint',
-                                                  JointState, latch=True, queue_size=1)
-        self.set_position_goal_joint = rospy.Publisher(full_ros_namespace + '/set_position_goal_joint',
-                                                       JointState, latch=True, queue_size=1)
-        self.set_position_cartesian = rospy.Publisher(full_ros_namespace + '/set_position_cartesian',
-                                                      Pose, latch=True, queue_size=1)
-        self.set_position_goal_cartesian = rospy.Publisher(full_ros_namespace + '/set_position_goal_cartesian',
-                                                           Pose, latch=True, queue_size=1)
-        self.set_jaw_position = rospy.Publisher(full_ros_namespace + '/set_jaw_position',
-                                                Float32, latch=True, queue_size=1)
+        self.set_robot_state_publisher = rospy.Publisher(full_ros_namespace + '/set_robot_state',
+                                                         String, latch=True, queue_size = 1)
+        self.set_position_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_joint',
+                                                            JointState, latch=True, queue_size = 1)
+        self.set_position_goal_joint_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_joint',
+                                                                 JointState, latch=True, queue_size = 1)
+        self.set_position_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_cartesian',
+                                                                Pose, latch=True, queue_size = 1)
+        self.set_position_goal_cartesian_publisher = rospy.Publisher(full_ros_namespace + '/set_position_goal_cartesian',
+                                                                     Pose, latch=True, queue_size = 1)
+        self.set_jaw_position_publisher = rospy.Publisher(full_ros_namespace + '/set_jaw_position',
+                                                          Float32, latch=True, queue_size = 1)
+        self.set_wrench_body_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_body',
+                                                         Wrench, latch=True, queue_size = 1)
+        self.set_wrench_spatial_publisher = rospy.Publisher(full_ros_namespace + '/set_wrench_spatial',
+                                                            Wrench, latch=True, queue_size = 1)
 
         # subscribers
         rospy.Subscriber(full_ros_namespace + '/robot_state',
@@ -212,7 +217,7 @@ def __dvrk_set_state(self, state, timeout = 5):
         if (self.__robot_state == state):
             return True
         self.__robot_state_event.clear()
-        self.set_robot_state.publish(state)
+        self.set_robot_state_publisher.publish(state)
         self.__robot_state_event.wait(timeout)
         # if the state is not changed return False
         if (self.__robot_state != state):
@@ -225,7 +230,7 @@ def home(self):
         the robot. This method requries the robot name."""
         rospy.loginfo(rospy.get_caller_id() + ' -> start homing')
         self.__robot_state_event.clear()
-        self.set_robot_state.publish('Home')
+        self.set_robot_state_publisher.publish('Home')
         counter = 10 # up to 10 transitions to get ready
         while (counter > 0):
             self.__robot_state_event.wait(20) # give up to 20 secs for each transition
@@ -346,9 +351,9 @@ def __check_input_type(self, input, type_list):
             while i < len(type_list):
                 print_medium2 = ' '+ str(type_list[i])
                 print_type2 += print_medium2
-                if(type_list[i] == list):
-                    i+=1
-                i+=1
+                if (type_list[i] == list):
+                    i += 1
+                i += 1
             print print_type2
         return False
 
@@ -376,18 +381,18 @@ def close_gripper(self):
         "Close the arm gripper"
         if (not self.__dvrk_set_state('DVRK_POSITION_GOAL_CARTESIAN')):
             return False
-        self.set_jaw_position.publish(-10.0 * math.pi / 180.0);
+        self.set_jaw_position_publisher.publish(-10.0 * math.pi / 180.0);
 
     def open_gripper(self):
         "Open the arm gripper"
         if (not self.__dvrk_set_state('DVRK_POSITION_GOAL_CARTESIAN')):
             return False
-        self.set_jaw_position.publish(80.0 * math.pi / 180.0);
+        self.set_jaw_position_publisher.publish(80.0 * math.pi / 180.0);
 
     def delta_move_cartesian(self, delta_input, interpolate=True):
-        """Incremental translation in cartesian space.
+        """Incremental motion in cartesian space.
 
-        :param delta_input: the incremental translation you want to make
+        :param delta_input: the incremental motion you want to make
         :param interpolate: see  :ref:`interpolate <interpolate>`
         """
         rospy.loginfo(rospy.get_caller_id() + ' -> starting delta move cartesian translation')
@@ -536,7 +541,7 @@ def __move_cartesian_direct(self, end_frame):
         if (not self.__dvrk_set_state('DVRK_POSITION_CARTESIAN')):
             return False
         # go to that position directly
-        self.set_position_cartesian.publish(end_position)
+        self.set_position_cartesian_publisher.publish(end_position)
         rospy.loginfo(rospy.get_caller_id() + ' <- completing move cartesian direct')
         return True
 
@@ -564,7 +569,7 @@ def __set_position_goal_cartesian_publish_and_wait(self, end_position):
         # the goal is originally not reached
         self.__goal_reached = False
         # recursively call this function until end is reached
-        self.set_position_goal_cartesian.publish(end_position)
+        self.set_position_goal_cartesian_publisher.publish(end_position)
         self.__goal_reached_event.wait(20) # 1 minute at most
         if not self.__goal_reached:
             return False
@@ -656,7 +661,7 @@ def __move_joint_direct(self, end_joint):
             # go to that position directly
             joint_state = JointState()
             joint_state.position[:] = end_joint
-            self.set_position_joint.publish(joint_state)
+            self.set_position_joint_publisher.publish(joint_state)
             rospy.loginfo(rospy.get_caller_id() + ' <- completing move joint direct')
             return True
 
@@ -683,9 +688,35 @@ def __set_position_goal_joint_publish_and_wait(self, end_position):
         :rtype: Bool"""
         self.__goal_reached_event.clear()
         self.__goal_reached = False
-        self.set_position_goal_joint.publish(end_position)
+        self.set_position_goal_joint_publisher.publish(end_position)
         self.__goal_reached_event.wait(20) # 1 minute at most
         if not self.__goal_reached:
             return False
         rospy.loginfo(rospy.get_caller_id() + ' -> completing set position goal joint publish and wait')
         return True
+
+    def set_wrench_spatial_force(self, force):
+        "Apply a wrench with force only (spatial), torque is null"
+        if (not self.__dvrk_set_state('DVRK_EFFORT_CARTESIAN')):
+            return False
+        w = Wrench()
+        w.force.x = force[0]
+        w.force.y = force[1]
+        w.force.z = force[2]
+        w.torque.x = 0.0
+        w.torque.y = 0.0
+        w.torque.z = 0.0
+        self.set_wrench_spatial_publisher.publish(w)
+
+    def set_wrench_body_force(self, force):
+        "Apply a wrench with force only (body), torque is null"
+        if (not self.__dvrk_set_state('DVRK_EFFORT_CARTESIAN')):
+            return False
+        w = Wrench()
+        w.force.x = force[0]
+        w.force.y = force[1]
+        w.force.z = force[2]
+        w.torque.x = 0.0
+        w.torque.y = 0.0
+        w.torque.z = 0.0
+        self.set_wrench_body_publisher.publish(w)

dvrk_python/tutorial/effort_test.py

@@ -0,0 +1,26 @@
+from robot import *
+import time
+
+def effort_test(robotName):
+    r=robot(robotName)
+
+    while(r.get_desired_joint_effort()[2] < 1):
+        print r.get_desired_joint_effort()[2]
+        r.delta_move_cartesian_translation([0.0,0.0,-0.001])
+        time.sleep(.3)
+    print r.get_desired_joint_effort()[2]
+
+
+
+
+
+
+
+
+
+
+if __name__ == '__main__':
+    if (len(sys.argv) != 2):
+        print sys.argv[0] + ' requires one argument, i.e. name of dVRK arm'
+    else:
+        effort_test(sys.argv[1])

dvrk_python/tutorial/power_off.py

@@ -0,0 +1,19 @@
+"""In example we will learn how to turn off the robot. We will show how the following methods work:
+  * shutdown()
+
+Lets take a look:"""
+
+# we are using the robot api and therefore we will need to import this class
+from dvrk_python.robot import *
+
+# check if we are given the name of the robot arm or not, if we are we can create a robot and call shutdown()
+if (len(sys.argv) != 2):
+    print sys.argv[0] + ' requires one argument, i.e. name of dVRK arm'
+
+else:
+    robotName = sys.argv[1]
+    # initialize the robot, in this demo we called the robot r, and power the robot.
+    r = robot(robotName)
+
+    # shutdown() will turn off the robot
+    r.shutdown()