diff --git a/README.md b/README.md
index 54badda..387aa87 100644
--- a/README.md
+++ b/README.md
@@ -3,10 +3,13 @@ This ROS package and associated GUI make use of the 6-axis force/torque sensor o
## Installation
-1. Clone this package to the `src` folder of your catkin workspace
-2. In the root folder of your workspace, install dependencies:
+1. Set up your robot arm for use with the [Universal_Robots_ROS_Driver](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver).
+ - Clone the package to the **src** folder of your catkin workspace
+ - Follow instructions in the [ur_robot_driver/doc](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/tree/master/ur_robot_driver/doc) folder. to set up the URCap on the robot
+2. Clone this package (armstron) to the **src** folder of your catkin workspace
+3. In the root folder of your workspace, install dependencies:
- `rosdep install --from-paths src --ignore-src -r -y`
-3. Navigate to the armstron package folder and install a few extra non-ROS python requirements:
+4. Navigate to the armstron package folder and install a few extra non-ROS python requirements:
- `cd src/armstron/armstron`
- `pip install -r requirements.txt`
5. Navigate back to the workspace folder, and build your workspace (`catkin_make`)
@@ -22,6 +25,7 @@ This ROS package and associated GUI make use of the 6-axis force/torque sensor o
### Use the Armstron test server
1. Start the test server: `roslaunch armstron bringup_testing.launch`
2. Start a test by publishing a goal message to the `/armstron/goal` topic
+
```bash
roslaunch armstron run_test.launch config:="ceti_pull_test.yaml" save:="~/vinst_data/testing_launch.csv"
roslaunch armstron run_test.launch config:="ceti_force_hold.yaml" save:="~/vinst_data/testing_launch.csv"
@@ -30,6 +34,7 @@ This ROS package and associated GUI make use of the 6-axis force/torque sensor o
### Start the GUI
1. Start the test server: `roslaunch armstron bringup_testing.launch`
2. Start the Armstron GUI:
+
```bash
rosrun armstron gui.py
```
diff --git a/armstron/config/test_profiles/ceti_force_hold.yaml b/armstron/config/test_profiles/ceti_force_hold.yaml
index f1128c7..00bef30 100644
--- a/armstron/config/test_profiles/ceti_force_hold.yaml
+++ b/armstron/config/test_profiles/ceti_force_hold.yaml
@@ -1,4 +1,3 @@
-
type: 'sequence'
params:
preload:
diff --git a/armstron/config/test_profiles/ceti_pull_test.yaml b/armstron/config/test_profiles/ceti_pull_test.yaml
index 01056b5..609d29f 100644
--- a/armstron/config/test_profiles/ceti_pull_test.yaml
+++ b/armstron/config/test_profiles/ceti_pull_test.yaml
@@ -1,4 +1,3 @@
-
type: 'sequence'
params:
preload:
diff --git a/armstron/config/test_profiles/ceti_pull_test_diag.yaml b/armstron/config/test_profiles/ceti_pull_test_diag.yaml
index eacab90..1d4375d 100644
--- a/armstron/config/test_profiles/ceti_pull_test_diag.yaml
+++ b/armstron/config/test_profiles/ceti_pull_test_diag.yaml
@@ -1,4 +1,3 @@
-
type: 'sequence'
params:
preload:
diff --git a/armstron/launch/bringup_testing.launch b/armstron/launch/bringup_testing.launch
index 68b263d..9565fd2 100755
--- a/armstron/launch/bringup_testing.launch
+++ b/armstron/launch/bringup_testing.launch
@@ -11,4 +11,13 @@
+
+
+
+
+
\ No newline at end of file
diff --git a/armstron/scripts/run_single_test.py b/armstron/scripts/run_single_test.py
index fad5d98..003df20 100755
--- a/armstron/scripts/run_single_test.py
+++ b/armstron/scripts/run_single_test.py
@@ -1,165 +1,32 @@
#!/usr/bin/env python
-import time
-#import roslib; roslib.load_manifest('ur_driver')
import rospy
import rospkg
-import actionlib
-import yaml
-import json
-import ast
import os
-import sys
-import copy
-import armstron.msg as msg
-from armstron.srv import Balance, Estop
+from armstron.test_interface import TestRunner
-filepath_config = os.path.join(rospkg.RosPack().get_path('armstron'), 'config')
-
-class TestRunner():
- '''
- A ROS Action server to run single tests.
-
- Parameters
- ----------
- name : str
- Name of the Action Server
- '''
-
- def __init__(self, name):
-
- self.DEBUG = rospy.get_param(rospy.get_name()+"/DEBUG",False)
- self._action_name = rospy.get_param(rospy.get_name()+"/action_name",name)
- self.config_file = rospy.get_param(rospy.get_name()+"/config_file",None)
- self.save_file = rospy.get_param(rospy.get_name()+"/save_file",None)
- self.config_path = os.path.join(filepath_config,'test_profiles')
- self.config = self._get_config(os.path.join(self.config_path, self.config_file))
-
- # Make an action client
- self._test_client = actionlib.SimpleActionClient(self._action_name, msg.RunTestAction)
- self._test_client.wait_for_server()
-
- # Make a service proxy for the estop service
-
-
- def _get_config(self, filename):
- '''
- Load a testing profile (config)
-
- Parameters
- ----------
- filename : str or Path
- Filename to load config from
-
- Returns
- -------
- config : dict
- The test config
- '''
- with open(filename, 'r') as f:
- config=yaml.safe_load(f)
- return config
-
-
- def run_test(self):
- '''
- Run a test and wait for the result
-
- Returns
- -------
- result : ActionResult
- The result of the test.
- '''
- # Balance at start
- balance = self.config['balance'].lower()
- if balance!="":
- if 'pose' in balance:
- self.balance('pose')
- if 'ft' in balance:
- self.balance('ft')
-
- # Build the goal message to send
- goal = msg.RunTestGoal()
- goal.command = self.config['type']
- goal.params = json.dumps(self.config['params'])
- goal.filename = self.save_file
-
- # Tell the test server to run the test.
- self._test_client.send_goal(goal)
-
- # Wait for the server to finish performing the test.
- self._test_client.wait_for_result()
-
- # Return the result
- return self._test_client.get_result()
-
-
- def estop(self):
- '''
- Perform an Emergency Stop
- '''
- # Send estop service call
- self._call_service(self._action_name+'/estop',Estop)
-
-
- def balance(self, type):
- '''
- Balance either the load or F/T sensor
-
- Parameters
- ----------
- type : str
- The type of balance to perform. Must be either
- ``pose`` or ``ft``
- '''
- # Send estop service call
- self._call_service(self._action_name+'/balance',Balance, type = str(type))
-
-
- def _call_service(self, name, service_type, **fargs):
- '''
- Call a service and handle the result
-
- Parameters
- ----------
- name : str
- Name of the service to call
- service_type : ROS srv
- ROS service class (must be correct for the service name you are calling)
- **args : Any
- Arguments to pass to the service
-
- Returns
- -------
- response : ROS srv response
- The response from the service. Returns ``None`` if the service
- call was unsuccessful.
- '''
- rospy.wait_for_service(name)
- try:
- fun = rospy.ServiceProxy(name, service_type)
- resp1 = fun(**fargs)
- return resp1
- except rospy.ServiceException as e:
- print("Service call failed: %s"%e)
- return None
-
-
- def shutdown(self):
- '''
- Shut down the node gracefully
- '''
- pass
-
-
-
+filepath_config = os.path.join(rospkg.RosPack().get_path('armstron'), 'config')
+
if __name__ == '__main__':
try:
rospy.init_node('v_inst_test_runner', disable_signals=True)
print("V_INST TEST RUNNER: Node Initiatilized (%s)"%(rospy.get_name()))
- sender = TestRunner(rospy.get_name())
- sender.run_test()
- print("V_INST TEST RUNNER: Ready!")
+
+ debug = rospy.get_param(rospy.get_name()+"/DEBUG",False)
+ action_name = rospy.get_param(rospy.get_name()+"/action_name",None)
+ config_file = rospy.get_param(rospy.get_name()+"/config_file",None)
+ save_file = rospy.get_param(rospy.get_name()+"/save_file",None)
+
+ config_path = os.path.join(filepath_config,'test_profiles',config_file)
+
+ # Set settings
+ sender = TestRunner(action_name, debug=debug)
+ sender.load_profile(config_path)
+ sender.set_savefile(save_file)
+
+ print("V_INST TEST RUNNER: Running Test")
+ sender.run_test(wait_for_finish=True)
+ print("V_INST TEST RUNNER: Finished!")
sender.shutdown()
except KeyboardInterrupt:
diff --git a/armstron/src/armstron/test_interface.py b/armstron/src/armstron/test_interface.py
index 65619d5..447026e 100644
--- a/armstron/src/armstron/test_interface.py
+++ b/armstron/src/armstron/test_interface.py
@@ -12,6 +12,7 @@
import copy
import armstron.msg as msg
+from armstron.utils import load_yaml
from armstron.srv import Balance, Estop
class TestRunner():
@@ -34,9 +35,27 @@ def __init__(self, name, debug=False):
self._test_client.wait_for_server()
+ def load_profile(self, filename):
+ '''
+ Load the config profile from a file
+
+ Parameters
+ ----------
+ filename : str
+ Filename to load
+ '''
+ profile = load_yaml(filename)
+ self.set_profile(profile)
+
+
def set_profile(self, profile):
'''
Set the config profile
+
+ Parameters
+ ----------
+ profile : dict
+ Testing profile to use
'''
self.config = profile
@@ -44,6 +63,11 @@ def set_profile(self, profile):
def set_savefile(self, save_file):
'''
Set the filename to save
+
+ Parameters
+ ----------
+ save_file : str
+ Filename to save data to
'''
self.save_file = save_file
diff --git a/docs/description.rst b/docs/description.rst
index e2c270b..5e4492a 100644
--- a/docs/description.rst
+++ b/docs/description.rst
@@ -24,13 +24,7 @@ The Armstron package allows you to This ROS package and associated GUI make use
Install
=======
-1. Clone this package to the `src` folder of your catkin workspace
-2. In the root folder of your workspace, install dependencies:
- - ``rosdep install --from-paths src --ignore-src -r -y``
-3. Navigate to the armstron package folder and install a few extra non-ROS python requirements:
- - ``cd src/armstron/armstron``
- - ``pip install -r requirements.txt``
-5. Navigate back to the workspace folder, and build your workspace (``catkin_make``)
+Follow instructions in the :ref:`Quickstart Guide `.
diff --git a/docs/examples/build_one.rst b/docs/examples/build_one.rst
deleted file mode 100644
index 00bc46d..0000000
--- a/docs/examples/build_one.rst
+++ /dev/null
@@ -1,101 +0,0 @@
-.. _build_one_example:
-
-====================
-Build One Trajectory
-====================
-
-In this example, we will build one trajectory from a definition file, create an interpolator, and use that interpolator to generate a graph.
-
-Trajectory Definition
----------------------
-
-Trajectories are defined in YAML files as either a series of waypoints, or simple periodic functions. Here we will look at a typical waypoint trajectory:
-
-
-:fa:`file-code-o,fa-primary` `setpoint_traj_demo.yaml `_
-
-.. literalinclude:: ../../examples/traj_setup/setpoint_traj_demo.yaml
-
-
-All trajectories can have three component:
-
-1. ``prefix`` - Runs once at the beginning of the trajectory
-2. ``main`` - Can be looped over and over (ensure the first and last waypoint are equal)
-3. ``suffix`` - Runs once at the end of the trajectory
-
-Within each trajectory component, each waypoint is defined as a list:
-
-* **arg #0** - Time (in seconds)
-* **args #1-N** - Waypoint values for each signal
-
-
-Trajectory Builder
-------------------
-We need to build this trajectory before we can use it. To do this, we use a ``TrajBuilder`` object, and give it a file to load the definition from.
-
-.. code-block:: python
-
- file_to_use = 'traj_setup/setpoint_traj_demo.yaml'
- builder = sorotraj.TrajBuilder() # Make a traj-builder object
- builder.load_traj_def(file_to_use) # Load the file
- traj = builder.get_trajectory()
-
-
-.. note::
-
- We can also directly load a trajectory definition py passing a dictionary to the ``TrajBuilder``: ``builder.set_definition(def_dict)``
-
-Now that we have the trajectory (``traj``), let's plot it:
-
-.. code-block:: python
-
- builder.plot_traj()
-
-.. image:: ../img/example_traj.png
- :alt: Graph of trajectory
-
-
-
-Trajectory Interpolator
------------------------
-
-Now we want to create an interpolator so we can have a convenient way to get trajectory values given arbitrary values of time. We do this with an ``Interpolator`` object. Here, we want to get an interpolator function that loops the ``main`` trajectory component 2 times, with a speed factor of 1.0 (times as defined in the definition), and invert signals 1 and 3 (these are indices).
-
-.. code-block:: python
-
- interp = sorotraj.Interpolator(traj)
-
- # Get the actuation function for the specified run parameters
- actuation_fn, final_time = interp.get_traj_function(
- num_reps=2,
- speed_factor=1.0,
- invert_direction=[1,3])
-
-
-Now that we have an interpolation function we can input a time (or vector of times) and obtain the trajectory at arbirary times. This function is useful when performing simulations of soft systems.
-
-In this example, we go from -1 (before the start of the trajectory) to 20 sec. (well beyond the end of the trajectory).
-
-.. code-block:: python
-
- times = np.linspace(-1,20,2000)
- vals = actuation_fn(times)
-
- plt.figure(figsize=(8,6),dpi=150)
- plt.plot(times, vals)
- plt.show()
-
-
-.. image:: ../img/example_traj_interpolated.png
- :alt: Graph of trajectory
-
-
-Full Code
----------
-
-Here's the whole example in one file:
-
-:fa:`file-code-o,fa-primary` `build_one_trajectory.py `_
-
-
-.. literalinclude:: ../../examples/build_one_trajectory.py
\ No newline at end of file
diff --git a/docs/examples/convert_traj.rst b/docs/examples/convert_traj.rst
deleted file mode 100644
index c599cdb..0000000
--- a/docs/examples/convert_traj.rst
+++ /dev/null
@@ -1,8 +0,0 @@
-====================
-Convert Trajectories
-====================
-
-
-:fa:`file-code-o,fa-primary` `build_convert_trajectories.py `_
-
-.. literalinclude:: ../../examples/build_convert_trajectories.py
\ No newline at end of file
diff --git a/docs/examples/first_test.rst b/docs/examples/first_test.rst
new file mode 100644
index 0000000..7f46095
--- /dev/null
+++ b/docs/examples/first_test.rst
@@ -0,0 +1,130 @@
+.. _first_test:
+
+==========
+Run a Test
+==========
+
+Lets walk through the manual way to build and run a test.
+
+
+.. contents:: Contents:
+ :local:
+ :depth: 1
+
+
+
+Build a Test Profile
+____________________
+
+Test profiles are defined in YAML files inside the `armstron/config/test_profiles `_ folder.
+
+
+Profile Structure
+-----------------
+
+Test profiles must be structured in a specific way:
+
+1. **type** (``str``): The type of profile. For now, the only valid value is "*sequence*"
+2. **params** (``dict``): A set of parameters
+
+ a. **preload** (``list``): A list of actions to perform during the "preload" phase
+ b. **test** (``list``): A list of actions to perform during the "test" phase
+
+
+Testing Actions
+---------------
+
+Actions can be either "motions" or "balancing".
+
+**Motion** steps have motion parameters (how the arm moves) and stop conditions (when the arm should stop moving):
+
+.. code-block:: yaml
+
+ motion: # jog motions about the end effector (TCP of the robot)
+ linear: [X, Y, Z] # [mm/sec]
+ angular: [X, Y, Z] # [rad/sec]
+ stop_conditions:
+ max_time: [TIME] # [sec]
+ max_force_x: [FORCE] # [N], options: min/max, and x/y/z
+ max_torque_x: [TORQUE] # [Nm], options: min/max, and x/y/z
+ max_position_x: [POSITION] # [m], options: min/max, and x/y/z
+ max_orientation_x: [ORI] # [rad], options: min/max, and x/y/z
+
+
+**Balancing** steps can balance (zero) either the pose or the F/T sensor readings:
+
+.. code-block:: yaml
+
+ balance: [TYPE] # options: 'pose' and 'ft'
+
+Examples
+--------
+
+Here is an example of a simple testing profile:
+
+:fa:`file-code-o,fa-primary` `ceti_pull_test.yaml `_
+
+
+.. literalinclude:: ../../armstron/config/test_profiles/ceti_pull_test.yaml
+
+
+Here is an example of a more complex, multi-step testing profile:
+
+:fa:`file-code-o,fa-primary` `ceti_force_hold.yaml `_
+
+
+.. literalinclude:: ../../armstron/config/test_profiles/ceti_force_hold.yaml
+
+
+
+
+Run a Test
+__________
+
+Running a test is just a matter of running one terminal command (after you start the rest of the system up) with the profile you want to use, and the filename to save data.
+
+Testing Procedure
+-----------------
+
+1. Bringup the robot
+
+ a. *(Teach Pendant)* Turn on the robot, get into _manual_ mode, then load the "EXTERNAL_CONTROL.urp" program.
+ b. *(Host Computer)* In a new terminal: ``roslaunch ur_user_calibration bringup_armando.launch``
+ c. *(Teach Pendant)* Run the "EXTERNAL_CONTROL.urp" program.
+
+2. Start the Armstron test server (this waits for tests to be started, and handles balancing and estop commands)
+
+ a. In a new terminal, start the test server: ``roslaunch armstron bringup_testing.launch``
+
+
+3. Start a test (for example, *ceti_pull_test.yaml*), and save data in the Documents folder (*~/Documents/vinst_data/test.csv*)
+
+ a. In a new terminal, run:
+
+.. code-block:: bash
+
+ roslaunch armstron run_test.launch config:="ceti_pull_test.yaml" save:="~/Documents/vinst_data/test.csv"
+
+.. note::
+
+ If you want to run more tests, just keep repeating step 3. Savefile names are auto-incremented to prevent overwriting of data, so you can keep sending the same filename (and thus the same terminal command) over and over to keep repeating the same test procedure.
+
+
+More Details
+------------
+
+When running the test, the launch file you are calling takes care of routing parameters to the correct script:
+
+:fa:`file-code-o,fa-primary` `run_test.launch `_
+
+
+.. literalinclude:: ../../armstron/launch/run_test.launch
+
+
+
+This launch file invokes a ros node that creates a :ref:`TestRunner ` object:
+
+:fa:`file-code-o,fa-primary` `run_single_test.py `_
+
+
+.. literalinclude:: ../../armstron/scripts/run_single_test.py
\ No newline at end of file
diff --git a/docs/examples/gui.rst b/docs/examples/gui.rst
new file mode 100644
index 0000000..4bdc516
--- /dev/null
+++ b/docs/examples/gui.rst
@@ -0,0 +1,14 @@
+.. _gui:
+
+===========
+Use the GUI
+===========
+
+.. code-block:: bash
+
+ rosrun armstron gui.py
+
+
+.. note::
+ GUI instructions Coming soon.
+
diff --git a/docs/examples/index.rst b/docs/examples/index.rst
index 0abf2e0..eca54a3 100644
--- a/docs/examples/index.rst
+++ b/docs/examples/index.rst
@@ -3,4 +3,11 @@
Examples
========
-Some basic examples of how armstron can be used will be include here. You can find them in the `armstron/launch folder `_ in the github repo.
\ No newline at end of file
+Some basic examples of how armstron can be used will be include here. You can find them in the `armstron/launch folder `_ in the github repo.
+
+.. toctree::
+ :maxdepth: 1
+
+
+ first_test
+ gui
\ No newline at end of file
diff --git a/docs/examples/save_traj.rst b/docs/examples/save_traj.rst
deleted file mode 100644
index 71acd9a..0000000
--- a/docs/examples/save_traj.rst
+++ /dev/null
@@ -1,8 +0,0 @@
-===========================
-Build and Save Trajectories
-===========================
-
-
-:fa:`file-code-o,fa-primary` `build_save_trajectories.py `_
-
-.. literalinclude:: ../../examples/build_save_trajectories.py
\ No newline at end of file
diff --git a/docs/reference/hardware_interface.rst b/docs/reference/hardware_interface.rst
index 7d7fb65..b03d7e8 100644
--- a/docs/reference/hardware_interface.rst
+++ b/docs/reference/hardware_interface.rst
@@ -7,7 +7,7 @@ Hardware Interface
Robot Controller
------------------
-Control the robot's motion and get data
+Control the robot's motion and get data. This interface relies on the `Universal_Robots_ROS_Driver `_ package to control UR robots using the builtin ROS controllers.
.. autoclass:: hardware_interface.RobotController
@@ -19,7 +19,7 @@ Control the robot's motion and get data
Data Logger
-----------------------------
- Log data to syncronized files.
+ Log data to syncronized files. Given a filename and a map of topics (see `armstron/config/data_to_save.yaml `_), log data from ROS topics to CSV files.
.. autoclass:: hardware_interface.DataLogger
:members:
diff --git a/docs/reference/test_interface.rst b/docs/reference/test_interface.rst
index 1f2245b..885053e 100644
--- a/docs/reference/test_interface.rst
+++ b/docs/reference/test_interface.rst
@@ -1,8 +1,10 @@
+.. _test_interface_api:
+
==================
Test Interface
==================
-Build trajectories from a definition (either from a yaml file or directly via python dictionary).
+Run and manage tests. This class can be imported into other ROS packages to enable control of tests. This interface is used in the backend of the GUI.
.. automodule:: test_interface