DX100 / SIA50D stops moving after several trajectories (no error / alarm)

[user107298]

I'm running a simple demo script which sends a series of two-waypoint jointspace trajectories to test out ROS control of an SIA50D with a DX100 controller.

The script is here:

Click to expand

import sys
import rospy
import actionlib
import time
import math

from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from sensor_msgs.msg import JointState
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint


def create_trajectory(q_target: list, duration: float):
    goal = FollowJointTrajectoryGoal()
    goal.trajectory = JointTrajectory()

    goal.trajectory.joint_names = [
        'joint_1_s',
        'joint_2_l',
        'joint_3_e',
        'joint_4_u',
        'joint_5_r',
        'joint_6_b',
        'joint_7_t'
    ]

    robot_joint_states = rospy.wait_for_message('joint_states', JointState)

    # make sure the state we get contains the same joints (both amount and names)
    if set(goal.trajectory.joint_names) != set(robot_joint_states.name):
        rospy.logfatal("Mismatch between joints specified and seen in current "
                       "JointState. Expected: '{}', got: '{}'. Cannot continue.".format(
                           ', '.join(robot_joint_states.name),
                           ', '.join(goal.trajectory.joint_names)))
        sys.exit(1)

    q0 = list(robot_joint_states.position)
    qdot = [0.0] * len(goal.trajectory.joint_names)

    goal.trajectory.points.append(
        JointTrajectoryPoint(
            positions=q0,
            velocities=qdot,
            time_from_start=rospy.Duration(0.0)
        )
    )

    goal.trajectory.points.append(
        JointTrajectoryPoint(
            positions=q_target,
            velocities=qdot,
            time_from_start=rospy.Duration(duration)
        )
    )

    return goal


def main():
    rospy.init_node('simple_trajectory_action_client')

    # create client and make sure it's available
    client = actionlib.SimpleActionClient(
        'joint_trajectory_action', FollowJointTrajectoryAction)
    rospy.loginfo('Waiting for driver\'s action server to become available ..')
    client.wait_for_server()
    rospy.loginfo('Connected to trajectory action server')

    robot_joint_states = rospy.wait_for_message('joint_states', JointState)
    q0 = list(robot_joint_states.position)

    for i in range(6):
        q_goal = list(q0)
        q_goal[i] = q_goal[i] + math.radians(30)
        goal = create_trajectory(q_goal, 2)

        # goal constructed, submit it for execution
        rospy.loginfo("Submitting goal ..")
        client.send_goal(goal)
        rospy.loginfo("Waiting for completion ..")
        client.wait_for_result()
        rospy.loginfo('Done.')

        time.sleep(0.1)
        q_goal = list(q0)
        goal = create_trajectory(q_goal, 2)

        # goal constructed, submit it for execution
        rospy.loginfo("Submitting goal ..")
        client.send_goal(goal)
        rospy.loginfo("Waiting for completion ..")
        client.wait_for_result()
        rospy.loginfo('Done.')


if __name__ == '__main__':
    main()

After successfully moving the S joint back and forth, this script will semi-reliably hang after moving the L joint forward.

• No ROS error messages are printed
• No alarms show on the teach pendant
• No error messages are printed over telnet

Wireshark analysis shows that the robot status is "in motion", perhaps this is why it isn't accepting new commands?

Looking visually at the "robot current pos" view on the teach pendant I see a jitter of around 150 pulse counts during station holding.

Based on the above, I modified Controller.h:76 to START_MAX_PULSE_DEVIATION = 150, built, and loaded the application on the DX100. However, this new binary doesn't seem to change the behavior.

I spoke with @ted-miller briefly who suggested that this may be the Functional Safety Unit interrupting motion. However, it seems that my DX100 is not equipped with an FSU (there are no related menu items, including ROBOT RANGE, in the ROBOT menu under MANAGEMENT mode).

I should also note that point to point job programming on the teach pendant works perfectly for a very wide range of motion and at high speeds. Manual jogging also works without issue, so I do not believe that I have a faulty unit.

[gavanderhoorn]

Could you please provide more information on your setup?

Version of the system software (DX100), version of ROS, how you installed things, which OS you're using, where (and how) you downloaded the files you are using, the commands used to start motoman_driver (specific .launch files, command line arguments, etc), any and all roslaunch output (make sure to start your .launch file(s) with roslaunch --screen <rest of your command line args>), etc, etc.

You mention Wireshark: please make a capture available. Ideally one that shows both successful motion and the 'hanging' you observe.

Additionally, use rosbag to record relevant topics, especially the traffic between all the nodes from motoman_driver, industrial_robot_client and your client script.

re: client script: some high-level comments: you don't check the result of the action execution anywhere. That would be a good thing to add, regardless of the issue you encounter.

Additionally, I would suggest to also always check RobotStatus::in_motion, RobotStatus::in_error and RobotStatus::motion_possible (on the /robot_status topic).

[gavanderhoorn]

I'm happy to report that 1.9.11 worked flawlessly, repeatedly.

Hm, ok.

This is what changed between 1.9.11 and 1.9.12: e98ec3e...562b0d5 (be sure to switch to the Files changed tab). This shows all changes, as I don't believe Github can filter for specific directories. The changes to motoman_driver are near the top of the page though.

(in a local clone: git diff e98ec3e1..562b0d59 -- /path/to/motoman_driver only shows changes to motoman_driver)

Is there any further information I can provide to help debug v1.9.12?

There aren't that many changes in that diff, but most of them do touch MotionServer.c. It'd be easy to immediately point to #542, but there were other changes as well.

If you'd have the time and be willing to debug, it would be interesting to figure out what makes those changes not work on (your) DX100. I believe you mentioned you have the SDK, so building binaries should not be a problem.

Is it useful if I contribute back my SIA50D packages that were tarballed above?

The main change I believe you made was to change the joint names in the xacro:macro, correct? If so, that would perhaps be a nice change to PR.

We're a bit hesitant to host MoveIt configuration packages here as they tend to be application specific (or in other words: for any real application you'd need a custom(ised) MoveIt configuration) and MoveIt (upstream) has a tendency to change, causing MoveIt configuration packages to go stale. We'd have to keep track of that, increasing our maintenance costs.

---

Edit: as the commit comment specifically calls out the DX100, perhaps 9e3babc might be one to check.

@EricMarcil @ted-miller: would you perhaps have any hunches where to look?

[ted-miller]

@EricMarcil @ted-miller: would you perhaps have any hunches where to look?

Nope. But given that user107298 provided his motion script, I should be able to try and reproduce. I need to setup ROS1 and reconfigure my robots first.

[ted-miller]

So, I started to try to reproduce this and found that my DX100 encoder cable is broken.

I'm hesitant to try this on a YRC1000 because the motion API is different. I also think that feedback data might be different. I don't want to add more variables into troubleshooting.

I'm going to try fixing my DX. If I can't get that working, then I'll switch to YRC and try to reproduce

[ted-miller]

Encoder cable is fixed and I got the noetic docker image working. I'll try next week to reproduce the issue and troubleshoot.

[ted-miller]

The issue appears to be DX100-specific. I was able to reproduce immediately and repeatedly on DX100, but could not reproduce on the YRC1000.

Looking through the changes, the only significant DX100-specific difference (wrt to motion control) is the motion API itself.

Wireshark does indicate that in motion is true. There is no telnet output when the error occurs.

Looking at the pendant, the axes are fluctuating a few pulse counts each when the robot is "frozen". But if I run a job that just sits on WAIT, I don't get the flux. So, this leads me to believe that MotoROS is the cause of the flux. (Though I'm not sure why multiple axes are fluctuating when MR is only commanding one axis at a time. Perhaps the older model drives behave differently.)

My best guess is that DX100 motion API does some filtering on the commanded increments such that the robot never really reaches the expected destination. Or its getting into some weird oscillation of overshooting the target. Before #542, a couple missing pulse counts would have been ignored.

Given that DX100 is a legacy controller, I don't know that it's worth the effort of pursuing this further. I'm leaning toward submitting a PR that doesn't incorporate #542 for the DX100.