Wrong EEF tf values with newer version (circa 1.0.27) #287

130s · 2014-11-21T15:37:40Z

A user is running a script attached at the bottom. With the following version of packages, tf frame RARM_JOINT5_Link and RARM_JOINT5_Link_Controller overlaps (almost) precisely, as expected.

hironx_ros_bridge 1.0.14
hrpsys_tools 1.2.1
hrpsys_ros_bridge 1.2.1
hrpsys 315.2.2

But with the versions currently available, 2 frames are displaced.

hironx_ros_bridge 1.0.27
hrpsys_tools 1.2.6
hrpsys_ros_bridge 1.2.6
hrpsys 315.2.7

#!/usr/bin/env python

from hrpsys import rtm
import rospy
import tf

from nextage_ros_bridge import nextage_client


class CoordSyst():

    def __init__(self):
        rtm.nshost = 'localhost'
        rtm.nsport = 15005
        self.hiro = nextage_client.NextageClient()
#        self.hiro.init(robotname='RobotHardware0', url='/opt/jsk/etc/HIRONX/model/main.wrl')
        self.hiro.init("HiroNX(Robot)0", '')

    def publishTF(self):
        global syst
        pos = self.hiro.getCurrentPosition('RARM_JOINT5')
        rpy = self.hiro.getReferenceRPY('RARM_JOINT5')
        syst.sendTransform((pos[0], pos[1], pos[2]),
                           tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2]),
                           rospy.Time.now(),
                           "RARM_JOINT5_Link_Controller",
                           "WAIST")

if __name__ == "__main__":
    rospy.init_node('tf_publisher')

    syst = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    coordSyst = CoordSyst()

    while not rospy.is_shutdown():
        coordSyst.publishTF()
        rate.sleep()

The text was updated successfully, but these errors were encountered:

k-okada · 2014-11-22T14:27:03Z

I think this is something related collada model or collada parser, not related to the EEF nor version of hrpsys. To confirm this, could you confirm following things

even if you used newest version, EEF of tf and controller did not differs on the posture shown in the top figure
check the RARM_JOINT0_link for tf and controller, it the joint angle value is 0, it same. But if you set joint angle to , say 45 degree, it differs.

If this insight is correct, next step is to check if this is the problem of tf or controller,

            syst.sendTransform((pos[0], pos[1], pos[2]),
                               tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2]),
                              tf.transformations.quaternion_multiply(
                    tf.transformations.quaternion_about_axis(deg2rad(15.1768), [1,0,0]),
                    tf.transformations.quaternion_about_axis(deg2rad(45), [0,0,1])),
                               rospy.Time.now(),
                               "RARM_JOINT"+str(i)+"_Link_Controller",
                               "WAIST")
# deg2rad is additionally defined.

is also differs from tf, so it seems something wrong on tf, that's uses collada_parser,

So, my guess on it is related to the collada model is if we convert kawada_hironx.dae (https://github.com/start-jsk/rtmros_hironx/blob/hydro-devel/hironx_ros_bridge/models/kawada-hironx.dae) to urdf, using rosrun collada_urdf urdf_to_collada kawada-hironx.dae and when we use them, it also have same erros.

Then we look into urdf file and it has

  <joint name="RARM_JOINT0" type="revolute">
    <parent link="CHEST_JOINT0_Link"/>
    <child  link="RARM_JOINT0_Link"/>
    <origin xyz="0 -0.145 0.370296" rpy="0.261799 -0 0 "/>
    <axis   xyz="0 -0.258819 0.965926"/ >
    <limit lower="-1.53589" upper="1.53589" effort="100" velocity="3.00197" />
    <dynamics damping="0.2" friction="0" />
  </joint>

and this is not correct, this should be

  <joint name="RARM_JOINT0" type="revolute">
    <parent link="CHEST_JOINT0_Link"/>
    <child  link="RARM_JOINT0_Link"/>
    <origin xyz="0 -0.145 0.370296" rpy="0.261799 -0 0 "/>
    <!-- axis   xyz="0 -0.258819 0.965926"/ -->
    <axis   xyz="0 0 1"/>
    <limit lower="-1.53589" upper="1.53589" effort="100" velocity="3.00197" />
    <dynamics damping="0.2" friction="0" />
  </joint>

and using this will get expected behavior.

And I'm not sure how to fix them, but one solution seems to removing

https://github.com/ros/robot_model/blob/indigo-devel/collada_parser/src/collada_parser.cpp#L1048
line,

and another one is to change dae file as

diff --git a/hironx_ros_bridge/models/kawada-hironx.dae b/hironx_ros_bridge/models/kawada-hironx.dae
index 5d9fea9..69468ab 100644
--- a/hironx_ros_bridge/models/kawada-hironx.dae
+++ b/hironx_ros_bridge/models/kawada-hironx.dae
@@ -82,8 +82,8 @@ available at http://www.eclipse.org/legal/epl-v10.html
                                                </node>
                                                <node id="v1.node4" name="RARM_JOINT0_Link" sid="node4">
                                                        <translate>0 -0.145 0.370296</translate>
-                                                       <rotate>1 0 0 15</rotate>
                                                        <rotate sid="node_joint4_axis0">0 0 1 0</rotate>
+                                                       <rotate>1 0 0 15</rotate>
                                                        <instance_geometry url="#g1_4_geom0">
                                                                <bind_material>
                                                                        <technique_common>

(off course, we need to change for LARM too)

I'm not confident on this solution, but changing collada_parser and release them with dep takes time so change dae may good solutoin, It may have some side effects ,so we need to run test code carefully, and if test code have some constant value that depends on this wrong dae, then we have to fix them. We also better to add test code to check this problem which reported on this issue.

Several things that I have not confirmed is:

that original dae is totally wrong, we need to check if it is ok using openrave
need to check dae and urdf of nextage model

k-okada · 2014-11-22T14:30:18Z

@YoheiKakiuchi , this is complex problem, and my proposal is to remove
https://github.com/ros/robot_model/blob/indigo-devel/collada_parser/src/collada_parser.cpp#L1048
but it may have may side effects.

It seems the problem only occurs with the aixs with offset, like RARM_JOINT0 of hironx, The question is do you have eve used similar robot model? HRP does not have these joints, I think. Atlas may have one. PA-10 have similar offset in collada model?

YoheiKakiuchi · 2014-11-22T18:31:11Z

I understand problem.

This code is wrong.
https://github.com/ros/robot_model/blob/indigo-devel/collada_parser/src/collada_parser.cpp#L1048
From urdf specification(http://wiki.ros.org/urdf/XML/joint), joint axis is in child frame.
But this code convert joint axis to parent frame.

This sample has the same joint, but this sample can be converted correctly (this is not good sample).
https://github.com/YoheiKakiuchi/openhrp3-1/blob/master/sample/model/sample3dof.dae

this will create correct urdf. (but it is not correct collada file)

diff --git a/hironx_ros_bridge/models/kawada-hironx.dae b/hironx_ros_bridge/models/kawada-hironx.dae
index 5d9fea9..9e10c61 100644
--- a/hironx_ros_bridge/models/kawada-hironx.dae
+++ b/hironx_ros_bridge/models/kawada-hironx.dae
@@ -1670,8 +1670,8 @@ available at http://www.eclipse.org/legal/epl-v10.html
                                                        </attachment_full>
                                                        <attachment_full joint="kmodel1/jointsid4">
                                                                <translate>0 -0.145 0.370296</translate>
-                                                               <rotate>1 0 0 15</rotate>
                                                                <link sid="link4" name="RARM_JOINT0_Link">
+                                                                  <rotate>1 0 0 15</rotate>
                                                                        <attachment_full joint="kmodel1/jointsid5">
                                                                                <translate>0 0 0</translate>
                                                                                <rotate>0 0 1 0</rotate>

k-okada · 2014-11-23T08:25:59Z

this will create correct urdf. (but it is not correct collada file)

Do you mean that changes in #287 (comment) with node is not correct and have to change attachment_full ?

130s · 2014-11-24T08:38:05Z

I made a temporary patch to collada_parser and with small manual tests the issue seems to be gone.

I'll have to test more by adding unittests with NEXTAGE OPEN.

@YoheiKakiuchi Is this worth opening an issue ticket in robot_model to raise attention among the people there?

YoheiKakiuchi · 2014-11-24T08:44:04Z

Do you mean that changes in #287 (comment) with node is not correct and have to change attachment_full ?

I think so. Node is visual node, then if you change node, visual (pose of mesh) is changed but kinematics of arm is not changed. Attachment_full is kinematics configuration.

@YoheiKakiuchi Is this worth opening an issue ticket in robot_model to raise attention among the people there?

Yes, please. I will make patch for robot_model.

130s · 2014-11-24T08:44:48Z

Just fyi; a code to publish sample tf (mentioned in #287 (comment)) is uploaded with a fix.

YoheiKakiuchi · 2014-11-25T02:31:25Z

This PR will fix this problem.
ros/robot_model#92
ros/robot_model#93

k-okada · 2014-11-28T13:23:23Z

I think, I bit a misunderstand. ModelLoader in hrpsys load collada correctly, but collada_parser in ROS did not, so we need two different dae for simulation, for real robot, ModelLoader uses wrl.

Create (temporary) fixed dae

k-okada@ubuntu:~/catkin_ws/ws_287/src/rtmros_hironx/hironx_ros_bridge$ diff -u models/kawada-hironx.dae models/kawada-hironx_287.dae
--- models/kawada-hironx.dae    2014-11-28 05:14:16.947813426 -0800
+++ models/kawada-hironx_287.dae    2014-11-28 05:14:09.299813289 -0800
@@ -82,8 +82,8 @@
                        </node>
                        <node id="v1.node4" name="RARM_JOINT0_Link" sid="node4">
                            <translate>0 -0.145 0.370296</translate>
-                           <rotate>1 0 0 15</rotate>
                            <rotate sid="node_joint4_axis0">0 0 1 0</rotate>
+                           <rotate>1 0 0 15</rotate>
                            <instance_geometry url="#g1_4_geom0">
                                <bind_material>
                                    <technique_common>
@@ -1670,8 +1670,8 @@
                            </attachment_full>
                            <attachment_full joint="kmodel1/jointsid4">
                                <translate>0 -0.145 0.370296</translate>
-                               <rotate>1 0 0 15</rotate>
                                <link sid="link4" name="RARM_JOINT0_Link">
+                                       <rotate>1 0 0 15</rotate>
                                    <attachment_full joint="kmodel1/jointsid5">
                                        <translate>0 0 0</translate>
                                        <rotate>0 0 1 0</rotate>

fix launch file so that ModelLoader uses original dae and ROS uses fixed version

$ git diff
diff --git a/hironx_ros_bridge/launch/hironx_ros_bridge.launch b/hironx_ros_bridge/launch/hironx_ros_bridge.launch
index de474cb..000eab9 100644
--- a/hironx_ros_bridge/launch/hironx_ros_bridge.launch
+++ b/hironx_ros_bridge/launch/hironx_ros_bridge.launch
@@ -6,7 +6,8 @@

   <arg name="nameserver" default="hiro014" /> <!-- for real robot -->
   <arg name="MODEL_FILE" default="$(find hironx_ros_bridge)/models/kawada-hironx.dae" />
-  <arg name="COLLADA_FILE" default="$(find hironx_ros_bridge)/models/kawada-hironx.dae" />
+  <arg name="COLLADA_FILE" default="$(find hironx_ros_bridge)/models/kawada-hironx_287.dae" />
   <arg name="SIMULATOR_NAME" default="RobotHardware0" />
   <arg name="corbaport" default="15005" />
   <arg name="CONF_FILE_COLLISIONDETECT" default="$(find hironx_ros_bridge)/conf/kawada-hironx.conf" /> <!-- Default is simulation -->

this should work.

130s · 2014-11-30T05:59:01Z

I think, I bit a misunderstand. ModelLoader in hrpsys load collada correctly, but collada_parser in ROS did not, so we need two different dae for simulation, for real robot, ModelLoader uses wrl.

So, I just like to confirm that for real robots we don't need any change because .wrl file (stored inside of QNX for hiro-nxo) is used? If this is correct it sounds like this issue should only happen with simulation?

130s · 2014-11-30T05:59:51Z

For Hironx: Manual fix to issue#287 for Hironx. #290 seems to work well for me.
For For NXO: I made a very similar change to the packages in rtmros_nextage Manual fix to issue#287 for NXO. tork-a/rtmros_nextage#134 but this issue still occurs.

130s · 2014-11-30T06:51:13Z

For For NXO: I made a very similar change to the packages in rtmros_nextage tork-a/rtmros_nextage#134 but this issue still occurs.

With the patch at collada_parser ros/robot_model#93 (hydro), the issue is gone.

130s · 2014-11-30T07:09:28Z

With the patch at collada_parser ros/robot_model#93 (hydro), without the temporary patch #290, this issue doesn't happen.

@k-okada I might be still misunderstanding something, but do we still need a patch like #290?

$ rospack find hironx_ros_bridge 
/opt/ros/hydro/share/hironx_ros_bridge
$ rospack find collada_parser 
/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/src/ros/robot_model/collada_parser
$ dpkg -p ros-hydro-hironx-ros-bridge |grep Ver
Version: 1.0.27-0precise-20141106-0321-+0000
$ env|grep ros
ROS_ROOT=/opt/ros/hydro/share/ros
ROS_PACKAGE_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/src:/opt/ros/hydro/share:/opt/ros/hydro/stacks
LD_LIBRARY_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/lib:/opt/ros/hydro/lib
CPATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/include:/opt/ros/hydro/include
PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/bin:/opt/ros/hydro/bin:/home/n130s/data/Dropbox/pg/Lateeye/bashapp:/usr/lib/lightdm/lightdm:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games
PYTHONPATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/lib/python2.7/dist-packages:/opt/ros/hydro/lib/python2.7/dist-packages
PKG_CONFIG_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/lib/pkgconfig:/opt/ros/hydro/lib/pkgconfig
CMAKE_PREFIX_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel:/opt/ros/hydro
ROS_ETC_DIR=/opt/ros/hydro/etc/ros
$ tree -L 3
.
└── src
    ├── CMakeLists.txt -> /opt/ros/hydro/share/catkin/cmake/toplevel.cmake
    └── ros
        └── robot_model

k-okada · 2014-12-01T03:25:13Z

dae is used for all ros-bridge, (ros system could not understand wrl) so
this problem occurs both real robot and simulation
the problem has gone with newer version of robot_model, so I don't think
we need a patch.
but current user (who usually uses released, not shadow-fixed to avoid
confusion) they need temporally fix until next sync, and they usually uses
rtmros_hironx from deb, so we need to script to patch
hiroinx_ros_bridge.launch and copy fixed version of dae

On Sun, Nov 30, 2014 at 4:09 PM, Isaac I.Y. Saito [email protected]
wrote:

With the patch at collada_parser ros/robot_model#93
ros/robot_model#93 (hydro), without the
temporary patch #290 #290,
this issue doesn't happen.

@k-okada https://github.com/k-okada I might be still misunderstanding
something, but do we still need a patch like #290
#290?

$ rospack find hironx_ros_bridge
/opt/ros/hydro/share/hironx_ros_bridge
$ rospack find collada_parser
/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/src/ros/robot_model/collada_parser
$ dpkg -p ros-hydro-hironx-ros-bridge |grep Ver
Version: 1.0.27-0precise-20141106-0321-+0000
$ env|grep ros
ROS_ROOT=/opt/ros/hydro/share/ros
ROS_PACKAGE_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/src:/opt/ros/hydro/share:/opt/ros/hydro/stacks
LD_LIBRARY_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/lib:/opt/ros/hydro/lib
CPATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/include:/opt/ros/hydro/include
PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/bin:/opt/ros/hydro/bin:/home/n130s/data/Dropbox/pg/Lateeye/bashapp:/usr/lib/lightdm/lightdm:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games
PYTHONPATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/lib/python2.7/dist-packages:/opt/ros/hydro/lib/python2.7/dist-packages
PKG_CONFIG_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel/lib/pkgconfig:/opt/ros/hydro/lib/pkgconfig
CMAKE_PREFIX_PATH=/home/tenkiwa/ROS/hydro_precise/cws_robotmodel/devel:/opt/ros/hydro
ROS_ETC_DIR=/opt/ros/hydro/etc/ros
$ tree -L 3
.
└── src
├── CMakeLists.txt -> /opt/ros/hydro/share/catkin/cmake/toplevel.cmake
└── ros
└── robot_model

—
Reply to this email directly or view it on GitHub
#287 (comment)
.

130s · 2014-12-02T11:06:06Z

dae is used for all ros-bridge, (ros system could not understand wrl) so this problem occurs both real robot and simulation

the problem has gone with newer version of robot_model, so I don't think we need a patch.

Got it. #290 is closed now.

but current user (who usually uses released, not shadow-fixed to avoid confusion) they need temporally fix until next sync, and they usually uses rtmros_hironx from deb, so we need to script to patch hiroinx_ros_bridge.launch and copy fixed version of dae

IMHO this depends on how we (service providers) want to provide the hotfix to the users. A patch script is very nice for the users perspective. But because editing protected area (eg. /opt) is generally not recommended (no matter how easy or not reverting the fix is), I'm personally not convinced with that idea. If modifying /opt is well-supported practice, I'm happy to follow that.

So for now for the users I'm talking to, I suggested downloading robot_mode, switch to hydro-devel branch and build the catkin workspace.

Not recommending shadow-fixed for end-users is +1; it's only in an advanced tutorial of NXO.

k-okada · 2014-12-02T13:53:53Z

edprotected area (eg. /opt) is generally not recommended (no matter how
easy or not to revert the fix), I

Yes, generally it should be prohibited.

So for now for the users I'm talking to, I suggested downloading
robot_mode, switch to hydro-devel branch and build the catkin workspace.

Oh, clone and checkout,,,, we should understand that not everyone is
familiar with git...

add test code to check #287 problem

workaround for #287 issue

130s added the bug label Nov 21, 2014

130s pushed a commit to 130s/robot_model that referenced this issue Nov 24, 2014

Tentative fix to frame conversion issue start-jsk/rtmros_hironx#287

560449f

130s self-assigned this Nov 24, 2014

YoheiKakiuchi mentioned this issue Nov 25, 2014

Fix rotation of joint axis ros/robot_model#92

Merged

This was referenced Nov 30, 2014

Manual fix to issue#287 for Hironx. #290

Closed

Manual fix to issue#287 for NXO. tork-a/rtmros_nextage#134

Closed

k-okada added a commit to k-okada/rtmros_hironx that referenced this issue Dec 1, 2014

add test code to check start-jsk#287 problem

4c71191

130s closed this as completed Dec 2, 2014

k-okada added a commit that referenced this issue Jan 7, 2015

Merge pull request #291 from k-okada/add_test_for_287

07535cd

add test code to check #287 problem

k-okada added a commit to k-okada/rtmros_hironx that referenced this issue Feb 15, 2015

relax test code, that start-jsk#287 is not closed

cd6f4c3

k-okada mentioned this issue Feb 15, 2015

workaround for #287 issue #325

Merged

130s added a commit that referenced this issue Feb 15, 2015

Merge pull request #325 from k-okada/still_need_to_fix_287

f915b1c

workaround for #287 issue

k-okada mentioned this issue Feb 24, 2015

[BodyInfoCollada_impl.cpp] fix for wrong collada interpretation, joint axis is in child frame fkanehiro/openhrp3#47

Merged

130s mentioned this issue Mar 17, 2015

yaw movement with setTargetPoseRelative actually moves roll #335

Closed

130s mentioned this issue Mar 29, 2015

COLLADA error: Trying to load an invalid COLLADA version for this DOM build! #348

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Wrong EEF tf values with newer version (circa 1.0.27) #287

Wrong EEF tf values with newer version (circa 1.0.27) #287

130s commented Nov 21, 2014

k-okada commented Nov 22, 2014

k-okada commented Nov 22, 2014

YoheiKakiuchi commented Nov 22, 2014

k-okada commented Nov 23, 2014

130s commented Nov 24, 2014

YoheiKakiuchi commented Nov 24, 2014

130s commented Nov 24, 2014

YoheiKakiuchi commented Nov 25, 2014

k-okada commented Nov 28, 2014

130s commented Nov 30, 2014

130s commented Nov 30, 2014

130s commented Nov 30, 2014

130s commented Nov 30, 2014

k-okada commented Dec 1, 2014

130s commented Dec 2, 2014

k-okada commented Dec 2, 2014

Wrong EEF tf values with newer version (circa 1.0.27) #287

Wrong EEF tf values with newer version (circa 1.0.27) #287

Comments

130s commented Nov 21, 2014

k-okada commented Nov 22, 2014

k-okada commented Nov 22, 2014

YoheiKakiuchi commented Nov 22, 2014

k-okada commented Nov 23, 2014

130s commented Nov 24, 2014

YoheiKakiuchi commented Nov 24, 2014

130s commented Nov 24, 2014

YoheiKakiuchi commented Nov 25, 2014

k-okada commented Nov 28, 2014

130s commented Nov 30, 2014

130s commented Nov 30, 2014

130s commented Nov 30, 2014

130s commented Nov 30, 2014

k-okada commented Dec 1, 2014

130s commented Dec 2, 2014

k-okada commented Dec 2, 2014