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:

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.