[ros-robot-model] 01/04: Imported Upstream version 1.12.3
Jochen Sprickerhof
jspricke-guest at moszumanska.debian.org
Sun Jul 10 22:37:43 UTC 2016
This is an automated email from the git hooks/post-receive script.
jspricke-guest pushed a commit to annotated tag debian/1.12.3-1
in repository ros-robot-model.
commit b30cd78078b59233942241cc54027ebf87862b92
Author: Jochen Sprickerhof <git at jochen.sprickerhof.de>
Date: Sun Jul 10 23:19:16 2016 +0200
Imported Upstream version 1.12.3
---
.travis.yml | 39 +
collada_parser/CHANGELOG.rst | 9 +
collada_parser/package.xml | 3 +-
collada_urdf/CHANGELOG.rst | 9 +
collada_urdf/package.xml | 3 +-
joint_state_publisher/CHANGELOG.rst | 16 +
.../joint_state_publisher/joint_state_publisher | 158 +-
joint_state_publisher/package.xml | 8 +-
kdl_parser/CHANGELOG.rst | 9 +
kdl_parser/CMakeLists.txt | 3 +
kdl_parser/package.xml | 3 +-
kdl_parser/src/kdl_parser.cpp | 26 +-
kdl_parser/test/testInertiaRPYmodel1.urdf | 29 +
kdl_parser/test/testInertiaRPYmodel2.urdf | 29 +
kdl_parser/test/test_inertia_rpy.cpp | 101 +
kdl_parser/test/test_inertia_rpy.launch | 6 +
kdl_parser/test/test_kdl_parser.cpp | 12 +-
kdl_parser/test/test_kdl_parser.launch | 5 +-
kdl_parser/test/test_robot.urdf | 425 +
kdl_parser_py/CHANGELOG.rst | 235 +
kdl_parser_py/CMakeLists.txt | 22 +
kdl_parser_py/kdl_parser_py/__init__.py | 0
kdl_parser_py/kdl_parser_py/urdf.py | 125 +
{kdl_parser => kdl_parser_py}/package.xml | 20 +-
kdl_parser_py/setup.py | 11 +
kdl_parser_py/test/kdl_parser_py.urdf | 300391 ++++++++++++++++++
kdl_parser_py/test/test.urdf | 235 +
kdl_parser_py/test/test_kdl_parser.launch | 3 +
kdl_parser_py/test/test_kdl_parser.py | 36 +
robot_model/CHANGELOG.rst | 9 +
robot_model/package.xml | 4 +-
urdf/CHANGELOG.rst | 9 +
urdf/package.xml | 3 +-
urdf/src/model.cpp | 3 +-
urdf/test/one_link.urdf | 2 +-
urdf/test/test_robot.urdf | 425 +
urdf/test/test_robot_model_parser.cpp | 65 +-
urdf/test/test_robot_model_parser.launch | 18 +-
urdf/test/two_links_one_joint.urdf | 2 +-
urdf_parser_plugin/CHANGELOG.rst | 9 +
urdf_parser_plugin/package.xml | 3 +-
41 files changed, 302394 insertions(+), 129 deletions(-)
diff --git a/.travis.yml b/.travis.yml
new file mode 100644
index 0000000..a603e39
--- /dev/null
+++ b/.travis.yml
@@ -0,0 +1,39 @@
+sudo: required
+dist: trusty
+# Force travis to use its minimal image with default Python settings
+language: generic
+compiler:
+ - gcc
+env:
+ global:
+ - CATKIN_WS=~/catkin_ws
+ - CATKIN_WS_SRC=${CATKIN_WS}/src
+ matrix:
+ - CI_ROS_DISTRO="indigo"
+ - CI_ROS_DISTRO="jade"
+install:
+ - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+ - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+ - sudo apt-get update -qq
+ - sudo apt-get install -qq -y python-rosdep python-catkin-tools
+ - sudo rosdep init
+ - rosdep update
+ # Use rosdep to install all dependencies (including ROS itself)
+ - rosdep install --from-paths ./ -i -y --rosdistro $CI_ROS_DISTRO
+script:
+ - source /opt/ros/$CI_ROS_DISTRO/setup.bash
+ - mkdir -p $CATKIN_WS_SRC
+ - ln -s $TRAVIS_BUILD_DIR $CATKIN_WS_SRC
+ - cd $CATKIN_WS
+ - catkin init
+ # Enable install space
+ #- catkin config --install
+ # Build [and Install] packages
+ - catkin build --limit-status-rate 0.1 --no-notify -DCMAKE_BUILD_TYPE=Release
+ # Build tests
+ - catkin build --limit-status-rate 0.1 --no-notify --make-args tests
+ # Run tests
+ - catkin run_tests
+ # catkin run_tests always returns 0, use catkin_test_results to check for errors
+ # https://github.com/catkin/catkin_tools/issues/245
+ - catkin_test_results
diff --git a/collada_parser/CHANGELOG.rst b/collada_parser/CHANGELOG.rst
index 4999e9a..68bbed2 100644
--- a/collada_parser/CHANGELOG.rst
+++ b/collada_parser/CHANGELOG.rst
@@ -2,6 +2,15 @@
Changelog for package collada_parser
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.12.3 (2016-06-10)
+-------------------
+
+1.12.2 (2016-04-12)
+-------------------
+
+1.12.1 (2016-04-10)
+-------------------
+
1.11.8 (2015-09-11)
-------------------
diff --git a/collada_parser/package.xml b/collada_parser/package.xml
index ba29ed4..8dc1d53 100644
--- a/collada_parser/package.xml
+++ b/collada_parser/package.xml
@@ -1,6 +1,6 @@
<package>
<name>collada_parser</name>
- <version>1.11.8</version>
+ <version>1.12.3</version>
<description>
This package contains a C++ parser for the Collada robot
description format. The parser reads a Collada XML robot
@@ -11,6 +11,7 @@
<author>Rosen Diankov</author>
<author>Kei Okada</author>
<maintainer email="isucan at gmail.com">Ioan Sucan</maintainer>
+ <maintainer email="jacquelinekay1 at gmail.com">Jackie Kay</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/collada_parser</url>
diff --git a/collada_urdf/CHANGELOG.rst b/collada_urdf/CHANGELOG.rst
index fba1e07..3acdce3 100644
--- a/collada_urdf/CHANGELOG.rst
+++ b/collada_urdf/CHANGELOG.rst
@@ -2,6 +2,15 @@
Changelog for package collada_urdf
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.12.3 (2016-06-10)
+-------------------
+
+1.12.2 (2016-04-12)
+-------------------
+
+1.12.1 (2016-04-10)
+-------------------
+
1.11.8 (2015-09-11)
-------------------
* Removed pcre hack for newer released collada-dom.
diff --git a/collada_urdf/package.xml b/collada_urdf/package.xml
index 3f196da..aa82eda 100644
--- a/collada_urdf/package.xml
+++ b/collada_urdf/package.xml
@@ -1,6 +1,6 @@
<package>
<name>collada_urdf</name>
- <version>1.11.8</version>
+ <version>1.12.3</version>
<description>
This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents.
@@ -11,6 +11,7 @@
<author>Tim Field</author>
<author>Rosen Diankov</author>
<maintainer email="isucan at gmail.com">Ioan Sucan</maintainer>
+ <maintainer email="jacquelinekay1 at gmail.com">Jackie Kay</maintainer>
<license>BSD</license>
diff --git a/joint_state_publisher/CHANGELOG.rst b/joint_state_publisher/CHANGELOG.rst
index 5ec25b5..3c1dd4e 100644
--- a/joint_state_publisher/CHANGELOG.rst
+++ b/joint_state_publisher/CHANGELOG.rst
@@ -2,6 +2,22 @@
Changelog for package joint_state_publisher
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.12.3 (2016-06-10)
+-------------------
+* Fix circular logic in joint state publisher events (`#140 <https://github.com/ros/robot_model/issues/140>`_)
+* Use signal and sys.exit to fix shutdown in joint_state_publisher (`#139 <https://github.com/ros/robot_model/issues/139>`_)
+* joint_state_publisher: Change slider update method (`#135 <https://github.com/ros/robot_model/issues/135>`_)
+* Contributors: Jackie Kay, vincentrou
+
+1.12.2 (2016-04-12)
+-------------------
+* Migrate qt (`#128 <https://github.com/ros/robot_model/issues/128>`_)
+ * Migrate JointStatePublisher from wxPython to qt5
+* Contributors: Jackie Kay
+
+1.12.1 (2016-04-10)
+-------------------
+
1.11.8 (2015-09-11)
-------------------
diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher/joint_state_publisher
index bc72e7d..ef1ffec 100755
--- a/joint_state_publisher/joint_state_publisher/joint_state_publisher
+++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher
@@ -2,15 +2,29 @@
import rospy
import random
-import wx
-import wx.lib.newevent
+
+from python_qt_binding.QtCore import Qt
+from python_qt_binding.QtCore import Signal
+from python_qt_binding.QtGui import QFont
+from python_qt_binding.QtWidgets import QApplication
+from python_qt_binding.QtWidgets import QHBoxLayout
+from python_qt_binding.QtWidgets import QLabel
+from python_qt_binding.QtWidgets import QLineEdit
+from python_qt_binding.QtWidgets import QPushButton
+from python_qt_binding.QtWidgets import QSlider
+from python_qt_binding.QtWidgets import QVBoxLayout
+from python_qt_binding.QtWidgets import QWidget
+
import xml.dom.minidom
from sensor_msgs.msg import JointState
from math import pi
from threading import Thread
+import sys
+import signal
RANGE = 10000
+
def get_param(name, value=None):
private = "~%s" % name
if rospy.has_param(private):
@@ -20,12 +34,13 @@ def get_param(name, value=None):
else:
return value
+
class JointStatePublisher():
def __init__(self):
description = get_param('robot_description')
robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
self.free_joints = {}
- self.joint_list = [] # for maintaining the original order of the joints
+ self.joint_list = [] # for maintaining the original order of the joints
self.dependent_joints = get_param("dependent_joints", {})
use_mimic = get_param('use_mimic_tags', True)
use_small = get_param('use_smallest_joint_limits', True)
@@ -59,7 +74,7 @@ class JointStatePublisher():
continue
safety_tags = child.getElementsByTagName('safety_controller')
- if use_small and len(safety_tags)==1:
+ if use_small and len(safety_tags) == 1:
tag = safety_tags[0]
if tag.hasAttribute('soft_lower_limit'):
minval = max(minval, float(tag.getAttribute('soft_lower_limit')))
@@ -67,7 +82,7 @@ class JointStatePublisher():
maxval = min(maxval, float(tag.getAttribute('soft_upper_limit')))
mimic_tags = child.getElementsByTagName('mimic')
- if use_mimic and len(mimic_tags)==1:
+ if use_mimic and len(mimic_tags) == 1:
tag = mimic_tags[0]
entry = {'parent': tag.getAttribute('joint')}
if tag.hasAttribute('multiplier'):
@@ -88,7 +103,7 @@ class JointStatePublisher():
else:
zeroval = 0
- joint = {'min':minval, 'max':maxval, 'zero':zeroval}
+ joint = {'min': minval, 'max': maxval, 'zero': zeroval}
if pub_def_positions:
joint['position'] = zeroval
if pub_def_vels:
@@ -100,13 +115,12 @@ class JointStatePublisher():
joint['continuous'] = True
self.free_joints[name] = joint
-
use_gui = get_param("use_gui", False)
if use_gui:
- self.app = wx.App()
+ self.app = QApplication(sys.argv)
self.gui = JointStatePublisherGui("Joint State Publisher", self)
- self.gui.Show()
+ self.gui.show()
else:
self.gui = None
@@ -145,12 +159,11 @@ class JointStatePublisher():
joint['effort'] = effort
if self.gui is not None:
- # post an event here instead of directly calling the update_sliders method, to switch to the wx thread
- wx.PostEvent(self.gui.GetEventHandler(), self.gui.UpdateSlidersEvent())
-
+ # signal instead of directly calling the update_sliders method, to switch to the QThread
+ self.gui.sliderUpdateTrigger.emit()
def loop(self):
- hz = get_param("rate", 10) # 10hz
+ hz = get_param("rate", 10) # 10hz
r = rospy.Rate(hz)
delta = get_param("delta", 0.0)
@@ -167,7 +180,7 @@ class JointStatePublisher():
has_position = len(self.dependent_joints.items()) > 0
has_velocity = False
has_effort = False
- for (name,joint) in self.free_joints.items():
+ for name, joint in self.free_joints.items():
if not has_position and 'position' in joint:
has_position = True
if not has_velocity and 'velocity' in joint:
@@ -183,7 +196,6 @@ class JointStatePublisher():
if has_effort:
msg.effort = num_joints * [0.0]
-
for i, name in enumerate(self.joint_list):
msg.name.append(str(name))
joint = None
@@ -228,14 +240,16 @@ class JointStatePublisher():
joint['position'] = joint['min']
joint['forward'] = not forward
-class JointStatePublisherGui(wx.Frame):
+
+class JointStatePublisherGui(QWidget):
+ sliderUpdateTrigger = Signal()
+
def __init__(self, title, jsp):
- wx.Frame.__init__(self, None, -1, title, (-1, -1));
+ super(JointStatePublisherGui, self).__init__()
self.jsp = jsp
self.joint_map = {}
- panel = wx.Panel(self, wx.ID_ANY);
- box = wx.BoxSizer(wx.VERTICAL)
- font = wx.Font(9, wx.SWISS, wx.NORMAL, wx.BOLD)
+ box_layout = QVBoxLayout(self)
+ font = QFont("Helvetica", 9, QFont.Bold)
### Sliders ###
for name in self.jsp.joint_list:
@@ -246,88 +260,92 @@ class JointStatePublisherGui(wx.Frame):
if joint['min'] == joint['max']:
continue
- row = wx.GridSizer(1,2)
- label = wx.StaticText(panel, -1, name)
- label.SetFont(font)
- row.Add(label, 1, wx.ALIGN_CENTER_VERTICAL)
+ joint_layout = QVBoxLayout()
+ row_layout = QHBoxLayout()
- display = wx.TextCtrl (panel, value=str(0),
- style=wx.TE_READONLY | wx.ALIGN_RIGHT)
+ label = QLabel(name)
+ label.setFont(font)
+ row_layout.addWidget(label)
+ display = QLineEdit("0.00")
+ display.setAlignment(Qt.AlignRight)
+ display.setFont(font)
+ display.setReadOnly(True)
+ row_layout.addWidget(display)
- row.Add(display, flag= wx.ALIGN_RIGHT| wx.ALIGN_CENTER_VERTICAL)
- box.Add(row, 1, wx.EXPAND)
- slider = wx.Slider(panel, -1, RANGE/2, 0, RANGE,
- style= wx.SL_AUTOTICKS | wx.SL_HORIZONTAL)
- slider.SetFont(font)
- box.Add(slider, 1, wx.EXPAND)
+ joint_layout.addLayout(row_layout)
- self.joint_map[name] = {'slidervalue':0, 'display':display,
- 'slider':slider, 'joint':joint}
+ slider = QSlider(Qt.Horizontal)
- self.UpdateSlidersEvent, self.EVT_UPDATESLIDERS = wx.lib.newevent.NewEvent()
- self.Bind(self.EVT_UPDATESLIDERS, self.updateSliders)
+ slider.setFont(font)
+ slider.setRange(0, RANGE)
+ slider.setValue(RANGE/2)
- ### Buttons ###
- self.randbutton = wx.Button(panel, 1, 'Randomize')
- self.ctrbutton = wx.Button(panel, 2, 'Center')
- self.Bind(wx.EVT_SLIDER, self.sliderUpdate)
+ joint_layout.addWidget(slider)
- wx.EVT_BUTTON(self, 1, self.randomize_event)
- wx.EVT_BUTTON(self, 2, self.center_event)
+ box_layout.addLayout(joint_layout)
- box.Add(self.randbutton, 0, wx.EXPAND)
- box.Add(self.ctrbutton, 1, wx.EXPAND)
+ self.joint_map[name] = {'slidervalue': 0, 'display': display,
+ 'slider': slider, 'joint': joint}
+ # Connect to the signal provided by QSignal
+ slider.valueChanged.connect(self.onValueChanged)
- panel.SetSizer(box)
- self.center()
- box.Fit(self)
- self.update_values()
+ # Synchronize slider and displayed value
+ self.sliderUpdate(None)
+ # Set up a signal for updating the sliders based on external joint info
+ self.sliderUpdateTrigger.connect(self.updateSliders)
- def update_values(self):
- for (name,joint_info) in self.joint_map.items():
- purevalue = joint_info['slidervalue']
+ ### Buttons ###
+ self.randbutton = QPushButton('Randomize', self)
+ self.randbutton.clicked.connect(self.randomize_event)
+ box_layout.addWidget(self.randbutton)
+ self.ctrbutton = QPushButton('Center', self)
+ self.ctrbutton.clicked.connect(self.center_event)
+ box_layout.addWidget(self.ctrbutton)
+
+ def onValueChanged(self, event):
+ # A slider value was changed, but we need to change the joint_info metadata.
+ for name, joint_info in self.joint_map.items():
+ joint_info['slidervalue'] = joint_info['slider'].value()
joint = joint_info['joint']
- value = self.sliderToValue(purevalue, joint)
- joint['position'] = value
- self.update_sliders()
+ joint['position'] = self.sliderToValue(joint_info['slidervalue'], joint)
+ joint_info['display'].setText("%.2f" % joint['position'])
def updateSliders(self, event):
self.update_sliders()
def update_sliders(self):
- for (name,joint_info) in self.joint_map.items():
+ for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
joint_info['slidervalue'] = self.valueToSlider(joint['position'],
joint)
- joint_info['slider'].SetValue(joint_info['slidervalue'])
- joint_info['display'].SetValue("%.2f"%joint['position'])
+ joint_info['slider'].setValue(joint_info['slidervalue'])
+
def center_event(self, event):
self.center()
def center(self):
rospy.loginfo("Centering")
- for (name,joint_info) in self.joint_map.items():
+ for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
- joint_info['slidervalue'] = self.valueToSlider(joint['zero'], joint)
- self.update_values()
+ joint_info['slider'].setValue(self.valueToSlider(joint['zero'], joint))
+
def randomize_event(self, event):
self.randomize()
def randomize(self):
rospy.loginfo("Randomizing")
- for (name,joint_info) in self.joint_map.items():
+ for name, joint_info in self.joint_map.items():
joint = joint_info['joint']
- joint_info['slidervalue'] = self.valueToSlider(random.uniform(joint['min'], joint['max']), joint)
- self.update_values()
-
+ joint_info['slider'].setValue(
+ self.valueToSlider(random.uniform(joint['min'], joint['max']), joint))
def sliderUpdate(self, event):
- for (name,joint_info) in self.joint_map.items():
- joint_info['slidervalue'] = joint_info['slider'].GetValue()
- self.update_values()
+ for name, joint_info in self.joint_map.items():
+ joint_info['slidervalue'] = joint_info['slider'].value()
+ self.update_sliders()
def valueToSlider(self, value, joint):
return (value - joint['min']) * float(RANGE) / (joint['max'] - joint['min'])
@@ -346,6 +364,8 @@ if __name__ == '__main__':
jsp.loop()
else:
Thread(target=jsp.loop).start()
- jsp.app.MainLoop()
+ signal.signal(signal.SIGINT, signal.SIG_DFL)
+ sys.exit(jsp.app.exec_())
- except rospy.ROSInterruptException: pass
+ except rospy.ROSInterruptException:
+ pass
diff --git a/joint_state_publisher/package.xml b/joint_state_publisher/package.xml
index 764c398..6b8fba4 100644
--- a/joint_state_publisher/package.xml
+++ b/joint_state_publisher/package.xml
@@ -1,22 +1,24 @@
<package>
<name>joint_state_publisher</name>
- <version>1.11.8</version>
+ <version>1.12.3</version>
<description>
This package contains a tool for setting and publishing joint state values for a given URDF.
</description>
<author email="davidvlu at gmail.com">David V. Lu!!</author>
<maintainer email="davidvlu at gmail.com">David V. Lu!!</maintainer>
+ <maintainer email="jacquelinekay1 at gmail.com">Jackie Kay</maintainer>
+
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/joint_state_publisher</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
- <build_depend>wxpython</build_depend>
+ <build_depend>python_qt_binding</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>rospy</run_depend>
- <run_depend>wxpython</run_depend>
+ <run_depend>python_qt_binding</run_depend>
<run_depend>sensor_msgs</run_depend>
</package>
diff --git a/kdl_parser/CHANGELOG.rst b/kdl_parser/CHANGELOG.rst
index 5a6b54a..e240cae 100644
--- a/kdl_parser/CHANGELOG.rst
+++ b/kdl_parser/CHANGELOG.rst
@@ -2,6 +2,15 @@
Changelog for package kdl_parser
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+1.12.3 (2016-06-10)
+-------------------
+
+1.12.2 (2016-04-12)
+-------------------
+
+1.12.1 (2016-04-10)
+-------------------
+
1.11.8 (2015-09-11)
-------------------
diff --git a/kdl_parser/CMakeLists.txt b/kdl_parser/CMakeLists.txt
index eedaf8a..43b96b7 100644
--- a/kdl_parser/CMakeLists.txt
+++ b/kdl_parser/CMakeLists.txt
@@ -38,6 +38,9 @@ if(CATKIN_ENABLE_TESTING)
find_package(catkin REQUIRED COMPONENTS rostest)
add_rostest_gtest(test_kdl_parser test/test_kdl_parser.launch test/test_kdl_parser.cpp )
target_link_libraries(test_kdl_parser ${PROJECT_NAME})
+
+ add_rostest_gtest(test_inertia_rpy test/test_inertia_rpy.launch test/test_inertia_rpy.cpp )
+ target_link_libraries(test_inertia_rpy ${PROJECT_NAME})
endif()
# How does CATKIN do this?
diff --git a/kdl_parser/package.xml b/kdl_parser/package.xml
index 6ec51f5..018c8f4 100644
--- a/kdl_parser/package.xml
+++ b/kdl_parser/package.xml
@@ -1,6 +1,6 @@
<package>
<name>kdl_parser</name>
- <version>1.11.8</version>
+ <version>1.12.3</version>
<description>
The Kinematics and Dynamics Library (KDL) defines a tree structure
to represent the kinematic and dynamic parameters of a robot
@@ -10,6 +10,7 @@
<author email="wim at willowgarage.com">Wim Meeussen</author>
<maintainer email="isucan at gmail.com">Ioan Sucan</maintainer>
+ <maintainer email="jacquelinekay1 at gmail.com">Jackie Kay</maintainer>
<license>BSD</license>
diff --git a/kdl_parser/src/kdl_parser.cpp b/kdl_parser/src/kdl_parser.cpp
index 4b8c3ab..7031247 100644
--- a/kdl_parser/src/kdl_parser.cpp
+++ b/kdl_parser/src/kdl_parser.cpp
@@ -96,8 +96,30 @@ Joint toKdl(boost::shared_ptr<urdf::Joint> jnt)
RigidBodyInertia toKdl(boost::shared_ptr<urdf::Inertial> i)
{
Frame origin = toKdl(i->origin);
- // kdl specifies the inertia in the reference frame of the link, the urdf specifies the inertia in the inertia reference frame
- return origin.M * RigidBodyInertia(i->mass, origin.p, RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz));
+
+ // the mass is frame indipendent
+ double kdl_mass = i->mass;
+
+ // kdl and urdf both specify the com position in the reference frame of the link
+ Vector kdl_com = origin.p;
+
+ // kdl specifies the inertia matrix in the reference frame of the link,
+ // while the urdf specifies the inertia matrix in the inertia reference frame
+ RotationalInertia urdf_inertia =
+ RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
+
+ // Rotation operators are not defined for rotational inertia,
+ // so we use the RigidBodyInertia operators (with com = 0) as a workaround
+ RigidBodyInertia kdl_inertia_wrt_com_workaround =
+ origin.M *RigidBodyInertia(0, Vector::Zero(), urdf_inertia);
+
+ // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
+ // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
+ // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
+ RotationalInertia kdl_inertia_wrt_com =
+ kdl_inertia_wrt_com_workaround.getRotationalInertia();
+
+ return RigidBodyInertia(kdl_mass,kdl_com,kdl_inertia_wrt_com);
}
diff --git a/kdl_parser/test/testInertiaRPYmodel1.urdf b/kdl_parser/test/testInertiaRPYmodel1.urdf
new file mode 100644
index 0000000..7e390a6
--- /dev/null
+++ b/kdl_parser/test/testInertiaRPYmodel1.urdf
@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="testInertialRPYmodel1">
+ <link name="base_link" />
+ <joint name="base_fixed_joint" type="fixed">
+ <origin xyz="0 0 0" rpy="0 -0 0" />
+ <axis xyz="0 0 0" />
+ <parent link="base_link" />
+ <child link="link1" />
+ </joint>
+ <link name="link1">
+ <inertial>
+ <mass value="1" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+ </inertial>
+ </link>
+ <joint name="joint_1_2" type="continuous">
+ <origin xyz="0 0 0" rpy="0 -0 0" />
+ <axis xyz="0 0 1" />
+ <parent link="link1" />
+ <child link="link2" />
+ </joint>
+ <link name="link2">
+ <inertial>
+ <mass value="100" />
+ <origin xyz="1 2 3" rpy="0 -0 0" />
+ <inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
+ </inertial>
+ </link>
+</robot>
diff --git a/kdl_parser/test/testInertiaRPYmodel2.urdf b/kdl_parser/test/testInertiaRPYmodel2.urdf
new file mode 100644
index 0000000..33dc229
--- /dev/null
+++ b/kdl_parser/test/testInertiaRPYmodel2.urdf
@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<robot name="testInertialRPYmodel2">
+ <link name="base_link" />
+ <joint name="base_fixed_joint" type="fixed">
+ <origin xyz="0 0 0" rpy="0 -0 0" />
+ <axis xyz="0 0 0" />
+ <parent link="base_link" />
+ <child link="link1" />
+ </joint>
+ <link name="link1">
+ <inertial>
+ <mass value="1" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
+ </inertial>
+ </link>
+ <joint name="joint_1_2" type="continuous">
+ <origin xyz="0 0 0" rpy="0 -0 0" />
+ <axis xyz="0 0 1" />
+ <parent link="link1" />
+ <child link="link2" />
+ </joint>
+ <link name="link2">
+ <inertial>
+ <mass value="100" />
+ <origin xyz="1 2 3" rpy="3.141592653589793 0 0" />
+ <inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
+ </inertial>
+ </link>
+</robot>
diff --git a/kdl_parser/test/test_inertia_rpy.cpp b/kdl_parser/test/test_inertia_rpy.cpp
new file mode 100644
index 0000000..05b12d2
--- /dev/null
+++ b/kdl_parser/test/test_inertia_rpy.cpp
@@ -0,0 +1,101 @@
+
+#include <string>
+#include <gtest/gtest.h>
+#include <kdl/jntarray.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+#include "kdl_parser/kdl_parser.hpp"
+
+using namespace kdl_parser;
+
+int g_argc;
+char** g_argv;
+
+class TestInertiaRPY : public testing::Test
+{
+public:
+
+protected:
+ /// constructor
+ TestInertiaRPY()
+ {
+ }
+
+
+ /// Destructor
+ ~TestInertiaRPY()
+ {
+ }
+};
+
+
+TEST_F(TestInertiaRPY, test_torques)
+{
+ //ASSERT_EQ(g_argc, 3);
+
+ // workaround for segfault issue with parsing 2 trees instantiated on the stack
+ KDL::Tree * tree_1 = new KDL::Tree;
+ KDL::Tree * tree_2 = new KDL::Tree;
+ KDL::JntArray torques_1;
+ KDL::JntArray torques_2;
+
+ {
+ ASSERT_TRUE(treeFromFile(g_argv[1], *tree_1));
+ KDL::Vector gravity(0, 0, -9.81);
+ KDL::Chain chain;
+ std::cout << "number of joints: " << tree_1->getNrOfJoints() << std::endl;
+ std::cout << "number of segments: " << tree_1->getNrOfSegments() << std::endl;
+
+ ASSERT_TRUE(tree_1->getChain("base_link", "link2", chain));
+ KDL::ChainIdSolver_RNE solver(chain, gravity);
+ //JntArrays get initialized with all 0 values
+ KDL::JntArray q(chain.getNrOfJoints());
+ KDL::JntArray qdot(chain.getNrOfJoints());
+ KDL::JntArray qdotdot(chain.getNrOfJoints());
+ //KDL::JntArray torques(chain.getNrOfJoints());
+ std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
+ solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_1);
+
+ delete tree_1;
+ tree_1 = NULL;
+ }
+
+ {
+ ASSERT_TRUE(treeFromFile(g_argv[2], *tree_2));
+ KDL::Vector gravity(0, 0, -9.81);
+ KDL::Chain chain;
+
+ ASSERT_TRUE(tree_2->getChain("base_link", "link2", chain));
+ KDL::ChainIdSolver_RNE solver(chain, gravity);
+ //JntArrays get initialized with all 0 values
+ KDL::JntArray q(chain.getNrOfJoints());
+ KDL::JntArray qdot(chain.getNrOfJoints());
+ KDL::JntArray qdotdot(chain.getNrOfJoints());
+ //KDL::JntArray torques(chain.getNrOfJoints());
+ std::vector<KDL::Wrench> wrenches(chain.getNrOfJoints());
+ solver.CartToJnt(q, qdot, qdotdot, wrenches, torques_2);
+
+ delete tree_2;
+ tree_2 = NULL;
+ }
+
+ ASSERT_TRUE(torques_1 == torques_2);
+
+ SUCCEED();
+}
+
+
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "test_kdl_parser");
+ for (size_t i = 0; i < argc; ++i) {
+ std::cout << argv[i] << std::endl;
+ }
+ g_argc = argc;
+ g_argv = argv;
+ return RUN_ALL_TESTS();
+}
diff --git a/kdl_parser/test/test_inertia_rpy.launch b/kdl_parser/test/test_inertia_rpy.launch
new file mode 100644
index 0000000..54fc441
--- /dev/null
+++ b/kdl_parser/test/test_inertia_rpy.launch
@@ -0,0 +1,6 @@
+<launch>
+ <test test-name="test_inertia_rpy" pkg="kdl_parser" type="test_inertia_rpy"
+ args="$(find kdl_parser)/test/testInertiaRPYmodel1.urdf
+ $(find kdl_parser)/test/testInertiaRPYmodel2.urdf"
+ />
+</launch>
diff --git a/kdl_parser/test/test_kdl_parser.cpp b/kdl_parser/test/test_kdl_parser.cpp
index c43d75f..ad0bb98 100644
--- a/kdl_parser/test/test_kdl_parser.cpp
+++ b/kdl_parser/test/test_kdl_parser.cpp
@@ -63,8 +63,6 @@ protected:
};
-
-
TEST_F(TestParser, test)
{
for (int i=1; i<g_argc-2; i++){
@@ -73,13 +71,13 @@ TEST_F(TestParser, test)
}
ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
- ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)44);
- ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)81);
- ASSERT_TRUE(my_tree.getSegment("base_footprint") == my_tree.getRootSegment());
+ ASSERT_EQ(my_tree.getNrOfJoints(), 8);
+ ASSERT_EQ(my_tree.getNrOfSegments(), 16);
+ ASSERT_TRUE(my_tree.getSegment("dummy_link") == my_tree.getRootSegment());
ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
- ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0);
- ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 15.6107, 0.001);
+ ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 10.0);
+ ASSERT_NEAR(my_tree.getSegment("base_link")->second.segment.getInertia().getRotationalInertia().data[0], 1.000, 0.001);
SUCCEED();
}
diff --git a/kdl_parser/test/test_kdl_parser.launch b/kdl_parser/test/test_kdl_parser.launch
index dad91ee..a730458 100644
--- a/kdl_parser/test/test_kdl_parser.launch
+++ b/kdl_parser/test/test_kdl_parser.launch
@@ -1,6 +1,3 @@
<launch>
- <test test-name="test_kdl_parser" pkg="kdl_parser" type="test_kdl_parser" args="$(find kdl_parser)/test/pr2_desc_vector.xml \
- $(find kdl_parser)/test/pr2_desc_bracket.xml \
- $(find kdl_parser)/test/pr2_desc_bracket2.xml \
- $(find kdl_parser)/test/pr2_desc.xml" />
+ <test test-name="test_kdl_parser" pkg="kdl_parser" type="test_kdl_parser" args="$(find kdl_parser)/test/test_robot.urdf"/>
</launch>
diff --git a/kdl_parser/test/test_robot.urdf b/kdl_parser/test/test_robot.urdf
new file mode 100644
index 0000000..731e1f3
--- /dev/null
+++ b/kdl_parser/test/test_robot.urdf
@@ -0,0 +1,425 @@
+<?xml version="1.0"?>
+<robot name="r2d2">
+
+ <material name="blue">
+ <color rgba="0 0 .8 1"/>
+ </material>
+ <material name="black">
+ <color rgba="0 0 0 1"/>
+ </material>
+ <material name="white">
+ <color rgba="1 1 1 1"/>
+ </material>
+
+ <link name="dummy_link"/>
+
+ <link name="base_link">
+ <visual>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ <material name="blue"/>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.6" radius="0.2"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="dummy_to_base" type="fixed">
+ <parent link="dummy_link"/>
+ <child link="base_link"/>
+ </joint>
+
+ <link name="right_leg">
+ <visual>
+ <geometry>
+ <box size="0.6 .1 .2"/>
+ </geometry>
+ <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.6 .1 .2"/>
+ </geometry>
+ <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+ </collision>
+ <inertial>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+ <joint name="base_to_right_leg" type="fixed">
+ <parent link="base_link"/>
+ <child link="right_leg"/>
+ <origin xyz="0 -0.22 .25"/>
+ </joint>
+
+ <link name="right_base">
+ <visual>
+ <geometry>
+ <box size="0.4 .1 .1"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.4 .1 .1"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_base_joint" type="fixed">
+ <parent link="right_leg"/>
+ <child link="right_base"/>
+ <origin xyz="0 0 -0.6"/>
+ </joint>
+
+ <link name="right_front_wheel">
+ <visual>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_front_wheel_joint" type="continuous">
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ <parent link="right_base"/>
+ <child link="right_front_wheel"/>
+ <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
+ </joint>
+
+ <link name="right_back_wheel">
+ <visual>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_back_wheel_joint" type="continuous">
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ <parent link="right_base"/>
+ <child link="right_back_wheel"/>
+ <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
+ </joint>
+
+ <link name="left_leg">
+ <visual>
+ <geometry>
+ <box size="0.6 .1 .2"/>
+ </geometry>
+ <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.6 .1 .2"/>
+ </geometry>
+ <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+ </collision>
+ <inertial>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="base_to_left_leg" type="fixed">
+ <parent link="base_link"/>
+ <child link="left_leg"/>
+ <origin xyz="0 0.22 .25"/>
+ </joint>
+
+ <link name="left_base">
+ <visual>
+ <geometry>
+ <box size="0.4 .1 .1"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size="0.4 .1 .1"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="10"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_base_joint" type="fixed">
+ <parent link="left_leg"/>
+ <child link="left_base"/>
+ <origin xyz="0 0 -0.6"/>
+ </joint>
+
+ <link name="left_front_wheel">
+ <visual>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_front_wheel_joint" type="continuous">
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ <parent link="left_base"/>
+ <child link="left_front_wheel"/>
+ <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
+ </joint>
+
+ <link name="left_back_wheel">
+ <visual>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <collision>
+ <origin rpy="1.57075 0 0" xyz="0 0 0"/>
+ <geometry>
+ <cylinder length="0.1" radius="0.035"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_back_wheel_joint" type="continuous">
+ <axis rpy="0 0 0" xyz="0 1 0"/>
+ <parent link="left_base"/>
+ <child link="left_back_wheel"/>
+ <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
+ </joint>
+
+ <joint name="gripper_extension" type="prismatic">
+ <parent link="base_link"/>
+ <child link="gripper_pole"/>
+ <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
+ <origin rpy="0 0 0" xyz="0.19 0 .2"/>
+ </joint>
+
+ <link name="gripper_pole">
+ <visual>
+ <geometry>
+ <cylinder length="0.2" radius=".01"/>
+ </geometry>
+ <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+ </visual>
+ <collision>
+ <geometry>
+ <cylinder length="0.2" radius=".01"/>
+ </geometry>
+ <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_gripper_joint" type="revolute">
+ <axis xyz="0 0 1"/>
+ <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+ <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+ <parent link="gripper_pole"/>
+ <child link="left_gripper"/>
+ </joint>
+
+ <link name="left_gripper">
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+ </geometry>
+ <origin rpy="0.0 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="left_tip_joint" type="fixed">
+ <parent link="left_gripper"/>
+ <child link="left_tip"/>
+ </joint>
+
+ <link name="left_tip">
+ <visual>
+ <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+ </geometry>
+ <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_gripper_joint" type="revolute">
+ <axis xyz="0 0 -1"/>
+ <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+ <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+ <parent link="gripper_pole"/>
+ <child link="right_gripper"/>
+ </joint>
+
+ <link name="right_gripper">
+ <visual>
+ <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+ </geometry>
+ <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="right_tip_joint" type="fixed">
+ <parent link="right_gripper"/>
+ <child link="right_tip"/>
+ </joint>
+
+ <link name="right_tip">
+ <visual>
+ <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+ </geometry>
+ </visual>
+ <collision>
+ <geometry>
+ <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+ </geometry>
+ <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+ </collision>
+ <inertial>
+ <mass value="0.05"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <link name="head">
+ <visual>
+ <geometry>
+ <sphere radius="0.2"/>
+ </geometry>
+ <material name="white"/>
+ </visual>
+ <collision>
+ <geometry>
+ <sphere radius="0.2"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="2"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="head_swivel" type="continuous">
+ <parent link="base_link"/>
+ <child link="head"/>
+ <axis xyz="0 0 1"/>
+ <origin xyz="0 0 0.3"/>
+ </joint>
+
+ <link name="box">
+ <visual>
+ <geometry>
+ <box size=".08 .08 .08"/>
+ </geometry>
+ <material name="blue"/>
+ <origin xyz="-0.04 0 0"/>
+ </visual>
+ <collision>
+ <geometry>
+ <box size=".08 .08 .08"/>
+ </geometry>
+ </collision>
+ <inertial>
+ <mass value="1"/>
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="tobox" type="fixed">
+ <parent link="head"/>
+ <child link="box"/>
+ <origin xyz="0.1814 0 0.1414"/>
+ </joint>
+
+</robot>
+
diff --git a/kdl_parser_py/CHANGELOG.rst b/kdl_parser_py/CHANGELOG.rst
new file mode 100644
index 0000000..4752196
--- /dev/null
+++ b/kdl_parser_py/CHANGELOG.rst
@@ -0,0 +1,235 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package kdl_parser_py
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.3 (2016-06-10)
+-------------------
+
+1.12.2 (2016-04-12)
+-------------------
+
+1.12.1 (2016-04-10)
+-------------------
+* Remove cmake_modules dependency
+* Contributors: Jackie Kay
+
+1.12.0 (2016-04-04)
+-------------------
+
+1.11.10 (2016-02-23)
+--------------------
+* another patch release
+* Remove cmake_modules dependency
+* Contributors: Jackie Kay
+
+1.11.9 (2016-02-22)
+-------------------
+* Changelog
+* kdl_parser_py: run_depend on urdfdom_py
+* Remove dependency in urdf_parser_py
+* kdl_parser: Adding python kdl parser
+* Contributors: Jackie Kay, Jonathan Bohren, Steven Peters
+
+1.11.8 (2015-09-11)
+-------------------
+
+1.11.7 (2015-04-22)
+-------------------
+
+1.11.6 (2014-11-30 11:17)
+-------------------------
+
+1.11.5 (2014-07-24)
+-------------------
+
+1.11.4 (2014-07-07)
+-------------------
+
+1.11.3 (2014-06-24)
+-------------------
+
+1.11.2 (2014-03-22)
+-------------------
+
+1.11.1 (2014-03-20)
+-------------------
+
+1.11.0 (2014-02-21)
+-------------------
+
+1.10.21 (2014-11-30 11:15)
+--------------------------
+
+1.10.20 (2014-08-01)
+--------------------
+
+1.10.19 (2014-02-15)
+--------------------
+
+1.10.18 (2013-12-04)
+--------------------
+
+1.10.17 (2013-11-22)
+--------------------
+
+1.10.16 (2013-11-18)
+--------------------
+
+1.10.15 (2013-08-17)
+--------------------
+
+1.10.14 (2013-07-26)
+--------------------
+
+1.10.13 (2013-07-17)
+--------------------
+
+1.10.12 (2013-07-02 08:41)
+--------------------------
+
+1.10.11 (2013-05-21)
+--------------------
+
+1.10.10 (2013-05-20)
+--------------------
+
+1.10.9 (2013-04-21)
+-------------------
+
+1.10.8 (2013-04-18 12:24)
+-------------------------
+
+1.10.7 (2013-04-18 10:19)
+-------------------------
+
+1.10.6 (2013-04-18 01:56)
+-------------------------
+
+1.10.5 (2013-04-18 01:46)
+-------------------------
+
+1.10.4 (2013-04-12)
+-------------------
+
+1.10.3 (2013-03-13 22:22)
+-------------------------
+
+1.10.2 (2013-03-13 17:34)
+-------------------------
+
+1.10.1 (2013-03-13 17:15)
+-------------------------
+
+1.10.0 (2013-03-11 19:48)
+-------------------------
+
+1.9.36 (2013-07-02 08:38)
+-------------------------
+
+1.9.35 (2013-04-29)
+-------------------
+
+1.9.34 (2013-04-18 18:17)
+-------------------------
+
+1.9.33 (2013-03-11 19:49)
+-------------------------
+
+1.9.32 (2012-12-22)
+-------------------
+
+1.9.31 (2012-12-18)
+-------------------
+
+1.9.30 (2012-12-14)
+-------------------
+
+1.9.29 (2012-12-05)
+-------------------
+
+1.9.28 (2012-11-07)
+-------------------
+
+1.9.27 (2012-11-06)
+-------------------
+
+1.9.26 (2012-11-05)
+-------------------
+
+1.9.25 (2012-10-29)
+-------------------
+
+1.9.24 (2012-10-25)
+-------------------
+
+1.9.23 (2012-10-14 15:26)
+-------------------------
+
+1.9.22 (2012-10-14 13:13)
+-------------------------
+
+1.9.21 (2012-10-14 12:25)
+-------------------------
+
+1.9.20 (2012-10-14 02:13)
+-------------------------
+
+1.9.19 (2012-10-13)
+-------------------
+
+1.9.18 (2012-10-07)
+-------------------
+
+1.9.17 (2012-10-06 21:27)
+-------------------------
+
+1.9.16 (2012-10-06 21:22)
+-------------------------
+
+1.9.15 (2012-10-06 20:47)
+-------------------------
+
+1.9.14 (2012-10-06 19:20)
+-------------------------
+
+1.9.13 (2012-09-16 16:51)
+-------------------------
+
+1.9.12 (2012-09-16 02:25)
+-------------------------
+
+1.9.11 (2012-09-15 13:45)
+-------------------------
+
+1.9.10 (2012-09-15 12:27)
+-------------------------
+
+1.9.9 (2012-09-12 14:38)
+------------------------
+
+1.9.8 (2012-09-12 14:27)
+------------------------
+
+1.9.7 (2012-09-11)
+------------------
+
+1.9.6 (2012-09-07)
+------------------
+
+1.9.5 (2012-09-06)
+------------------
+
+1.9.4 (2012-09-04)
+------------------
+
+1.9.3 (2012-09-03)
+------------------
+
+1.9.2 (2012-08-14 20:34)
+------------------------
+
+1.9.1 (2012-08-14 20:33)
+------------------------
+
+1.9.0 (2012-08-02)
+------------------
diff --git a/kdl_parser_py/CMakeLists.txt b/kdl_parser_py/CMakeLists.txt
new file mode 100644
index 0000000..6bbffaf
--- /dev/null
+++ b/kdl_parser_py/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(kdl_parser_py)
+
+find_package(catkin REQUIRED
+ COMPONENTS urdf
+)
+
+catkin_package(
+ LIBRARIES ${PROJECT_NAME}
+ CATKIN_DEPENDS urdf
+)
+
+catkin_python_setup()
+
+catkin_install_python(PROGRAMS ${PROJECT_NAME}/urdf.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(catkin REQUIRED COMPONENTS rostest)
+ add_rostest(test/test_kdl_parser.launch)
+endif()
diff --git a/kdl_parser_py/kdl_parser_py/__init__.py b/kdl_parser_py/kdl_parser_py/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/kdl_parser_py/kdl_parser_py/urdf.py b/kdl_parser_py/kdl_parser_py/urdf.py
new file mode 100644
index 0000000..99f1c4c
--- /dev/null
+++ b/kdl_parser_py/kdl_parser_py/urdf.py
@@ -0,0 +1,125 @@
+
+from __future__ import print_function
+
+import urdf_parser_py.urdf as urdf
+
+import PyKDL as kdl
+
+def treeFromFile(filename):
+ """
+ Construct a PyKDL.Tree from an URDF file.
+ :param filename: URDF file path
+ """
+
+ with open(filename) as urdf_file:
+ return treeFromUrdfModel(urdf.URDF.from_xml_string(urdf_file.read()))
+
+def treeFromParam(param):
+ """
+ Construct a PyKDL.Tree from an URDF in a ROS parameter.
+ :param param: Parameter name, ``str``
+ """
+
+ return treeFromUrdfModel(urdf.URDF.from_parameter_server())
+
+def treeFromString(xml):
+ """
+ Construct a PyKDL.Tree from an URDF xml string.
+ :param xml: URDF xml string, ``str``
+ """
+
+ return treeFromUrdfModel(urdf.URDF.from_xml_string(xml))
+
+def _toKdlPose(pose):
+ if pose and pose.rpy and len(pose.rpy) == 3 and pose.xyz and len(pose.xyz) == 3:
+ return kdl.Frame(
+ kdl.Rotation.RPY(*pose.rpy),
+ kdl.Vector(*pose.xyz))
+ else:
+ return kdl.Frame.Identity()
+
+def _toKdlInertia(i):
+ # kdl specifies the inertia in the reference frame of the link, the urdf
+ # specifies the inertia in the inertia reference frame
+ origin = _toKdlPose(i.origin)
+ inertia = i.inertia
+ return origin.M * kdl.RigidBodyInertia(
+ i.mass, origin.p,
+ kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz));
+
+def _toKdlJoint(jnt):
+
+ fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
+ rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
+ translational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis)
+
+ type_map = {
+ 'fixed': fixed,
+ 'revolute': rotational,
+ 'continuous': rotational,
+ 'prismatic': translational,
+ 'floating': fixed,
+ 'planar': fixed,
+ 'unknown': fixed,
+ }
+
+ return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
+
+def _add_children_to_tree(robot_model, root, tree):
+
+
+ # constructs the optional inertia
+ inert = kdl.RigidBodyInertia(0)
+ if root.inertial:
+ inert = _toKdlInertia(root.inertial)
+
+ # constructs the kdl joint
+ (parent_joint_name, parent_link_name) = robot_model.parent_map[root.name]
+ parent_joint = robot_model.joint_map[parent_joint_name]
+
+ # construct the kdl segment
+ sgm = kdl.Segment(
+ root.name,
+ _toKdlJoint(parent_joint),
+ _toKdlPose(parent_joint.origin),
+ inert)
+
+ # add segment to tree
+ if not tree.addSegment(sgm, parent_link_name):
+ return False
+
+ if root.name not in robot_model.child_map:
+ return True
+
+ children = [robot_model.link_map[l] for (j,l) in robot_model.child_map[root.name]]
+
+ # recurslively add all children
+ for child in children:
+ if not _add_children_to_tree(robot_model, child, tree):
+ return False
+
+ return True;
+
+def treeFromUrdfModel(robot_model, quiet=False):
+ """
+ Construct a PyKDL.Tree from an URDF model from urdf_parser_python.
+
+ :param robot_model: URDF xml string, ``str``
+ :param quiet: If true suppress messages to stdout, ``bool``
+ """
+
+ root = robot_model.link_map[robot_model.get_root()]
+
+ if root.inertial and not quiet:
+ print("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." % root.name);
+
+ ok = True
+ tree = kdl.Tree(root.name)
+
+ # add all children
+ for (joint,child) in robot_model.child_map[root.name]:
+ if not _add_children_to_tree(robot_model, robot_model.link_map[child], tree):
+ ok = False
+ break
+
+ return (ok, tree)
diff --git a/kdl_parser/package.xml b/kdl_parser_py/package.xml
similarity index 58%
copy from kdl_parser/package.xml
copy to kdl_parser_py/package.xml
index 6ec51f5..2355886 100644
--- a/kdl_parser/package.xml
+++ b/kdl_parser_py/package.xml
@@ -1,34 +1,32 @@
<package>
- <name>kdl_parser</name>
- <version>1.11.8</version>
+ <name>kdl_parser_py</name>
+ <version>1.12.3</version>
<description>
The Kinematics and Dynamics Library (KDL) defines a tree structure
to represent the kinematic and dynamic parameters of a robot
- mechanism. <tt>kdl_parser</tt> provides tools to construct a KDL
+ mechanism. <tt>kdl_parser_py</tt> provides Python tools to construct a KDL
tree from an XML robot representation in URDF.
</description>
- <author email="wim at willowgarage.com">Wim Meeussen</author>
- <maintainer email="isucan at gmail.com">Ioan Sucan</maintainer>
+ <author email="jonathan.bohren at gmail.com">Jonathan Bohren</author>
+ <maintainer email="jackie at osrfoundation.org">Jackie Kay</maintainer>
<license>BSD</license>
- <url type="website">http://ros.org/wiki/kdl_parser</url>
+ <url type="website">http://ros.org/wiki/kdl_parser_py</url>
<url type="repository">https://github.com/ros/robot_model</url>
<url type="bugtracker">https://github.com/ros/robot_model/issues</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+ <buildtool_depend>python-catkin-pkg</buildtool_depend>
<build_depend version_gte="1.3.0">orocos_kdl</build_depend>
- <build_depend>rosconsole</build_depend>
- <build_depend>roscpp</build_depend>
<build_depend>urdf</build_depend>
- <build_depend>cmake_modules</build_depend>
<build_depend>rostest</build_depend>
<run_depend version_gte="1.3.0">orocos_kdl</run_depend>
- <run_depend>rosconsole</run_depend>
- <run_depend>roscpp</run_depend>
<run_depend>urdf</run_depend>
+ <run_depend>urdfdom_py</run_depend>
+ <run_depend>python_orocos_kdl</run_depend>
</package>
diff --git a/kdl_parser_py/setup.py b/kdl_parser_py/setup.py
new file mode 100644
index 0000000..0ff6afe
--- /dev/null
+++ b/kdl_parser_py/setup.py
@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+ packages=['kdl_parser_py'],
+ package_dir={'': ''}
+)
+
+setup(**d)
diff --git a/kdl_parser_py/test/kdl_parser_py.urdf b/kdl_parser_py/test/kdl_parser_py.urdf
new file mode 100644
index 0000000..d7e86be
--- /dev/null
+++ b/kdl_parser_py/test/kdl_parser_py.urdf
@@ -0,0 +1,300391 @@
+%!PS-Adobe-3.0
+%%Creator: (ImageMagick)
+%%Title: (kdl_parser_py.urdf)
+%%CreationDate: (2015-10-12T17:22:19-07:00)
+%%BoundingBox: -0 -0 2560 1524
+%%HiResBoundingBox: 0 0 2560 1524
+%%DocumentData: Clean7Bit
+%%LanguageLevel: 1
+%%Orientation: Portrait
+%%PageOrder: Ascend
+%%Pages: 1
+%%EndComments
+
+%%BeginDefaults
+%%EndDefaults
+
+%%BeginProlog
+%
+% Display a color image. The image is displayed in color on
+% Postscript viewers or printers that support color, otherwise
+% it is displayed as grayscale.
+%
+/DirectClassPacket
+{
+ %
+ % Get a DirectClass packet.
+ %
+ % Parameters:
+ % red.
+ % green.
+ % blue.
+ % length: number of pixels minus one of this color (optional).
+ %
+ currentfile color_packet readhexstring pop pop
+ compression 0 eq
+ {
+ /number_pixels 3 def
+ }
+ {
+ currentfile byte readhexstring pop 0 get
+ /number_pixels exch 1 add 3 mul def
+ } ifelse
+ 0 3 number_pixels 1 sub
+ {
+ pixels exch color_packet putinterval
+ } for
+ pixels 0 number_pixels getinterval
+} bind def
+
+/DirectClassImage
+{
+ %
+ % Display a DirectClass image.
+ %
+ systemdict /colorimage known
+ {
+ columns rows 8
+ [
+ columns 0 0
+ rows neg 0 rows
+ ]
+ { DirectClassPacket } false 3 colorimage
+ }
+ {
+ %
+ % No colorimage operator; convert to grayscale.
+ %
+ columns rows 8
+ [
+ columns 0 0
+ rows neg 0 rows
+ ]
+ { GrayDirectClassPacket } image
+ } ifelse
+} bind def
+
+/GrayDirectClassPacket
+{
+ %
+ % Get a DirectClass packet; convert to grayscale.
+ %
+ % Parameters:
+ % red
+ % green
+ % blue
+ % length: number of pixels minus one of this color (optional).
+ %
+ currentfile color_packet readhexstring pop pop
+ color_packet 0 get 0.299 mul
+ color_packet 1 get 0.587 mul add
+ color_packet 2 get 0.114 mul add
+ cvi
+ /gray_packet exch def
+ compression 0 eq
+ {
+ /number_pixels 1 def
+ }
+ {
+ currentfile byte readhexstring pop 0 get
+ /number_pixels exch 1 add def
+ } ifelse
+ 0 1 number_pixels 1 sub
+ {
+ pixels exch gray_packet put
+ } for
+ pixels 0 number_pixels getinterval
+} bind def
+
+/GrayPseudoClassPacket
+{
+ %
+ % Get a PseudoClass packet; convert to grayscale.
+ %
+ % Parameters:
+ % index: index into the colormap.
+ % length: number of pixels minus one of this color (optional).
+ %
+ currentfile byte readhexstring pop 0 get
+ /offset exch 3 mul def
+ /color_packet colormap offset 3 getinterval def
+ color_packet 0 get 0.299 mul
+ color_packet 1 get 0.587 mul add
+ color_packet 2 get 0.114 mul add
+ cvi
+ /gray_packet exch def
+ compression 0 eq
+ {
+ /number_pixels 1 def
+ }
+ {
+ currentfile byte readhexstring pop 0 get
+ /number_pixels exch 1 add def
+ } ifelse
+ 0 1 number_pixels 1 sub
+ {
+ pixels exch gray_packet put
+ } for
+ pixels 0 number_pixels getinterval
+} bind def
+
+/PseudoClassPacket
+{
+ %
+ % Get a PseudoClass packet.
+ %
+ % Parameters:
+ % index: index into the colormap.
+ % length: number of pixels minus one of this color (optional).
+ %
+ currentfile byte readhexstring pop 0 get
+ /offset exch 3 mul def
+ /color_packet colormap offset 3 getinterval def
+ compression 0 eq
+ {
+ /number_pixels 3 def
+ }
+ {
+ currentfile byte readhexstring pop 0 get
+ /number_pixels exch 1 add 3 mul def
+ } ifelse
+ 0 3 number_pixels 1 sub
+ {
+ pixels exch color_packet putinterval
+ } for
+ pixels 0 number_pixels getinterval
+} bind def
+
+/PseudoClassImage
+{
+ %
+ % Display a PseudoClass image.
+ %
+ % Parameters:
+ % class: 0-PseudoClass or 1-Grayscale.
+ %
+ currentfile buffer readline pop
+ token pop /class exch def pop
+ class 0 gt
+ {
+ currentfile buffer readline pop
+ token pop /depth exch def pop
+ /grays columns 8 add depth sub depth mul 8 idiv string def
+ columns rows depth
+ [
+ columns 0 0
+ rows neg 0 rows
+ ]
+ { currentfile grays readhexstring pop } image
+ }
+ {
+ %
+ % Parameters:
+ % colors: number of colors in the colormap.
+ % colormap: red, green, blue color packets.
+ %
+ currentfile buffer readline pop
+ token pop /colors exch def pop
+ /colors colors 3 mul def
+ /colormap colors string def
+ currentfile colormap readhexstring pop pop
+ systemdict /colorimage known
+ {
+ columns rows 8
+ [
+ columns 0 0
+ rows neg 0 rows
+ ]
+ { PseudoClassPacket } false 3 colorimage
+ }
+ {
+ %
+ % No colorimage operator; convert to grayscale.
+ %
+ columns rows 8
+ [
+ columns 0 0
+ rows neg 0 rows
+ ]
+ { GrayPseudoClassPacket } image
+ } ifelse
+ } ifelse
+} bind def
+
+/DisplayImage
+{
+ %
+ % Display a DirectClass or PseudoClass image.
+ %
+ % Parameters:
+ % x & y translation.
+ % x & y scale.
+ % label pointsize.
+ % image label.
+ % image columns &