Error "Validation failed: Trajectory doesn't start at current position." when executing high speed back to back moves #324
Comments
|
I ran into a similar issue on a project where I was using a rail-mounted ES200RN robot. We weren't doing any sort of high speed operation, but one of the axes (S, if I remember correctly) would consistently be outside the 30 pulse-count threshold. We ended up writing a function in our application that compared the current state of the robot to the first state of the planned trajectory and replacing it with the current state if it was within our own defined threshold. This solution was really simple and worked well for us. I'm not sure if adding this functionality to the driver would be advisable due to the safety/accuracy concerns described in #312, but it should be easy enough to implement on your application side |
|
@marip8 do you have a branch of code with your fix implemented? How did you go about inflating the time_from_start and velocities of the initial start_point? This functionality seems like an easy enough feature to add a configuration variable for end users to enable/disable if there are safety concerns. |
|
All our trajectories started from a zero-velocity state. I'm not entirely sure what you mean by inflating the
True. There would probably have to be an upper limit on the allowable tolerance specified by the user above which replacing the start state of the trajectory with the current state would not be allowed. We could probably come up with a reasonable number from the testing that's been done around this issue. We would also need to be transparent to the user about this functionality. Maybe log warnings that the start state is being replaced would be enough, but it's something to think about |
|
Sorry @marip8 I was thinking you were appending the current_state to the trajectory not just replacing it. I tried this too and it fixes some of the execution requests but still fails frequently. |
|
We've got a MA2010 manipulator for welding applications with a DX200 controller. I am using the same method as @marip8 describes. This works excellent at least for trajectories where the welding torch is not activated. We are using a Fronius Robacta torch for welding and occasionally, the force of the welding wire pushing down onto the substrate plate just before arc ignition can cause enough joint displacement in some of the more distal joints (B and T) for the error to be thrown--I can actually observe the displacement of the torch. I don't know your particular setup @MarqRazz but maybe in your case, your gripping tool could have the same effect. I've also noticed that certain torch orientations add to this problem because of varying torques imposed on each joint by the advancing wire. Note that in my use case, this displacement can occur just after the current joint angles are obtained because the arc ignition and motion start need to be very synchronous. For now, in these cases where the torch causes displacement I simply detect that the manipulator has not started moving when it should have ( I would agree that this kind of feature of updating the initial trajectory point if the given one deviates too much would add to the robustness of the driver and would also agree with @marip8 that there must be an upper tolerance limit at which updating the start point is not allowed to avoid unintended behavior.
I think it should go into |
|
I did some measurement tests to see how much the joint angles settle after moveit has returned from executing the trajectory and here are the results. I have made adjustments to the driver so that when the first point in the trajectory does not pass the is_valid test I then check to see if it is withing a safety tolerance. After that point when the driver is converting the joint trajectory msg to a robot msg I replace the first point with it's current known position Here is a successful replacement: The modified driver has made my script run much better at executing but I still get frequent failures: on the MotoROS side: Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011) Looking through the code more the I also have my entire robot setup running in Gazebo and have been doing some tests. I have found that if I reduce my My question is can I easily increase the joint_state update rate on the motoman_driver side? I have done some digging but cant find the location to update the polling rate. Any other suggestions on things I can do to fix this error @EricMarcil or @ted-miller? Is there any chance I can get Yaskawa to also start investigating a fix? |
|
I found the update rate, its in the MotoPlus application in StateServer.h. I have a request out to Yaskawa for access to the MotoPlus SDK. I'll let you know when I get a chance to run the joint states at 50 Hz. |
|
To update the joint-state rate on the MotoROS side, you just need to modify the define in StateServer.h. But, I think that you should be looking at the "in_motion" flag to determine when the robot has reached the destination of the previous trajectory. If the robot is still decelerating when you fetch the current position, then it will have moved by the time you issue the next trajectory. This will lead to the invalid-trajectory error again. Alternatively, it sounds like your application would be good for the point streaming interface. (#215) I believe this interface allows you to continuously stream in points without starting a "new trajectory". The new points would just appended to the end of what is currently executing. The joint tolerance check is only performed at the first point of a new trajectory. Please note there is a 4 hour limit for a single trajectory. After that, you must start a new trajectory, or you will get an error. |
If you look in joint_trajectory_action.cpp line 240 you will see that this is where the action server is checking to see if the arm has made it within the required goal constrains. Once the arm is considered stopped it sets the in_motion flag false and returns from the action. This is when my MoveIt call of move_group.execute(plan, wait=True) returns and I go on planning the next move. So in the end I am waiting until the in_motion flag is false.
This sounds like a hack and would be a lot of extra work on my application to do something I think the robot driver should be capable of. If you look above at my test where I poll the joint states after executing a move you can see that the arm is still moving for more than a half second after it has said it finished. Do you think that all applications should have to wait that long before they send another move to the arm? I would like my code to work on any industrial arm and would prefer to not have to implement the streaming interface on all of them for this work around. |
|
@MarqRazz wrote:
I'm not sure I understand this: The action server uses it, but it certainly does it set it. |
|
There are basically 3 main tasks in the MotoRos driver:
Like any control system, the feedback position tries to catch-up to the command position but will lag behind because of inertia, filtering, communication delays. So even if you have completed sending your command, the robot may still be in motion. In theory, if you keep track of where you've finish you last position, you could just continue on from it. The robot will eventually reach that position and continue on afterwards. The problem is when something interrupts the motion, like a hold, e-stop, alarm... and the robot never reached it last commanded position. How can we ensure that we are starting from robot real position and not the last final destination that was never reached. Since we don't know how people are implemented their code on the ROS side, we were extra careful to keep the system as safe as possible and added the check to enforce that new trajectory always starts from the robot stopped feedback position. The drawback is that delay to make sure that the robot feedback comes to a complete stop. We might be able to improve on that by storing the last commanded position and allow a new trajectory to restart from that position, but we would need to make sure that this position is invalidate in any situation where the motion is interrupted. We also need to make sure that there is not accumulation of small errors anywhere which might accumulate over time and cause the robot to gradually drift over time. |
Sorry I worded that incorrectly. It checks the in_motion status and when it is false it returns from the action which means that I am waiting for the in_motion flag to be false before planning another move. As you can see below it does not write the value it just reads it.
This is why I have added a second check to see if the robot is close enough (0.02 radians) to its current "known" position (this should cover both of the cases you listed). The issue I am running into is that the motoman_driver does not have current enough information to pass the lower level check on the controller. Do you guys think this approach is acceptable and safe? In the end I think that this check belongs on the MotoROS side as its the only one with the true state of the robot. I am saying as an engineer in the industrial sector that the current level of performance will not meet my customers need for cycle time. As you can see below MoveIt is more than capable of creating motion plans in the same order of time that the controller is running at and we will always have to deal with delayed joint_states. |
|
I've been testing some changes to the MotoRos driver:
|
|
The code for the change mention in 1) above that allow a trajectory to start from the end of the previous one is available on my personal branch AppendTrajectory |
|
So I have been working with Eric to test new firmware and we have the arm behaving much better but I am still running into issues with the ROS side driver when calling stop from MoveIt and then making a plan immediately afterwards to somewhere else. Below is a printout from my terminal where I have modified the jointTrajectoryCB to print out what is happening when it receives a message. [1589925191.609793889] ros.industrial_robot_client.joint_trajectory_action: Received new goal [1589925191.609963927] ros.industrial_robot_client.joint_trajectory_action: Publishing trajectory [1589925191.610378818] ros.motoman_driver: Receiving joint trajectory message of length 11 [1589925191.610445939] ros.motoman_driver: Current state is: 0 [1589925191.610478691] ros.motoman_driver: converting trajectory_to_msgs [1589925191.611467332] ros.motoman_driver: Sending to robot [1589925191.628056196] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.647084399] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.666138282] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.684926813] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.704091417] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.723076208] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.741829370] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925191.760686421] ros.industrial_robot_client.joint_trajectory_action: Waiting to check for goal completion until halfway through trajectory [1589925192.490669244] ros.moveit_simple_controller_manager.ActionBasedController: Cancelling execution for arm_controller [1589925192.490797334] ros.moveit_ros_planning.trajectory_execution_manager: Stopped trajectory execution. [1589925192.491157204] ros.industrial_robot_client.joint_trajectory_action: Received action cancel request [1589925192.491545641] ros.moveit_simple_controller_manager.SimpleControllerManager: Controller arm_controller successfully finished [1589925192.491605338] ros.motoman_driver: Receiving joint trajectory message of length 0 [1589925192.491688121] ros.motoman_driver: Current state is: 0 [1589925192.491710002] ros.moveit_ros_planning.trajectory_execution_manager: Completed trajectory execution with status PREEMPTED ... [1589925192.491723883] ros.motoman_driver: Empty trajectory received while in IDLE state, nothing is done [1589925192.492048552] ros.pick_place_action.pick_place_robot: Finished executing seek move [1589925192.521729483] ros.industrial_robot_client.joint_trajectory_action: Received new goal [1589925192.521918117] ros.industrial_robot_client.joint_trajectory_action: Publishing trajectory [1589925192.522409668] ros.motoman_driver: Receiving joint trajectory message of length 10 [1589925192.522490935] ros.motoman_driver: Current state is: 0 [1589925192.522537263] ros.motoman_driver: converting trajectory_to_msgs [1589925192.523551861] ros.motoman_driver: Sending to robot [1589925192.530826511] ros.motoman_driver: Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011) If you look at jointTrajectoryCB() you will see that the function checks if the TransferState != IDLE and if that is the case it should send the robot the trajectoryStop() command but if it is equal to IDLE then it does nothing and just prints a message. If you look at my printout from above you can see that as the arm is executing my first move when MoveIt cancels the trajectory and the state == IDLE so the robot is not sent the trajectoryStop() command. Here is an issue on the industrial_core side with the same problem. Looks like the Fanuc robot is also having an issue (that will probably be my next robot arm). I changed this behavior so the trajectoryStop() command is sent but I have found another issue. Below is a Wireshark capture of the above event. Inspecting the industrial_robot_client/joint_trajectory_action cancelCB you can see that the method publishes an empty trajectory message and then returns before the robot has actually stopped. @gavanderhoorn do you think that the trajectory action server should be required to wait until the robot stops before it returns from a stop command? I can open an issued over there but wanted to see what you think the best course of action should be here. |
|
Is the IDLE state corresponding to my In_Motion state? So toward the end of the motion, there is probably a 300 to 400 ms where the robot feedback position is still catching up to the command position, but the In_Motion will be false. But then you don't actually need the stop motion at that point, since the command motion is complete. At that time, the reported current position should be the command position which has reached the end of the trajectory and you should be able to replan from there... So I guess that is not what is happening. But still, I thought I should share the reflection and make everyone aware that this is a modified version where the current position and the in_motion logic is slightly different. Maybe someone can see something we are missing? |
|
The code for the change mention in 2) above that allow a new trajectory to be planned and started as soon as the previous trajectory has been completed transferred to the controller or aborted (MotionStop cmd). This is available on my personal branch Command-position |
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
@EricMarcil the AppendTrajectory changes look very promising for our current issues with very fast back-to-back trajectories. Could you please rebase that branch to MotoROS 1.9.11? There have been some safety/error-handling commits in between and I'm wary of doing that rebase myself without lots of care and testing. |
|
@cjue I'm working on a tight deadline for the end of Nov. I probably won't have time to work on this until early December. |
|
We noticed another possible cause for mismatch between command and feedback position: Conditionally replacing the first trajectory point as described by @marip8 above helped mitigating this problem. |
Summary
I get following error when I run this python test_script.zip with the robot at max speed.
Validation failed: Trajectory doesn't start at current position.Every once in a while I also get the error:
Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)Environment
OS : Ubuntu 18.04
ROS Version : Melodic
Robot : MH5L (I have a forked repo here if you want to test on this robot)
Controller : FS100
Robot configuration details
Because my joint accelerations don't match the robots capabilities exactly I have increased my allowed_execution_duration_scaling to 1.4 (I want to to always go as fast as possible)
<param name="trajectory_execution/allowed_execution_duration_scaling" value="1.4"/>Below is a link to a copy of my joint_limits.yaml file along with a python script I have been hacking together to run the robot through a benchmark test to compare their planning/execution speed.
test_script.zip
Details
I have added a line into utils.cpp in the industrial_core lib to print out which joint is causing the problem and how much error I am seeing.
ROS_ERROR_STREAM(__FUNCTION__ << " Joint " << keys[i] << " has lhs position of: " << lhs.at(keys[i]) << " and rhs has position of: " << rhs.at(keys[i]) << " Total error: " << fabs(lhs.at(keys[i]) - rhs.at(keys[i])));In my python script if I set the dwell_time_between_moves (line 34) to 0.5 second the robot performs as expected and I do not get the "Validation falied: ..." error. Here is a printout of each joints position error after executing move_to_joint_value_target() to give you an idea of how well the controller is doing (it looks like is it doing really good!).
['joint_s', 'joint_l', 'joint_u', 'joint_r', 'joint_b', 'joint_t']Error is in radians[-0.00007, -0.00006, -0.00007, 0.00018, -0.00007, 0.00035][-0.00004, 0.00002, -0.00004, 0.00018, -0.00013, 0.00044][-0.00011, -0.00006, -0.00011, 0.00024, -0.00013, 0.00047][0.00004, 0.00005, -0.00011, -0.00008, 0.00006, -0.00049][0.00002, -0.00005, -0.00007, 0.00012, 0.00011, 0.00052]If I lower the dwell_time_between_moves to anything below 0.3 I start getting "Validation falied: ..." errors and below is the printout I get from utils.cpp:
isWithinRange Joint joint_b has lhs position of: -0.952353 and rhs has position of: -0.952468 Total error: 0.000115037isWithinRange Joint joint_t has lhs position of: 0.708883 and rhs has position of: 0.709037 Total error: 0.000153422isWithinRange Joint joint_r has lhs position of: -0.000997088 and rhs has position of: -0.00107379 Total error: 7.6699e-05isWithinRange Joint joint_s has lhs position of: -0.708659 and rhs has position of: -0.70873 Total error: 7.01547e-05isWithinRange Joint joint_r has lhs position of: -0.000824515 and rhs has position of: -0.000920388 Total error: 9.58738e-05isWithinRange Joint joint_u has lhs position of: -0.699196 and rhs has position of: -0.699141 Total error: 5.51939e-05As noted above also sometimes I also get the error
Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)and do not receive a printout from utils.cpp which I assume means the validation passed on the ROS side driver.
Lastly I am also getting the error:
Validation failed: Missing velocity data for trajectory pt 0when running with dwell_time_between_moves set below 0.3 but that is a MoveIt error and I am working with that source code to fix the problem.
Use Case
My use case that I am targeting is high speed pick and place. The program I use plans the entire pick and place of the item up front to ensure success and then breaks the execution up into three sections:
This methodology builds each section such that all of the trajectories are continuous but is now assuming that the robot controller has the capability to handle any stopping error or joint_state sync error that happens between moves. I am considering adding functionality to the driver if needed that will append start points if the start_pose does not match the current_pose but is within a specified amount of error. One question I have is do you think that this is something other users would like or should I keep this in my own fork? My second question is do you have any suggestions of where I should put this functionality (inside motoman_driver or MotoROS)? Do you have other suggestions on how I can get around this error?
The text was updated successfully, but these errors were encountered: