PlayJointTrajectory goes the long way round.

[jabbershort]

Description

When using the service PlayJointTrajectory @ /my_gen3/base/play_joint_trajectory, any of the unrestricted rotational joints (i.e. joint 1, 3, 5, 7) will go the long way round to reach the goal when the goal passes 180 degrees. I.e.

When the joint is at 170 degrees and the goal is 190, it will travel via zero to reach this. The same behaviour occurs when going from 190 -> 170.

Version

ROS distribution : Melodic

Branch and commit you are using : melodic-devel

Steps to reproduce

So far only tested with gazebo simulation, will try with the physical arm later this week:

1. roslaunch kortex_gazebo my_custom_launch.launch (based on spawn_kortex_robot.launch)
2. rosservice call /my_robot_name/base/play_joint_trajectory "input:
   joint_angles:
   joint_angles:
   • joint_identifier: 1
     value: 0.0
   • joint_identifier: 2
     value: 0.0
   • joint_identifier: 3
     value: 170.0
   • joint_identifier: 4
     value: 0.0
   • joint_identifier: 5
     value: 0.0
   • joint_identifier: 6
     value: 0.0
   • joint_identifier: 7
     value: 0.0
     constraint:
     type: 0
     value: 0.0"

And then again with 190 for joint 3.

[felixmaisonneuve]

Hi @jabbershort,

I have looked into it briefly. I found this

ros_kortex/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp

Lines 819 to 820 in 476faba

	m_math_util.wrapRadiansFromMinusPiToPi(current.position[m_first_arm_joint_index + i], n_turns);
	const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value));

I think there is a problem, the fonction takes the number of turn of the current position instead of the goal. I think if you change it to something like this, it might fix it. (you will need to recompile everything for this to take effect)

int n_turns = 0;
const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value), n);

Notice the ,n at the end of wrapRadiansFromMinusPiToPi. I also deleted the line above, since it is doing nothing I think.

Also, if you test on a real robot, I expect the real robot to not travel via zero.

If you can test this and tell me if it solve the issue, I would very much appreciate it.
I'll make sure to update it after.

Regards,
Felix

[jabbershort]

Hi @felixmaisonneuve

Thank you for such a rapid response! I've added that code and initially it failed, it didn't recognise n. I changed it to n_turns instead and that has compiled and now it doesn't travel via zero anymore. I hope I haven't just broken it in another way, but I think n is the function variable for wrapRadiansFromMinusPiToPi, rather than the variable in kortex_arm_simulation.cpp?

i.e

int n_turns = 0;
const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value), n_turns);

I'm visiting my robots on thursday, so will give it a try both ways then and let you know.

[felixmaisonneuve]

Hi @jabbershort,

That's right, n_turns is correct.

I will wait for you to test.

Best,
Felix

[jabbershort]

Hi @felixmaisonneuve,

Sorry to be the bearer of bad news, but it turns out that fix has just introduced a new problem I hadn't noticed until now. It seems that fixing the n_turns to 0 has prevented the other joints (2, 4, 6) from running properly. Here is an example of what it looks like when you 'home' the arm now.

Whereas this is what it looked like before the change using exactly the same command.

[felixmaisonneuve]

I guess it makes sense. We probably need to use one method for limited joints (with boundaries) and the other method for unlimited joints (so they can move without passing by 0).

We could probably do something like that

// Transform kortex structure to trajectory_msgs to fill endpoint structure
    trajectory_msgs::JointTrajectoryPoint endpoint;
    int limited_joints[3]; // joints limited in range
    int degrees_of_freedom = constrained_joint_angles.joint_angles.joint_angles.size();
    if (degrees_of_freedom == 6)
    {
        limited_joints = {1,2,4};
    }
    else if (degrees_of_freedom == 7)
    {
        limited_joints = {1,3,5};
    }
    else 
    {
        ROS_ERROR("Unsupported numbers of joints : %i", degrees_of_freedom);
        return;
    }
    for (int i = 0; i < constrained_joint_angles.joint_angles.joint_angles.size(); i++)
    {
        // If the current actuator has turned on itself many times, we need the endpoint to follow that trend too
        int n_turns = 0;
        if (std::count (limited_joints, limited_joints+3, i))
        {
            const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value));
        }
        else
        {
            const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value), n_turns);
        }
        endpoint.positions.push_back(rad_wrapped_goal + double(n_turns) * 2.0 * M_PI);
        endpoint.velocities.push_back(0.0);
        endpoint.accelerations.push_back(0.0);
    }

Try this (hopefully it will compile). Keep me updated if it works or not

Best,
Felix

[jabbershort]

Hey @felixmaisonneuve

Thanks for sending that over so quick. I've tested it and done some tweaking but I can't seem to get it to compile.

I'm getting the following couple of errors:

/catkin_workspace/src/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp:819:32: error: assigning to an array from an initializer list
         limited_joints = {1,2,4}; // joints limited in range
                                ^

I tried changing the assignment of the array, but I'm not that experienced with C++ and couldn't get it working properly. I get the same error on both lines assinging an array to limited_joints.

The next error is:

/catkin_workspace/src/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp:828:9: error: return-statement with no value, in function returning ‘kortex_driver::KortexError {aka kortex_driver::KortexError_<std::allocator<void> >}’ [-fpermissive]
         return;
         ^~~~~~

I also got an error with the variable rad_wrapped_goal, because it wasn't available outside its scope, so I amend this section to this:

        double rad_wrapped_goal;
        if (std::count (limited_joints, limited_joints+3, i))
        {
            rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value));
        }
        else
        {
            rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value), n_turns);
        }

Hope this makes sense.

Do you have any suggestions for the first two?

Thanks,
Matt

[felixmaisonneuve]

Hi @jabbershort,

So I stopped being lazy and actually tried to compile it. This a version something that should work (works for me)

// Transform kortex structure to trajectory_msgs to fill endpoint structure
    trajectory_msgs::JointTrajectoryPoint endpoint;
    std::vector<int> limited_joints; // joints limited in range
    int degrees_of_freedom = constrained_joint_angles.joint_angles.joint_angles.size();
    if (degrees_of_freedom == 6)
    {
        limited_joints = {1,2,4};
    }
    else if (degrees_of_freedom == 7)
    {
        limited_joints = {1,3,5};
    }
    else 
    {
        return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE,
                                kortex_driver::SubErrorCodes::INVALID_PARAM,
                                "Unsupported number of joints, expected 6 or 7");
    }
    for (int i = 0; i < constrained_joint_angles.joint_angles.joint_angles.size(); i++)
    {
        // If the current actuator has turned on itself many times, we need the endpoint to follow that trend too
        int n_turns = 0;
        double rad_wrapped_goal;
        if (std::count (limited_joints.begin(), limited_joints.end(), i))
        {
            rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value));
        }
        else
        {
            rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value), n_turns);
        }
        endpoint.positions.push_back(rad_wrapped_goal + double(n_turns) * 2.0 * M_PI);
        endpoint.velocities.push_back(0.0);
        endpoint.accelerations.push_back(0.0);
    }

Try it and tell me if it solves your issue. If it does, I will add this modification to our current code base.

Best,
Felix

[jabbershort]

@felixmaisonneuve
Thanks so much, that appears to be working well now. I'll give it a test with the real robot tomorrow and let you know.

Thanks again.
Matt

[felixmaisonneuve]

Hi @jabbershort,

I did some testing on my side and everything seems to work fine. I opened a PR and I will soon merge this.

Thank You for your help,
Felix