v1.0.0 (#3)

* Rename mr_desc_param.py script to mr_desc_param
* Add command line args to mr_desc_param
* Tool now takes a dict instead of kwargs
* Changed Tool arg robot_namespace to namespace
* Changed License holder
* Improved README with more detailed usage info
* Add requirements.txt file
* Added ROS_PYTHON_VERSION conditionals to package dependencies
* Change required version of scipy

Author: lukeschmitt-tr

CHANGELOG.rst

@@ -2,6 +2,18 @@
 Changelog for package kinematics_from_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.0.0 (2020-04-26)
+------------------
+* Rename mr_desc_param.py script to mr_desc_param
+* Add command line args to mr_desc_param
+* Tool now takes a dict instead of kwargs
+* Changed Tool arg robot_namespace to namespace
+* Changed License holder
+* Improved README with more detailed usage info
+* Add requirements.txt file
+* Added ROS_PYTHON_VERSION conditionals to package dependencies
+* Change required version of scipy
+
 0.0.2 (2021-01-13)
 ------------------
 * (`#1 <https://github.com/Interbotix/kinematics_from_description/pull/1>`_)

LICENSE

@@ -1,6 +1,6 @@
 BSD 2-Clause License
 
-Copyright (c) 2022, Luke Schmitt
+Copyright (c) 2022, Trossen Robotics
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

README.md

@@ -2,73 +2,150 @@
 
 ## Overview
 
-This package contains a simple Python program that calculates some properties required to run parts of the [Modern Robotics Library](https://github.com/NxRLab/ModernRobotics) from a robot's URDF or robot_description parameter. Specifically, this program calculates the M and Slist parameters required by the MR inverse kinematics functions:
+This package contains a simple Python program that calculates some properties required to run parts of the [Modern Robotics Library](https://github.com/NxRLab/ModernRobotics) from a robot's URDF or robot_description parameter. Specifically, this program calculates the M and Slist parameters required by MR kinematics functions:
 
-- The **M** matrix is the position and orientation of the end effector frame when the robot is in its home position (i.e. all joints are at position 0).
-- The **Slist** matrix is the joint screw axes in the end effector frame when the robot is in its home position. This matrix is formatted to have each axis as a column.
+- The **M** matrix is the end effector configuration in SE(3) when the robot is in its home position (i.e. all joints are at position 0).
+- The **Slist** matrix is the joint screw axes expressed in the space frame when the robot is in its home position. This matrix is formatted to have each axis as a column.
+
+Note that this package is only compatible with ROS Noetic and Python3. However, it can be used other ROS distributions by installing using the requirements.txt and using the Importing method described below.
 
 ## Dependencies
 
 The dependencies of this software package are:
 
 - urdf_parser_py
 - numpy
-- scipy
+- scipy>=1.2.0 (for the scipy.spatial.transform module)
 - pyyaml
 
-You can use rosdep or the setup file to install the required dependencies.
-
-Note that this package is only compatible with  ROS Noetic and Python3.
+You can use rosdep (only on Noetic), the requirements.txt file, or the setup.py script to install the required dependencies.
 
 ## Usage
 
 You must specify three parameters before using this library:
 
-- **namespace** - The namespace of the robot_description parameter and the namespace that each link will be listed under in the URDF. For example, if the robot is named "vx300s", a link may be listed like "vx300s/shoulder_link". If no namespace is used, leave this as an empty string, i.e. `''`.
+- **namespace** - The namespace of the robot_description parameter and the namespace that each link will be listed under in the URDF. For example, if the robot is named `vx300s`, a link may be named something like `vx300s/shoulder_link`. If no namespace is used, leave this as an empty string, i.e. `''`.
 - **space_frame** - The name given to the link that will serve as the location of the robot's base, or {0} link.
 - **body_frame** - The name given to the link that will serve as the location of the end effector.
 
 There are two primary ways to use this library: importing the module, and using rosrun.
 
 ### Importing
 
-The KinematicsFromDescription Tool can be imported into your Python3 script.
+The KinematicsFromDescription Tool can be imported into your Python script. The Tool accepts a single positional argument - a dictionary with the keys `space_frame`, `body_frame`, and `namespace`. See the [config file](config/config.yaml) for details.
 
 ```python
 from kinematics_from_description.kfd import KinematicsFromDescriptionTool as KFD
 
 ...
 
 tool = KFD(
-    space_frame="space_frame", 
-    body_frame="body_frame",
-    robot_namespace="namespace")
+    {
+        space_frame: "space_frame",
+        body_frame: "body_frame",
+        namespace: "namespace"
+    }
 tool.load_desc_from_file(filepath)
 tool.run()
 ```
 
 ### Using rosrun
 
-Since this package is catkin-ized, it can be launched using ROS. 
+Since this package is catkin-ized, it can be launched using ROS.
 
-1. Modify the [config.yaml](./config/config.yaml) file to match your description setup.
-2. Launch your robot's robot_description package to load the robot_description parameter into ROS' parameter server.
-3. Run the [mr_desc_param.py](./scripts/mr_desc_param.py) script using rosrun.
+#### Using a configuration file:
+ - Modify the [config.yaml](./config/config.yaml) file to match your description setup.
+ - Launch your robot's robot_description package to load the robot_description parameter into ROS' parameter server.
+ - Run the [mr_desc_param](./scripts/mr_desc_param) script using rosrun, specifying the location of the config file.
 
 ```console
-$ rosrun kinematics_from_description mr_desc_param.py
+$ rosrun kinematics_from_description mr_desc_param -f /path/to/config.yaml
+
+Loading configs from '/path/to/config.yaml'
+Using configs:
+        space_frame: base_link
+        body_frame:  ee_gripper_link
+        namespace:   wx200
+        precision:   6
+
+The MR Descriptions are as follows:
 
 M:
-[[1.       0.       0.       0.248575]
- [0.       1.       0.       0.      ]
- [0.       0.       1.       0.1931  ]
- [0.       0.       0.       1.      ]]
+array([[1.      , 0.      , 0.      , 0.408575],
+       [0.      , 1.      , 0.      , 0.      ],
+       [0.      , 0.      , 1.      , 0.31065 ],
+       [0.      , 0.      , 0.      , 1.      ]])
 
 Slist:
-[[ 0.      0.      1.      0.      0.      0.    ]
- [ 0.      1.      0.     -0.0931  0.      0.    ]
- [ 0.      1.      0.     -0.1931  0.      0.035 ]
- [ 0.      1.      0.     -0.1931  0.      0.135 ]]
+array([[ 0.     ,  0.     ,  1.     ,  0.     ,  0.     ,  0.     ],
+       [ 0.     ,  1.     ,  0.     , -0.11065,  0.     ,  0.     ],
+       [ 0.     ,  1.     ,  0.     , -0.31065,  0.     ,  0.05   ],
+       [ 0.     ,  1.     ,  0.     , -0.31065,  0.     ,  0.25   ],
+       [ 1.     ,  0.     ,  0.     ,  0.     ,  0.31065,  0.     ]]).T
+
+Paste these arrays into your mr_descriptions.py file to use in the Interbotix Python-ROS API.
+```
+
+#### Using command line arguments.
+ - Specify the `namespace`, `body_frame`, and `space_frame` configs from the command line.
+
+```console
+$ rosrun kinematics_from_description mr_desc_param -n wx200 -b ee_gripper_link -s base_link -p 6
+
+Using configs:
+        space_frame: base_link
+        body_frame:  ee_gripper_link
+        namespace:   wx200
+        precision:   6
+
+The MR Descriptions are as follows:
+
+M:
+array([[1.      , 0.      , 0.      , 0.408575],
+       [0.      , 1.      , 0.      , 0.      ],
+       [0.      , 0.      , 1.      , 0.31065 ],
+       [0.      , 0.      , 0.      , 1.      ]])
+
+Slist:
+array([[ 0.     ,  0.     ,  1.     ,  0.     ,  0.     ,  0.     ],
+       [ 0.     ,  1.     ,  0.     , -0.11065,  0.     ,  0.     ],
+       [ 0.     ,  1.     ,  0.     , -0.31065,  0.     ,  0.05   ],
+       [ 0.     ,  1.     ,  0.     , -0.31065,  0.     ,  0.25   ],
+       [ 1.     ,  0.     ,  0.     ,  0.     ,  0.31065,  0.     ]]).T
+
+Paste these arrays into your mr_descriptions.py file to use in the Interbotix Python-ROS API.
+```
+
+See usage details with the `-h` flag.
+```console
+$ rosrun kinematics_from_description mr_desc_param -h
+
+usage: mr_desc_param [-h] [--file FILE] [--space_frame SPACE_FRAME]
+                     [--body_frame BODY_FRAME] [--namespace NAMESPACE]
+                     [--precision PRECISION]
+
+A program that will parse a robot's kinematic properties from its
+`/robot_description` rosparam.
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --file FILE, -f FILE  The path to the config file to use.
+  --space_frame SPACE_FRAME, -s SPACE_FRAME
+                        The name given to the link that will serve as the
+                        location of the robot's base, or {0} link. This is
+                        typically `base_link`.
+  --body_frame BODY_FRAME, -b BODY_FRAME
+                        The name given to the link that will serve as the
+                        location of the end effector. This is typically
+                        `ee_gripper_link`.
+  --namespace NAMESPACE, -n NAMESPACE
+                        The namespace under which the `/robot_description`
+                        rosparam will be found, as well as the names of each
+                        link (i.e. `/namespace/robot_description`, and
+                        `namespace/base_link`. (default: )
+  --precision PRECISION, -p PRECISION
+                        The precision with which to print the kinematic
+                        properties. (default: 6)
 ```
 
 ## Contributors
@@ -79,5 +156,4 @@ Slist:
 
 - Include other useful variables like Blist
 - Allow for joint types other than revolute
-- Incorporate error handling
-- Improve robustness
+- Allow for different rosparam and joint namespaces

config/config.yaml

@@ -1,4 +1,12 @@
+# The namespace under which the `/robot_description` rosparam will be found, as well as
+# the names of each link (i.e. `/namespace/robot_description`, and `namespace/base_link`).
 namespace: ''
+
+# The name given to the link that will serve as the location of the robot's base, or {0} link.
 space_frame: ''
+
+# The name given to the link that will serve as the location of the end effector.
 body_frame: ''
+
+# The precision with which to print the kinematic properties.
 precision: 6

package.xml

@@ -1,27 +1,31 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>kinematics_from_description</name>
-  <version>0.0.2</version>
+  <version>1.0.0</version>
   <description>This package contains a simple Python program that calculates properties required to run parts of the <a href="https://github.com/NxRLab/ModernRobotics">Modern Robotics Library</a> from a robot's robot_description parameter.</description>
 
   <license>BSD</license>
 
-  <author email="luke@trossenrobotics.com">Luke Schmitt</author>
+  <author email="trsupport@trossenrobotics.com">Luke Schmitt</author>
   <maintainer email="trsupport@trossenrobotics.com">Luke Schmitt</maintainer>
 
-  <url type="website">https://github.com/LSinterbotix/kinematics_from_description</url>
-  <url type="bugtracker">https://github.com/LSinterbotix/kinematics_from_description/issues</url>
-  <url type="repository">https://github.com/LSinterbotix/kinematics_from_description</url>
+  <url type="website">https://github.com/Interbotix/kinematics_from_description</url>
+  <url type="bugtracker">https://github.com/Interbotix/kinematics_from_description/issues</url>
+  <url type="repository">https://github.com/Interbotix/kinematics_from_description</url>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <buildtool_depend>python3-setuptools</buildtool_depend>
+  <buildtool_depend condition="$ROS_PYTHON_VERSION==2">python-setuptools</buildtool_depend>
+  <buildtool_depend condition="$ROS_PYTHON_VERSION==3">python3-setuptools</buildtool_depend>
 
   <exec_depend>rospy</exec_depend>
   <exec_depend>urdfdom_py</exec_depend>
-  <exec_depend>python3-yaml</exec_depend>
-  <exec_depend>python3-numpy</exec_depend>
-  <exec_depend version_gte="1.4.0">python3-scipy</exec_depend>
-  
+  <exec_depend condition="$ROS_PYTHON_VERSION==2">python-yaml</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION==2">python-numpy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION==2" version_gte="1.2.0">python-scipy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION==3">python3-yaml</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION==3">python3-numpy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION==3" version_gte="1.2.0">python3-scipy</exec_depend>
+
   <build_depend>rostest</build_depend>
 
 </package>

requirements.txt

@@ -0,0 +1,7 @@
+# $ python -m pip install -r requirements.txt
+# $ python3 -m pip3 install -r requirements.txt
+
+urdf_parser_py
+numpy
+scipy>=1.2.0
+pyyaml

scripts/mr_desc_param

@@ -0,0 +1,116 @@
+#!/usr/bin/env python
+
+import argparse
+import yaml
+import os
+import numpy as np
+import rospkg
+from kinematics_from_description.kfd import KinematicsFromDescriptionTool as KFD
+
+PKG = 'kinematics_from_description'
+
+def main(args):
+    if args.file is not None:
+        filepath = os.path.join(
+            rospkg.RosPack().get_path(PKG),
+            'config',
+            'config.yaml'
+        )
+        print("Loading configs from '%s'" % filepath)
+        try:
+            with open(filepath) as config_file:
+                config = yaml.safe_load(config_file)
+        except yaml.YAMLError as e:
+            print(e)
+            exit(1)
+    elif args.space_frame is not None and args.body_frame is not None:
+        config={}
+        config['space_frame']=args.space_frame
+        config['body_frame']=args.body_frame
+        config['namespace']=args.namespace
+        config['precision']=args.precision
+    else:
+        raise ValueError(
+            'Either a file must be specified or the space and body frames must be specified.'
+            'Use the --help flag for details.'
+        )
+
+    np.set_printoptions(precision=int(config["precision"]), suppress=True)
+
+    print('Using configs:')
+    print('\tspace_frame: %s' % config['space_frame'])
+    print('\tbody_frame:  %s' % config['body_frame'])
+    print('\tnamespace:   %s' % config['namespace'])
+    print('\tprecision:   %i' % config['precision'])
+
+    tool = KFD(config)
+
+    tool.load_desc_from_param()
+    tool.run()
+
+    print('\nThe MR Descriptions are as follows:')
+
+    print("\nM:")
+    print(repr(tool.get_M()))
+
+    print("\nSlist:")
+    print(str(repr(tool.get_Slist(transposed=False))) + ".T\n")
+
+    print(
+        (
+            'Paste these arrays into your mr_descriptions.py file to use in the Interbotix '
+            'Python-ROS API.'
+        )
+    )
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description=(
+            "A program that will parse a robot's kinematic properties from its "
+            '`/robot_description` rosparam.'
+        )
+    )
+    parser.add_argument(
+        '--file',
+        '-f',
+        type=str,
+        help='The path to the config file to use.',
+    )
+    parser.add_argument(
+        '--space_frame',
+        '-s',
+        type=str,
+        help=(
+            "The name given to the link that will serve as the location of the robot's base, or "
+            "{0} link. This is typically `base_link`."
+        ),
+    )
+    parser.add_argument(
+        '--body_frame',
+        '-b',
+        type=str,
+        help=(
+            'The name given to the link that will serve as the location of the end effector. This'
+            ' is typically `ee_gripper_link`.'
+        ),
+    )
+    parser.add_argument(
+        '--namespace',
+        '-n',
+        type=str,
+        default='',
+        help=(
+            'The namespace under which the `/robot_description` rosparam will be found, as well as'
+            ' the names of each link (i.e. `/namespace/robot_description`, and '
+            '`namespace/base_link`. (default: %(default)s)'
+        ),
+    )
+    parser.add_argument(
+        '--precision',
+        '-p',
+        type=int,
+        help='The precision with which to print the kinematic properties. (default: %(default)s)',
+        default=6,
+    )
+    main(args=parser.parse_args())