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

Question

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

dean4ta opened this issue 2 years ago · comments

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

Nicholas Ardavin commented 2 years ago

#488

G.A. vd. Hoorn · Answer 1 · Wed Feb 02 2022 19:26:31 GMT+0800 (China Standard Time)

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.

Dean Fortier · Answer 2 · Thu Feb 03 2022 02:50:57 GMT+0800 (China Standard Time)

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

G.A. vd. Hoorn · Answer 3 · Thu Feb 03 2022 16:55:04 GMT+0800 (China Standard Time)

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

Dean Fortier · Answer 4 · Thu Feb 03 2022 17:50:58 GMT+0800 (China Standard Time)

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

<launch>
    <!-- motoman driver specific -->
    <arg name="robot_ip" default="10.191.0.40" />
    <arg name="robot_namespace" default="yaskawa" />
    <arg name="jt_controller_ns" default="yaskawa" />
    <arg name="trajectory_action_name" default="follow_joint_trajectory" />
    <arg name="using_robot" default="true" />
    <!-- motoman adapter specific -->
    <arg name="read_single_io_srv_name" default="read_single_io"/>
    <arg name="gripper_state_addr" default="10"/>
    <arg name="service_call_rate" default="10"/>
    <arg name="write_single_io_srv_name" default="write_single_io"/>
    <arg name="close_gripper_address" default="10010"/>
    <arg name="open_gripper_address" default="10011"/>
    <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" />

    <node name="joint_state_filter" pkg="yaskawa_robot_hw_interface" type="joint_state_filter" />

    <remap from="/joint_states" to="/joint_states_raw"/>
    <remap from="/joint_trajectory_action/cancel" to="/$(arg jt_controller_ns)/$(arg trajectory_action_name)/cancel"/>
    <remap from="/joint_trajectory_action/feedback" to="/$(arg jt_controller_ns)/$(arg trajectory_action_name)/feedback"/>
    <remap from="/joint_trajectory_action/goal" to="/$(arg jt_controller_ns)/$(arg trajectory_action_name)/goal"/>
    <remap from="/joint_trajectory_action/result" to="/$(arg jt_controller_ns)/$(arg trajectory_action_name)/result"/>
    <remap from="/joint_trajectory_action/status" to="/$(arg jt_controller_ns)/$(arg trajectory_action_name)/status"/>
    <include if="$(arg using_robot)" file="$(find motoman_driver)/launch/robot_multigroup_interface_streaming_yrc1000.launch">
        <arg name="robot_ip" value="$(arg robot_ip)" />
    </include>

    <include file="$(find yaskawa_robot_hw_interface)/launch/motoman_adapter.launch">
        <arg name="read_single_io_srv_name" value="$(arg read_single_io_srv_name)" />
        <arg name="gripper_state_addr" value="$(arg gripper_state_addr)" />
        <arg name="service_call_rate" value="$(arg service_call_rate)" />
        <arg name="write_single_io_srv_name" value="$(arg write_single_io_srv_name)" />
        <arg name="close_gripper_address" value="$(arg close_gripper_address)" />
        <arg name="open_gripper_address" value="$(arg open_gripper_address)" />
    </include>
</launch>

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

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

#!/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

G.A. vd. Hoorn · Answer 5 · Mon Feb 07 2022 21:18:27 GMT+0800 (China Standard Time)

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 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:

include a screenshot of rqt_graph. I'm wondering whether there are incorrect connections due to the remaps you've introduced
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.
show the contents of yaskawa_robot_hw_interface/config/gp7_motion_interface.yaml
explain the need for joint_state_filter
explain how your Yaskawa controller is setup wrt motion groups
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)
try again without remapping anything, and using a simple action client

Ted Miller · Answer 6 · Mon Feb 07 2022 21:28:20 GMT+0800 (China Standard Time)

# 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 · Answer 7 · Mon Feb 07 2022 21:31:15 GMT+0800 (China Standard Time)

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.

G.A. vd. Hoorn · Answer 8 · Mon Feb 07 2022 21:33:52 GMT+0800 (China Standard Time)

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

Nicholas Ardavin · Answer 9 · Tue Feb 08 2022 09:07:33 GMT+0800 (China Standard Time)

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

<launch>
    <arg name="robot_ip" default="10.191.0.42" />
    <rosparam command="load" file="$(find yaskawa_robot_hw_interface)/config/gp7_motion_interface.yaml" />

    <include file="$(find motoman_driver)/launch/robot_multigroup_interface_streaming_yrc1000.launch" >
        <arg name="robot_ip" value="$(arg robot_ip)" />
    </include>

    <node pkg="yaskawa_robot_hw_interface" type="joint_traj_client" name="joint_traj_client" output="screen" />

</launch>

gp7_motion_interface.yaml - click to expand

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

#!/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.

G.A. vd. Hoorn · Answer 10 · Tue Feb 08 2022 15:54:16 GMT+0800 (China Standard Time)

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?

G.A. vd. Hoorn · Answer 11 · Tue Feb 08 2022 15:58:08 GMT+0800 (China Standard Time)

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.

Richard Srisamang · Answer 12 · Tue Feb 08 2022 16:29:22 GMT+0800 (China Standard Time)

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?

G.A. vd. Hoorn · Answer 13 · Wed Feb 09 2022 00:14:46 GMT+0800 (China Standard Time)

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 #450 (comment).

G.A. vd. Hoorn · Answer 14 · Fri Feb 11 2022 15:31:51 GMT+0800 (China Standard Time)

Friendly ping

G.A. vd. Hoorn · Answer 15 · Tue Feb 15 2022 00:07:30 GMT+0800 (China Standard Time)

Closing due to inactivity.

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

Richard Srisamang · Answer 16 · Tue Feb 15 2022 00:12:57 GMT+0800 (China Standard Time)

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.

G.A. vd. Hoorn · Answer 17 · Tue Feb 22 2022 18:40:05 GMT+0800 (China Standard Time)

Just checking you've resolved the issue.

Richard Srisamang · Answer 18 · Wed Feb 23 2022 15:50:03 GMT+0800 (China Standard Time)

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

G.A. vd. Hoorn · Answer 19 · Wed Feb 23 2022 16:55:38 GMT+0800 (China Standard Time)

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.

G.A. vd. Hoorn · Answer 20 · Thu Mar 03 2022 21:16:08 GMT+0800 (China Standard Time)

Friendly ping?

G.A. vd. Hoorn · Answer 21 · Wed Mar 09 2022 20:36:02 GMT+0800 (China Standard Time)

And another ping.

G.A. vd. Hoorn · Answer 22 · Mon Mar 14 2022 22:51:09 GMT+0800 (China Standard Time)

And another friendly ping.

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

G.A. vd. Hoorn · Answer 23 · Tue Mar 22 2022 01:19:44 GMT+0800 (China Standard Time)

And another ping.

Jakub Marsik · Answer 24 · Tue Mar 22 2022 14:26:33 GMT+0800 (China Standard Time)

I would be also interested in this info, could you elaborate @rsrisamang-miso?
See also my comment #259 (comment)

G.A. vd. Hoorn · Answer 25 · Thu Mar 24 2022 03:55:46 GMT+0800 (China Standard Time)

You may be interested in the combination of kinetic-devel...gavanderhoorn:coalesced_joint_feedback_ex_relay and 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.

Nicholas Ardavin · Answer 26 · Thu Mar 24 2022 06:26:00 GMT+0800 (China Standard Time)

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.

G.A. vd. Hoorn · Answer 27 · Thu Mar 24 2022 16:08:52 GMT+0800 (China Standard Time)

A pointer to a branch should be enough.

G.A. vd. Hoorn · Answer 28 · Tue Mar 29 2022 16:42:18 GMT+0800 (China Standard Time)

Friendly ping @nardavin