Joint Trajectory Action status is always ends in "LOST" after executing command

[dean4ta]

Context

When sending trajectories to a Yaskawa GP7 using an action server hosted by the joint_trajectory_action.cpp, the status goes from ACTIVE to LOST (see GoalStatus msg) regardless of the trajectory executed by the arm is a "success" (gets to its desired final position) or is interrupted by an e-stop or collision.

Process

We bring up an Action Client to the Action Server provided by Yaskawa and send a goal that executes as expected. We read the state output and it changes from ACTIVE to LOST.

partial code from the client side:

def submit_trajectory_goal(gp7_client, goal, duration_sec):
    gp7_client.send_goal(goal)
    gp7_client.wait_for_result()
    # We were logging the state and callback status here with gp7_client.get_state()
    rospy.loginfo(gp7_client.get_result())

def main():
    # create client and make sure it's available
    client = actionlib.SimpleActionClient('joint_trajectory_action',
                                           FollowJointTrajectoryAction, done)
    client.wait_for_server()

    # Create FollowJointTrajectoryGoal object
    new_joint_goal = FollowJointTrajectoryGoal()
    new_joint_goal.trajectory = JointTrajectory()
    start_time_sec = 0.0
    time_from_start_sec = 0.0
    duration_sec = 10

    # Get Current position of joints
    [gp7_initial_joint_states, joint_names_r1s1] = get_gp7_current_joint_states()
    # Initialize goal joint_names
    new_joint_goal.trajectory.joint_names = joint_names_r1s1
    # Initialize velocity
    qdot = [0.0] * len(joint_names_r1s1)
    # Initialize joint_goal to current position.
    new_joint_goal.trajectory.points.append(JointTrajectoryPoint(positions=gp7_initial_joint_states,
        velocities=qdot, time_from_start=rospy.Duration(start_time_sec)))

    # Make a copy of gp7_initial_joint_states to pass into create_joint_goal and to preserve
    # initial joint states
    new_joint_states = gp7_initial_joint_states[:]
    # Elapsed time for each joint move

    time_from_start_sec += duration_sec
    create_joint_goal(new_joint_goal, 'joint_1_s', new_joint_states, 60, time_from_start_sec)
    time_from_start_sec += duration_sec
    create_joint_home_goal(new_joint_goal, gp7_initial_joint_states, time_from_start_sec)
    rospy.loginfo("Submit joint goal : %s", new_joint_goal)
    submit_trajectory_goal(client, new_joint_goal, time_from_start_sec)

Terminal Output

[INFO] [1643656915.186073 /trajectory_manager] [/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/core.py:__call__:177]: $
rajectory state: 1
[INFO] [1643656915.445529 /trajectory_manager] [/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/core.py:__call__:177]: $
rajectory state: 1
[INFO] [1643656915.683436 /trajectory_manager] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/src$
yaskawa_robot_hw_interface/trajectory_manager.py:doneCb:121]: doneCb_status: 9
[INFO] [1643656915.683649 /trajectory_manager] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/src$
yaskawa_robot_hw_interface/trajectory_manager.py:doneCb:122]: doneCb_result: None
[INFO] [1643656915.705677 /trajectory_manager] [/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/core.py:__call__:177]: T
rajectory state: 9
[INFO] [1643656915.965501 /trajectory_manager] [/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/core.py:__call__:177]: T
rajectory state: 9

What we are seeing is a log of the state when the robot arm achieves its trajectory.

it is worth noting that Trajectory state: 1 means status is "ACTIVE" and Trajectory state: 9 means status is "LOST". You can also see there was a done_cb for the action client that printed the status and result

Expected Results

I was expecting the Motoman's ActionServer to output the status "SUCCEEDED" when the trajectory was achieved and "ABORTED" or "PREEMPTED" on collision and e-stop.

Where I am at

I did not get very far, but I traced the launch file robot_interface_streaming_yrc1000.launch to this node implementation joint_trajectory_action.cpp where the Action Server is hosted, but I do not see where the result or status is handled or sent.

Thanks

[gavanderhoorn]

Could you please describe your setup more? Is this a single- or multi-group robot, how are you starting things, which .launch files are you using?

And if possible, a minimal script which reproduces what you're reporting.

References to code I cannot access complicate diagnosing what's going on.

[dean4ta]

Hi thanks for the support, I will get you more concise setup information and how we are launching things later today. In the meantime, I was having difficulty finding where the Action Server actually sends results using methods like: action_server.set_succeeded(result), action_server.set_preempted(), action_server.set_aborted(). Where are these resutls set?

---

edit: I found where the goals are accepted/succeeded. they are using method names different from the python API setSucceeded() and other camelcase examples

[gavanderhoorn]

motoman_driver is not written in Python, so I'm not surprised you couldn't find the methods you mention.

[dean4ta]

Yaskawa Action Server Launch

Okay so I gathered some data after launching the motoman drivers and Trajectory action server with the following:

top-level launch file:

`yaskawa.launch` - click to expand

```xml ```

joint_state_filter node is meant to combine the published joint_states from the yaskawa arm and a slider rail into one message. (This step complicates the answer if we are using a single group or multigroup robot)

`joint_state_filter.py` - click to expand

```python import rospy from sensor_msgs.msg import JointState from message_filters import ( ApproximateTimeSynchronizer, Subscriber, ) class JointStateFilter(object): def __init__(self): self.joint_state_topics = [ "gp7/gp7_yrc1000_r1s1/joint_states", "gp7/gp7_yrc1000_s1/joint_states", ] self.joint_state_pub = rospy.Publisher( "joint_states", JointState, queue_size=10 ) self.TIMESTAMP_TOLERANCE = 0.1 self.setup_subscribers(self.joint_state_callback, self.TIMESTAMP_TOLERANCE, 10) def setup_subscribers(self, callback, slop, queue_size): subscriber_list = [ Subscriber(joint_state_topic, JointState) for joint_state_topic in self.joint_state_topics ] ats = ApproximateTimeSynchronizer( subscriber_list, queue_size=queue_size, slop=slop ) ats.registerCallback(callback) def joint_state_callback(self, r1s1_joint_state, s1_joint_state): joint_state = JointState() joint_state.header.stamp = r1s1_joint_state.header.stamp joint_state.header.frame_id = r1s1_joint_state.header.frame_id joint_state.name = r1s1_joint_state.name joint_state.position = list(r1s1_joint_state.position) joint_state.velocity = r1s1_joint_state.velocity joint_state.effort = r1s1_joint_state.effort joint_state.position[6] = s1_joint_state.position[0] self.joint_state_pub.publish(joint_state) ```

The motoman_adapter.launch file launches one additional node that reads yaskawa alarms and interfaces with some IO.

Joint Trajectory Client

The action client node was launched standalone with rosrun to create a trajectory consisting of 3 point with the first and third points being the starting position of the arm.

`motoman_gp7_test.py` - click to expand

```python #!/usr/bin/env python # A simple example showing the use of an Action client to request execution of # a complete JointTrajectory. # # Note: joint names must match those used in the urdf, or the driver will # refuse to accept the goal. import rospy import actionlib import time import math import sys from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # The group numbers below are define in gp7_motion_interface.yaml file # That file specifies R1+S1 as Group 0, and S1 as Group 1. GROUP_R1S1 = 0 # R1S1 - GP7 arm and rail motor GROUP_S1 = 1 # S1 - station motor (rail motor) MAX_FRAME_COUNT = 1000 JOINT_ANGLE_DEG_0 = 60.0 # Group 0 Target Joint Position JOINT_ANGLE_DEG_1 = 360.0 # Group 1 Target Joint Position # GOAL_START_TIME = 0.0 def get_actionlib_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") return client def get_current_joint_states(group_number): """ Retrieves messages from 'joint_states' topic with the required joint_states names associated with the group number. Will continually request messages for up to MAX_FRAME_COUNT messages before giving up and exiting. Arguments: group_number - Group number associated with the joint_states frame that is being sought after. For the GP-7, there is only Group 0 (R1+S1), and Group 1 (S1). Return: [robot_joint_states, expected_joint_names] - list containing joint_states object and expected_joint_names list """ # Define expected joint_states names if group_number == GROUP_R1S1: expected_joint_names = [ "joint_1_s", "joint_2_l", "joint_3_u", "joint_4_r", "joint_5_b", "joint_6_t", "joint_1_rail", ] elif group_number == GROUP_S1: expected_joint_names = ["joint_1_rail"] # Get first frame robot_joint_states = rospy.wait_for_message("joint_states", JointState) # Continue looking for that frame up to MAX_FRAME_COUNT frame_count = 0 while set(expected_joint_names) != set(robot_joint_states.name): # \ and frame_count < MAX_FRAME_COUNT: # print("robot_joint_states.name = {}".format(robot_joint_states.name)) robot_joint_states = rospy.wait_for_message("joint_states", JointState) frame_count += 1 rospy.loginfo("Joint States = %s", robot_joint_states) # If we couldn't get any frames with the matching joint names, then there's something wrong # Log fatal error and stop the script. if frame_count >= MAX_FRAME_COUNT: rospy.logfatal( "Could not get joint_states frames for Group(%d) to match with required joint_names. ", group_number, ) sys.exit(1) else: return [robot_joint_states, expected_joint_names] def get_gp7_current_joint_states(): """ returns a list containing current joint angles of all 7 joints of the GP7 R1+S1 configuration and a list of containing the valid joint_names of all joints. """ # Get joint states for R1S1 and S1. Note that GROUP_R1S1 doesn't contain info on # S1 joint position. Must grab that info specifically from GROUP_S1. This is an # idiosyncrasy of the ROS driver for the GP7. [gp7_joint_states_r1s1, joint_names_r1s1] = get_current_joint_states(GROUP_R1S1) [gp7_joint_states_s1, joint_names_s1] = get_current_joint_states(GROUP_S1) # Create initial gp7 joint_states list with correct info for all 7 joints gp7_initial_joint_positions = list(gp7_joint_states_r1s1.position) gp7_initial_joint_positions[-1] = gp7_joint_states_s1.position[0] return [gp7_initial_joint_positions, joint_names_r1s1] def create_joint_goal( goal, joint_name, current_joint_states, joint_angle_deg, time_from_start ): """ Returns an updated goal to move one joint the specified amount of degrees from its current position. If goal argument is null, will create a new object and return it. Otherwise, will append a new goal trajectory as specified by the user. Arguments: goal - FollowJointTrajectoryGoal object. Will be updated with new joint goal joint_name - name of joint to move. Must come from the list as returned by the get_current_joint_states method. The list includes the following valid joint names: 'joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t', 'joint_1_rail'. current_joint_states - joint states of the robot prior to executing this goal. This will be updated to the new joint state of the robot when this method completes. joint_angle_deg - number of degrees to move the specified joint. time_from_start - The time that will have elapsed after achieving this trajectory state. Updates goal, current_joint_states """ # Check if joint name is valid if joint_name in set(goal.trajectory.joint_names) == False: rospy.logfatal("Invalid joint name : %s", joint_name) sys.exit(1) # Update gp7_initial_joint_states to new goal trajectory q1 index = 0 for x in goal.trajectory.joint_names: if x == joint_name: break else: index += 1 # Init qdot qdot = [0.0] * len(goal.trajectory.joint_names) # Make a copy of current_joint_states and assign to updated_joint_states updated_joint_states = current_joint_states[:] updated_joint_states[index] += math.radians(joint_angle_deg) # Update current_joint_states current_joint_states[index] += math.radians(joint_angle_deg) # Add trajectory goal goal.trajectory.points.append( JointTrajectoryPoint( positions=updated_joint_states, velocities=qdot, time_from_start=rospy.Duration(time_from_start), ) ) def create_joint_home_goal(goal, joints_home_positions, time_from_start): """ Creates a goal to return joints to their home position, which is defined by the argument joints_home_position. This position can be whatever position the user defines. Typically for the Yaskawa robot on the test stand, the home position would be defined as [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]. Arguments: goal - joints_home_positions - time_from_start - """ # Init qdot qdot = [0.0] * len(goal.trajectory.joint_names) # Add trajectory goal goal.trajectory.points.append( JointTrajectoryPoint( positions=joints_home_positions, velocities=qdot, time_from_start=rospy.Duration(time_from_start), ) ) def submit_trajectory_goal(gp7_client, goal, duration_sec): rospy.loginfo( "\n##### Submitting trajectory goal : #####\n######################\n%s", goal ) gp7_client.send_goal(goal) gp7_client.wait_for_result() rospy.loginfo(gp7_client.get_result()) time.sleep(duration_sec) def safety_e_stop_test(duration_sec): """ """ # create client and make sure it's available client = get_actionlib_client() # Create FollowJointTrajectoryGoal object new_joint_goal = FollowJointTrajectoryGoal() new_joint_goal.trajectory = JointTrajectory() start_time_sec = 0.0 time_from_start_sec = 0.0 # Get Current position of joints [gp7_initial_joint_states, joint_names_r1s1] = get_gp7_current_joint_states() # Initialize goal joint_names new_joint_goal.trajectory.joint_names = joint_names_r1s1 # Initialize velocity qdot = [0.0] * len(joint_names_r1s1) # Initialize joint_goal to current position. # First point in any new goal should be current joint positions at time = 0.0 new_joint_goal.trajectory.points.append( JointTrajectoryPoint( positions=gp7_initial_joint_states, velocities=qdot, time_from_start=rospy.Duration(start_time_sec), ) ) # Make a copy of gp7_initial_joint_states to pass into create_joint_goal and to preserve # initial joint states new_joint_states = gp7_initial_joint_states[:] # Elapsed time for each joint move time_from_start_sec += duration_sec create_joint_goal( new_joint_goal, "joint_1_s", new_joint_states, 60, time_from_start_sec ) time_from_start_sec += duration_sec create_joint_home_goal( new_joint_goal, gp7_initial_joint_states, time_from_start_sec ) rospy.loginfo("Submit joint goal : %s", new_joint_goal) submit_trajectory_goal(client, new_joint_goal, time_from_start_sec) def main(): rospy.init_node("simple_trajectory_action_client") # Check for duration_sec argument if len(sys.argv) < 3: print_usage() sys.exit(-1) safety_e_stop_test(float(sys.argv[2])) if __name__ == "__main__": main() ```

Data and Results

I conducted two tests: one with the robot completing the trajectory and one with the robot experiencing a collision part way. In both cases, the result ends the same with a goal status of 9 or LOST and a result of None.

Test 1 data:

output from motoman_gp7_test node with no collision

``` [INFO] [1643866137.866873 /simple_trajectory_action_client] [./motoman_gp7_test.py:get_actionlib_client:39]: Waiting for driver's action server to become available .. [INFO] [1643866138.119294 /simple_trajectory_action_client] [./motoman_gp7_test.py:get_actionlib_client:41]: Connected to trajectory action server [INFO] [1643866138.172989 /simple_trajectory_action_client] [./motoman_gp7_test.py:safety_e_stop_test:434]: Submit joint goal : trajectory: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' joint_names: [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t, joint_1_rail] points: - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 0 nsecs: 0 - positions: [1.047211612686817, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 10 nsecs: 0 - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 20 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 [INFO] [1643866138.174173 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:272]: ##### Submitting trajectory goal : ##### ###################### trajectory: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' joint_names: [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t, joint_1_rail] points: - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 0 nsecs: 0 - positions: [1.047211612686817, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 10 nsecs: 0 - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 20 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 [INFO] [1643866138.175839 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:274]: sending goal: trajectory: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' joint_names: [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t, joint_1_rail] points: - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 0 nsecs: 0 - positions: [1.047211612686817, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 10 nsecs: 0 - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 20 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 [INFO] [1643866143.515529 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:277]: Traj Goal State: 9 [INFO] [1643866143.515847 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:278]: Traj Goal Result: None ```

Test 2 data:

output from motoman_gp7_test node with induced collision

``` [INFO] [1643866325.313990 /simple_trajectory_action_client] [./motoman_gp7_test.py:get_actionlib_client:39]: Waiting for driver's action server to become available .. [INFO] [1643866325.577075 /simple_trajectory_action_client] [./motoman_gp7_test.py:get_actionlib_client:41]: Connected to trajectory action server [INFO] [1643866325.600355 /simple_trajectory_action_client] [./motoman_gp7_test.py:safety_e_stop_test:434]: Submit joint goal : trajectory: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' joint_names: [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t, joint_1_rail] points: - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 0 nsecs: 0 - positions: [1.047211612686817, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 10 nsecs: 0 - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 20 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 [INFO] [1643866325.601573 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:272]: ##### Submitting trajectory goal : ##### ###################### trajectory: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' joint_names: [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t, joint_1_rail] points: - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 0 nsecs: 0 - positions: [1.047211612686817, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 10 nsecs: 0 - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 20 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 [INFO] [1643866325.603097 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:274]: sending goal: trajectory: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' joint_names: [joint_1_s, joint_2_l, joint_3_u, joint_4_r, joint_5_b, joint_6_t, joint_1_rail] points: - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 0 nsecs: 0 - positions: [1.047211612686817, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 10 nsecs: 0 - positions: [1.406149021931924e-05, 0.18079881370067596, -0.35413479804992676, 0.0, -0.6158693432807922, 0.0, -0.0005113269435241818] velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] accelerations: [] effort: [] time_from_start: secs: 20 nsecs: 0 path_tolerance: [] goal_tolerance: [] goal_time_tolerance: secs: 0 nsecs: 0 [INFO] [1643866330.910571 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:277]: Traj Goal State: 9 [INFO] [1643866330.910954 /simple_trajectory_action_client] [./motoman_gp7_test.py:submit_trajectory_goal:278]: Traj Goal Result: None ```

There is also a bag file for each of the tests in the included zip: relevant_bags.zip

Results

One thing worth noting is that when recording "all the topics" most of the action related topics had 0 messages to them and therefore did not show up in the bag recordings.

here is a list of the topics from a rostopic list:

click to expand

``` /dynamic_feedback_states /feedback_states /gp7/gp7_yrc1000_r1s1/feedback_states /gp7/gp7_yrc1000_r1s1/joint_path_command /gp7/gp7_yrc1000_r1s1/joint_states /gp7/gp7_yrc1000_r1s1/joint_trajectory_action/cancel /gp7/gp7_yrc1000_r1s1/joint_trajectory_action/feedback /gp7/gp7_yrc1000_r1s1/joint_trajectory_action/goal /gp7/gp7_yrc1000_r1s1/joint_trajectory_action/result /gp7/gp7_yrc1000_r1s1/joint_trajectory_action/status /gp7/gp7_yrc1000_s1/feedback_states /gp7/gp7_yrc1000_s1/joint_path_command /gp7/gp7_yrc1000_s1/joint_states /gp7/gp7_yrc1000_s1/joint_trajectory_action/cancel /gp7/gp7_yrc1000_s1/joint_trajectory_action/feedback /gp7/gp7_yrc1000_s1/joint_trajectory_action/goal /gp7/gp7_yrc1000_s1/joint_trajectory_action/result /gp7/gp7_yrc1000_s1/joint_trajectory_action/status /joint_path_command /joint_states /joint_states_raw /motoman_adapter/gripper_command /motoman_adapter/gripper_state /robot_status /rosout /rosout_agg /yaskawa/follow_joint_trajectory/cancel /yaskawa/follow_joint_trajectory/feedback /yaskawa/follow_joint_trajectory/goal /yaskawa/follow_joint_trajectory/result /yaskawa/follow_joint_trajectory/status /yaskawa_alarms ```

and a quick rosbag info on bag using: rosbag info all_topics_motoman_traj_action-collision_2022-02-03-05-32-01.bag results in:

click to expand

``` path: all_topics_motoman_traj_action-collision_2022-02-03-05-32-01.bag version: 2.0 duration: 22.2s start: Feb 02 2022 21:32:02.03 (1643866322.03) end: Feb 02 2022 21:32:24.26 (1643866344.26) size: 2.2 MB messages: 10028 compression: none [3/3 chunks] types: actionlib_msgs/GoalStatusArray [8b2b82f13216d0a8ea88bd3af735e619] control_msgs/FollowJointTrajectoryFeedback [10817c60c2486ef6b33e97dcd87f4474] industrial_msgs/RobotStatus [b733cb45a38101840b75c4c0d6093d19] miso_robot_msgs/AlarmState [77550ad7c5b0ea05af74d4401f486da0] miso_robot_msgs/IOState [8b35663f14aa0799d510e8cbdf5fa8eb] motoman_msgs/DynamicJointTrajectory [81bfbf2d02070fdef3a528bd72b49257] motoman_msgs/DynamicJointTrajectoryFeedback [84d3bbf7103790ff0a8946017b895a1a] rosgraph_msgs/Log [acffd30cd6b6de30f120938c17c593fb] sensor_msgs/JointState [3066dcd76a6cfaef579bd0f34173e9fd] topics: /dynamic_feedback_states 815 msgs : motoman_msgs/DynamicJointTrajectoryFeedback /feedback_states 1628 msgs : control_msgs/FollowJointTrajectoryFeedback /gp7/gp7_yrc1000_r1s1/feedback_states 816 msgs : control_msgs/FollowJointTrajectoryFeedback /gp7/gp7_yrc1000_r1s1/joint_states 814 msgs : sensor_msgs/JointState /gp7/gp7_yrc1000_r1s1/joint_trajectory_action/status 111 msgs : actionlib_msgs/GoalStatusArray /gp7/gp7_yrc1000_s1/feedback_states 815 msgs : control_msgs/FollowJointTrajectoryFeedback /gp7/gp7_yrc1000_s1/joint_states 815 msgs : sensor_msgs/JointState /gp7/gp7_yrc1000_s1/joint_trajectory_action/status 111 msgs : actionlib_msgs/GoalStatusArray /joint_path_command 1 msg : motoman_msgs/DynamicJointTrajectory /joint_states 815 msgs : sensor_msgs/JointState /joint_states_raw 1630 msgs : sensor_msgs/JointState /motoman_adapter/gripper_state 219 msgs : miso_robot_msgs/IOState /robot_status 815 msgs : industrial_msgs/RobotStatus /rosout 27 msgs : rosgraph_msgs/Log (6 connections) /rosout_agg 14 msgs : rosgraph_msgs/Log /yaskawa/follow_joint_trajectory/status 112 msgs : actionlib_msgs/GoalStatusArray /yaskawa_alarms 470 msgs : miso_robot_msgs/AlarmState ```

I hope this isn't too much information. Thanks

[gavanderhoorn]

thanks for all the information.

This step complicates the answer if we are using a single group or multigroup robot

You have a multi-group system. I'm not really concerned with the ROS side. MotoROS and the configuration of the Yaskawa controller determine whether you have a multi-group system or not.

The fact you mention you have an R1 and an S1 already imply you have a multi-group system.

joint_state_filter node is meant to combine the published joint_states from the yaskawa arm and a slider rail into one message.

I'm a little confused as to why this is necessary.

motoman_driver publishes combined JointStates for all defined groups on /joint_states (together with separate publishers for each configured group).

Could you summarise why you found the extra script necessary?

What about the source_list parameter of a regular joint_state_publisher instead?

# The group numbers below are define in gp7_motion_interface.yaml file
# That file specifies R1+S1 as Group 0, and S1 as Group 1.
GROUP_R1S1 = 0  # R1S1 - GP7 arm and rail motor
GROUP_S1 = 1  # S1 - station motor (rail motor)

could you clarify how your controller is configured?

Afaik MotoROS should report the joints in R1 in a message, and the joints of your S1 in a message, and the combination of those. motoman_driver then converts those to separate messages on the joint_states topics for each respective group and a single, combined JointState message for everything together.

@ted-miller @EricMarcil: is it possible for MotoROS to do what the comment states (ie: have a joint in two groups)?

# Get joint states for R1S1 and S1.  Note that GROUP_R1S1 doesn't contain info on
# S1 joint position.  Must grab that info specifically from GROUP_S1.  This is an
# idiosyncrasy of the ROS driver for the GP7.

this comment also confuses me, but perhaps you'll clear that up when you explain the need for the custom joint_state_filter script.

<rosparam command="load" file="$(find yaskawa_robot_hw_interface)/config/joint_names_gp7.yaml" />
<rosparam command="load" file="$(find yaskawa_robot_hw_interface)/config/gp7_motion_interface.yaml" />

you don't need both of these. Either you have a multi-group system and use the topic_list (ie: "motion interface .yaml"), or you don't, and you use controller_joint_names.

yaskawa.launch: lots of remapping

I would recommend to see what the behaviour is if you remove all the remaps, and use a simple action client like shown in https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:actionclient_example.

If you want to namespace "everything yaskawa" in a yaskawa namespace, could you perhaps just include the relevant .launch files with a ns="yaskawa" attribute?

actions

please:

1. include a screenshot of rqt_graph. I'm wondering whether there are incorrect connections due to the remaps you've introduced
2. show the output of roslaunch --screen /path/to/yaskawa.launch (or via the package it is in, doesn't matter). We don't need everything, but make sure to copy-paste all lines originating from the nodes in motoman_driver.
3. show the contents of yaskawa_robot_hw_interface/config/gp7_motion_interface.yaml
4. explain the need for joint_state_filter
5. explain how your Yaskawa controller is setup wrt motion groups
6. provide a whireshark trace of the traffic between motoman_driver and MotoROS (ie: the traffic between the Yaskawa controller and the ROS nodes when executing a trajectory)
7. try again without remapping anything, and using a simple action client

[ted-miller]

# The group numbers below are define in gp7_motion_interface.yaml file
# That file specifies R1+S1 as Group 0, and S1 as Group 1.
GROUP_R1S1 = 0  # R1S1 - GP7 arm and rail motor
GROUP_S1 = 1  # S1 - station motor (rail motor)

@ted-miller @EricMarcil: is it possible for MotoROS to do what the comment states (ie: have a joint in two groups)?

No, not with the ROS interface.

On the teach pendant, there are "group combinations" that can be defined. These can appear to be a single group when programming an Inform job. However, they are distinct separate groups at a lower level.

As you stated above, the MotoROS interface will send three feedback messages to motoman_driver. One for R1, one for S1, and one with both groups.

[ted-miller]

To elaborate a little more, it can be little extra confusing when using a rail mounted to the robot's base. The rail is implicitly linked to R1 without defining a "group combination". This makes it part of the robot's kinematic chain. But, the rail is still an independent group at the lowest level.

[gavanderhoorn]

Yes, that's how our YRC1000 is also configured (GP25 on a TSL-1000).

[nardavin]

Hi, I'm another engineer working on the same project as @dean4ta - we were able to strip down our code to create a more concise example of the error we experienced, incorporating the advice we've received so far. Hopefully this helps explain what we are seeing and what is going wrong.

test_yaskawa.launch - click to expand

```xml ```

gp7_motion_interface.yaml - click to expand

```yaml topic_list: - name: gp7_yrc1000_r1 ns: gp7 group: 0 joints: ['joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t'] - name: gp7_yrc1000_s1 ns: gp7 group: 1 joints: ['joint_1_rail'] ```

joint_traj_client - click to expand

```python #!/usr/bin/env python # TODO: add license # A simple example showing the use of an Action client to request execution of # a complete JointTrajectory. # # Note: joint names must match those used in the urdf, or the driver will # refuse to accept the goal. import rospy import actionlib import time import math import sys from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from industrial_msgs.msg import RobotStatus arm = False rail = False start_pos = [0.0] * 7 def joint_state_cb(msg): # Find the starting position of all joints among the two groups # This is just a hacky way to get the entire robot's initial joints without # having to set up a joint_state_publisher, which eliminates some # potential issues for this simple test global arm global rail global start_pos if not arm and 'joint_1_s' in msg.name: arm = True rospy.loginfo('Got arm message') for i in range(6): start_pos[i] = msg.position[i] elif not rail and 'joint_1_rail' in msg.name: rail = True rospy.loginfo('Got rail message') start_pos[6] = msg.position[0] def main(): global arm global rail global start_pos 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') # setup simple goal goal = FollowJointTrajectoryGoal() goal.trajectory = JointTrajectory() # this should correspond to the joint names of the robot being used (ie: # the joint names as specific in the urdf). The list specified here contains # the default Motoman joints. # NOTE: order matters here goal.trajectory.joint_names = ['joint_1_s', 'joint_2_l', 'joint_3_u', 'joint_4_r', 'joint_5_b', 'joint_6_t', 'joint_1_rail'] # create a subscriber to find initial joints rospy.Subscriber('joint_states', JointState, callback=joint_state_cb, queue_size=10) # wait until initial joint state is populated while not arm or not rail: rospy.sleep(0.1) rospy.loginfo('Got full state') # create the trajectory: it will consist of three points: # # 1. T= 0: current pose # 2. T= 5: T joint rotated -20 degrees # 3. T=10: T joint rotated +20 degrees q0 = start_pos q1 = list(q0) q2 = list(q0) # Move all joints in the middle waypoint for i in range(7): q1[i] -= math.radians(20) # Make the robot come to a complete stop at each trajectory point (ie: # zero target velocity). qdot = [0.0] * len(goal.trajectory.joint_names) # add points goal.trajectory.points.append(JointTrajectoryPoint(positions=q0, velocities=qdot, time_from_start=rospy.Duration( 0.0))) goal.trajectory.points.append(JointTrajectoryPoint(positions=q1, velocities=qdot, time_from_start=rospy.Duration( 10.0))) goal.trajectory.points.append(JointTrajectoryPoint(positions=q2, velocities=qdot, time_from_start=rospy.Duration(20.0))) rospy.loginfo('Waiting for robot to be active') while True: status = rospy.wait_for_message('robot_status', RobotStatus) if status.motion_possible.val == 1: rospy.loginfo('Robot active!') break # 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.') rospy.loginfo('Action server state: ' + str(client.get_state())) rospy.loginfo('Action server result: ' + str(client.get_result())) if __name__ == '__main__': main() ```

Starting the launch file, we see the output:

[INFO] [1644281698.999809 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:52]: Waiting for driver's action server to become available ..
[INFO] [1644281699.222412 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:54]: Connected to trajectory action server
[INFO] [1644281699.274133 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:joint_state_cb:41]: Got rail message
[INFO] [1644281699.274389 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:joint_state_cb:36]: Got arm message
[INFO] [1644281699.324443 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:72]: Got full state
[INFO] [1644281699.324679 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:102]: Waiting for robot to be active

And after we send a rosservice call robot_enable in a separate terminal, we see the following output:

[INFO] [1644281704.761014 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:106]: Robot active!
[INFO] [1644281704.761187 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:110]: Submitting goal ..
[INFO] [1644281704.761398 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:112]: Waiting for completion ..
[INFO] [1644281710.138613 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:114]: Done.
[INFO] [1644281710.139182 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:115]: Action server state: 9
[INFO] [1644281710.139769 /joint_traj_client] [/root/workspace/src/miso-robot-driver-stack/yaskawa_robot_hw_interface/nodes/joint_traj_client:main:116]: Action server result: None

While waiting for completion, all joints do successfully move as we would expect. However, we get the LOST state (9) when querying the action server state, rather than SUCCEEDED (3). We also get the LOST state when terminating the trajectory early, such as through an e-stop.

[gavanderhoorn]

Thanks for the update, but I'd still like to see:

• the wireshark capture
• the rqt_graph screenshot, and
• the output of the motoman_driver nodes when you start everything with roslaunch --screen ...

def joint_state_cb(msg):
    # Find the starting position of all joints among the two groups
    # This is just a hacky way to get the entire robot's initial joints without 
    # having to set up a joint_state_publisher, which eliminates some 
    # potential issues for this simple test
    global arm
    global rail
    global start_pos

    if not arm and 'joint_1_s' in msg.name:
        arm = True
        rospy.loginfo('Got arm message')
        for i in range(6):
            start_pos[i] = msg.position[i]
    elif not rail and 'joint_1_rail' in msg.name:
        rail = True
        rospy.loginfo('Got rail message')
        start_pos[6] = msg.position[0]

this should not be necessary, provided you subscribe to the correct topic.

On my personal setup, I use a single rospy.wait_for_message('joint_states', JointState) which gets me the full JointState, including the track.

The /joint_states topic should publish messages containing the full JointState. The /gp7/gp7_yrc1000_r1/joint_states topic should carry messages containing just the JointStates of the gp7_yrc1000_r1 group, and the /gp7/gp7_yrc1000_s1/joint_states should have them for just the gp7_yrc1000_s1 group.

Does that not work for you?

[gavanderhoorn]

Seeing as you have a multi-group system, it's likely you're being bitten by what #259 attemps to fix. It's not complete (hence why it hasn't been merged), but there's a good chance it'll resolve the lost goals.

[rsrisamang-miso]

Hi @gavanderhoorn , I'm not sure why, but we are not seeing the /joint_states message containing all joint states in one frame. Would that data be originating from the MotoROS Application or is that something the motoman_driver will generate and publish to the /joint_states topic?

[gavanderhoorn]

Please provide me with at least the wireshark trace, so we can rule out a couple of things.

I'd also still like the rest of the items I described in https://github.com/ros-industrial/motoman/issues/450#issuecomment-1031460588.

[gavanderhoorn]

Friendly ping

[gavanderhoorn]

Closing due to inactivity.

I'd still be interested in diagnosing this, but without more information, that's going to be difficult.

[rsrisamang-miso]

Hi @gavanderhoorn, sorry for the late response. Sorry for the late response. Will send you info in the next day or two. Thanks for following up.

[gavanderhoorn]

Just checking you've resolved the issue.

[rsrisamang-miso]

Yes, we've applied similar changes to the issue you mentioned in #259. We were able to resolve the lost goals. Thank you.

[gavanderhoorn]

we've applied similar changes

I'd be interested to know exactly which changes. That could help resolve it here as well.

---

Edit: I don't see a fork of motoman on github.com/orgs/MisoRobotics.

[gavanderhoorn]

Friendly ping?

[gavanderhoorn]

And another ping.

[gavanderhoorn]

And another friendly ping.

@dean4ta @nardavin @rsrisamang-miso: could you please respond to my request?

[gavanderhoorn]

And another ping.

[jmarsik]

I would be also interested in this info, could you elaborate @rsrisamang-miso? See also my comment https://github.com/ros-industrial/motoman/pull/259#issuecomment-1074784596

[gavanderhoorn]

You may be interested in the combination of https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:coalesced_joint_feedback_ex_relay and https://github.com/ros-industrial/motoman/compare/kinetic-devel...gavanderhoorn:rebased_two_arms_on_a_rail as well @dean4ta @nardavin @rsrisamang-miso

NOTE: this is not a complete solution. There are additional issues to address. I just want to test on more than my own hw config.

[nardavin]

Hi all, sorry for leaving everyone hanging on this. Our changes have been in a constant state of "mostly working" for the last couple weeks, but we believe that our version of the motoman driver is now stable and correctly supports multigroup motion, at least for our hardware configuration. I'll make a PR in the next couple days once we have final approval on our end.

[gavanderhoorn]

A pointer to a branch should be enough.

[gavanderhoorn]

Friendly ping @nardavin

[nardavin]

https://github.com/ros-industrial/motoman/pull/488