Joint Trajectory Action status is always ends in "LOST" after executing command #450
Comments
|
Could you please describe your setup more? Is this a single- or multi-group robot, how are you starting things, which And if possible, a minimal script which reproduces what you're reporting. References to code I cannot access complicate diagnosing what's going on. |
|
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: edit: |
|
|
Yaskawa Action Server LaunchOkay 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.py` - click to expandimport 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 Joint Trajectory ClientThe 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 ResultsI 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 Test 1 data: output from motoman_gp7_test node with no collisionTest 2 data: output from motoman_gp7_test node with induced collisionThere is also a bag file for each of the tests in the included zip: ResultsOne 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 click to expandand a quick rosbag info on bag using: click to expandI hope this isn't too much information. Thanks |
|
thanks for all the information.
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
I'm a little confused as to why this is necessary.
Could you summarise why you found the extra script necessary? What about the
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. @ted-miller @EricMarcil: is it possible for MotoROS to do what the comment states (ie: have a joint in two groups)?
this comment also confuses me, but perhaps you'll clear that up when you explain the need for the custom
you don't need both of these. Either you have a multi-group system and use the
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 actionsplease:
|
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 |
|
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. |
|
Yes, that's how our YRC1000 is also configured (GP25 on a TSL-1000). |
|
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 expandtopic_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: And after we send a 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. |
|
Thanks for the update, but I'd still like to see:
this should not be necessary, provided you subscribe to the correct topic. On my personal setup, I use a single The Does that not work for you? |
|
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. |
|
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? |
|
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). |
gavanderhoorn
added
the
more-info-needed
|
Friendly ping |
|
Closing due to inactivity. I'd still be interested in diagnosing this, but without more information, that's going to be difficult. |
|
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. |
|
Just checking you've resolved the issue. |
|
Yes, we've applied similar changes to the issue you mentioned in #259. We were able to resolve the lost goals. Thank you. |
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 |
|
Friendly ping? |
|
And another ping. |
|
And another friendly ping. @dean4ta @nardavin @rsrisamang-miso: could you please respond to my request? |
|
And another ping. |
|
I would be also interested in this info, could you elaborate @rsrisamang-miso? |
|
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. |
|
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. |
|
A pointer to a branch should be enough. |
|
Friendly ping @nardavin |
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:
Terminal Output
What we are seeing is a log of the state when the robot arm achieves its trajectory.
it is worth noting that
Trajectory state: 1means status is "ACTIVE" andTrajectory state: 9means status is "LOST". You can also see there was adone_cbfor the action client that printed the status and resultExpected 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
The text was updated successfully, but these errors were encountered: