Joint ordering bug in JointTrajectoryActionController (ros ticket #5148)

[ahendrix]

For electric release of pr2_controller 1.5.6.

I have a python script to grab the current positions of the left arm joints from from the {{{l_arm_controller/state}}} topic.

The python script then uses those positions action interface to send a goal trajectory the the l_arm_controller using the !SimpleActionClient. The trajectory has a single point and all velocities and accelerations are zero.

Since goal point is based on current joint positions, the arm should only move minimal amounts.

However, the desired position for the l_forearm_roll joint and the l_write_roll_joint do not match the goal position.

I've attached a short capture of the l_arm_controller_messages when this script is run.

It should be possible to reproduce this issue by run the "simple_example.py" script in the "doorknob" package that is available here {{{ https://code.ros.org/svn/wg-ros-pkg/branches/dking/dkstuff }}}

trac data:

• Owner: sglaser
• Reporter: dking
• Reported at: Tue Sep 20 22:49:31 2011
• URL: https://code.ros.org/trac/wg-ros-pkg/ticket/5148

[ahendrix]

[dking] Things work as expected if I publish the goal with the same ordering for the joint_name list.
I've added a '''-o''' option to the simple_example script that will do this: {{{ rosrun doorknob simple_example.py -o }}}

The '''commandTrajectory()''' function makes a '''lookup''' table that maps its internal joint ordering to the joint ordering the in the goal message. However, the following code does not use this lookup for wrapping the angle of the continuous joints:

from '''JointTrajectoryActionController::commandTrajectory''' in ''joint_trajectory_actioncontroller.cpp'': {{{ // Checks if we should wrap std::vector wrap(joints.size(), 0.0); assert(!msg->points[0].positions.empty()); for (sizet j = 0; j < joints.size(); ++j) { if (joints[j]->joint->type == urdf::Joint::CONTINUOUS) { double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]); wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j]; } } }}}

This explains why only the forearm_roll and wrist_roll were having a problem

[ahendrix]

[sglaser] Fixed in r53797 (pr2_controllers 1.7.2). Thanks for the helpful bug report.