Open harumo11 opened 4 years ago
gavanderhoorn
commented
4 years ago You still appear to be using MoveIt to plan trajectories for you. In essence, you are using it to generate very short plans, very rapidly. You then publish those trajectories to the joint_command topic.
There is a very good chance MoveIt isn't completely up-to-date on the current joint states of the robot when you ask it to plan again.
harumo11
commented
4 years ago Thanks for quick response @gavanderhoorn .
Yes, your understanding is right. I'm sorry I didn't explain program flow. I used MoveIt to solve inverse kinematics.
I added spinOnce() before planning with move_group in pose_follow_fast.cpp at line 143.
But, I'm not sure spinOnce() can modify joint state of move_group.
And then I got same error.
[ERROR] [1578425443.356365813]: Aborting point stream operation. Failed to send point (#10): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)Could you tell me appropriate method to modify joint state of move_group?
harumo11
commented
4 years ago I think up-to-date joint state isn't needed for planning because my program just use plan.trajectory_.joint_trajectory.point.back().positions in order to make target joints.
Do you know when the motoman driver need up-to-date joint state.
EricMarcil
commented
4 years ago The error 3011 basically indicates that you are trying to start a new trajectory but your starting point is different than the robot current position. The internal function used in the MotoRos driver is using small increments to move the robot, so it is essential that your trajectory starting point matches the robot current position. The drawback of this is that if you stop a trajectory and try to restart a new one too quickly, the robot current position hasn't had the time to settle and this introduces a small position error that generate the error 3011.
So you either have to keep the current motion alive when you stop by sending the same position over and over. Or you need to give it a few seconds for the last motion to settle and then calculate the new trajectory from a stable current position. The state server update the position every 25 ms.
gavanderhoorn
commented
4 years ago @EricMarcil: @harumo11 is using #215, which extends the driver with a streaming interface. This allows users to produce JointTrajectoryPoints at a suitable rate, while the driver essentially executes an "infinite" trajectory.
I'm not entirely sure whether the way @harumo11 interfaces with the extended version of the driver is correct.
harumo11
commented
4 years ago Thanks for your great support @gavanderhoorn and @EricMarcil !
As @gavanderhoorn and @EricMarcil said above
The state server update the position every 25 ms.
and
while the driver essentially executes an "infinite" trajectory.
I changed publish rate of joint_command topic from 1Hz to 40Hz and I got same error.
Is this publish rate(40Hz) appropriate?
Before this trial, I made joint_trajectory_publisher.cpp which could send command via joint_command topic. That program just rotates joint_s slowly but the robot moved completely.
I would be glad if this information helps us.
tanimfresh
commented
4 years ago Hello there @harumo11 and @gavanderhoorn ,
Was there any conclusion or update to this? I am not sure if my implementation is faulty or I am running into a new issue.
OS : Ubuntu 16.04 ROS Version : Kinetic Robot : GP8 Controller : YRC1000 micro Motoman Package : #215 merged
I can tell I am entering the point streaming mode, but I don't understand fully how the PR is implemented. From joint_trajectory_streamer.cpp line 184-187 on PR #215 I understand that JointTrajectory msg should not have more than one point. I am able to stream JointTrajectory msg with the current position, but if I update the position at all I receive the following error:
[ERROR] [1587109125.377911108] [/motion_streaming_interface] [ros.motoman_driver]: Aborting point stream operation. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)
Perhaps I don't fully understand, but how am I supposed to jog/stream points without changing the position? Won't each JointTrajectory msg I stream (containing one JointTrajectoryPoint) not start at the current position?
Below is my client code and at the bottom is the output:
harumo11
commented
4 years ago Hello @tanimfresh I yet solve this problem completely. But I found the temporary solution.
Please try to send initial message which has zero velocity command time and again with 40Hz.
I'm glad if this solution helps you.
tanimfresh
commented
4 years ago @harumo11 When you say again, do you mean I should send this point twice? Similar to your joint_trajectory.cpp, I send it once outside of a loop (with time0 and position0), and then I enter into a for loop where I increment the time from start and position (time from start increases by 1/40 seconds and position I increase one joint position by 0.01; time_n and position_n). Do you mean to say once I enter into the loop I should again send the initial point (time0 and position0) once again before getting into time_n and position_n?
MarqRazz
commented
4 years ago I have a MH5L that I have recently received and am running into this same error.
Trajectory start position doesn't match current robot position
After some investigating I have found that the motoman driver holds extremely tight tolerances between start_state and the first point in the trajectory.
If you look on line 62 of joint_trajecotry_streamer.cpp it sets the start_postol to 1e-4
This value is passed into isWithinRange which halfs the value and compares it to the difference between the current position of the robot and the first waypoint.
I modified isWithinRange with the following whenever the difference is above the threshold to see what joint was causing the error and how large it was.
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])));Some runs I would see the error get as high as 0.0002 and only 1 joint was having an issue (the joint would change from run to run)
My first question is why does the motoman driver require the start position to have error less than 0.00005 radians (start_postol/2)? MoveIt only checks if start_state > 0.01 by default. I think we are dealing with rounding errors that cause this to fail.
To get my arm to run well I had to change the start_postol to 1e-3. Do I need to change my goal_constraints within MoveIt?
MarqRazz
commented
4 years ago So my previous clams were not true. This only fixed my problem when commanding the arm through the RVIZ motion planning plugin.
When running the arm through a python test script that I wrote I am seeing more error on the joints...
isWithinRange Joint joint_l has lhs position of: 0.602153 and rhs has position of: 0.600162 Total error: 0.00199032
I have also set move_group.set_goal_position_tolerance(0.001) and the program is running fast enough that I still get stale joint states and this is even after adding move_group.set_start_state_to_current_state() before every plan.
isWithinRange Joint joint_l has lhs position of: 0.602164 and rhs has position of: 0.599472 Total error: 0.0026921
I also have found that after opening up the tolerance in joint_trajectory_streamer.cpp I still sometimes get an error
Aborting Trajectory. Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)
Does the robot driver code also check the start state of the trajectory? If so why do we need to redundantly keep checking the trajectory?
@ted-miller or anyone else from Yaskawa any ideas on how I can get this driver to work reliably?? I will work on cleaning up my test script and will put it up here if you think it will help you test this. My script runs the robot through a simple pick and place motion.
ted-miller
commented
4 years ago Yes, the robot code also has a check for the starting tolerance. The limit on the robot side is 30 pulse counts. Each axis has different pulse/rad ratio. But, for an MH5L, it looks like your worst case tolerance would be 3.45e-4 rad on the L axis.
MarqRazz
commented
4 years ago @ted-miller is there a reason this driver stack checks the start state multiple times (this seems redundant and just adds extra delay)? Who decided on the "30 pulse counts"? Is it really required to have it match that close?
I am considering making a function that appends the current pose of the robot if the tolerances are out of spec but within a specified range. We have successfully implemented this on our own robot because of the race condition of saying the robot is stopped and when the PC has current enough information to plan its next move (it looks like this driver reports in_motion is false when it is withing the specified goalthreshold ). I cant deal with the robot requiring it to come to a stop within 5e-4 radians of its final position, it takes too long!!
I am working on a high speed picking application and need to be able to stream moves to it as quickly and reliably as possible. I actually calculate the entire pick and place prior to sending any trajectories to the robot. The trajectories are continuous but do not account for stopping error and will require the driver to handle that portion.
gavanderhoorn
commented
4 years ago @MarqRazz: could I please ask you to calm down a little? Your phrasing and choice of words make your comments sound somewhat antagonistic. You may not intend it that way, but please take that into account.
Is it really required to have it match that close?
the reason the check is in there is because MotoROS essentially uses incremental motion from the "current pose" to execute the entire trajectory. As such, discrepancies between the start state of the trajectory and the current pose of the robot will not be corrected during execution, leading to further divergence over time.
Increasing the tolerances will essentially mean that you'll start losing trajectory execution accuracy.
is there a reason this driver stack checks the start state multiple times (this seems redundant and just adds extra delay)?
your statement is somewhat ambiguous, as this driver consists of two parts: MotoROS (running on the controller) and motoman_driver (which is the ROS side). MotoROS can be (and is) used without motoman_driver, so having the check on both sides makes perfect sense.
gavanderhoorn
commented
4 years ago Additionally: @MarqRazz: could you confirm whether you are using the standard driver or with #215 merged? If you don't have #215 merged, this issue is not the place to post about your problems.
Please open a new issue in that case.
EricMarcil
commented
4 years ago As far as the MotoRos is concern, the check is is only done on the initial starting point. To avoid delay from stop and start, some people just keep the trajectory running even if is the robot is static. However, there is a limit in doing that of about 4 hours because of the lost of precision for the float variable specified by the simple message structure. So, they usually reset the trajectory from time to time at the importunate time.
ted-miller
commented
4 years ago I believe that #215 is designed around the idea of using a single long trajectory. The point streaming interface should allow you to keep sending commands without waiting for in_motion to stop.
MarqRazz
commented
4 years ago @MarqRazz: could I please ask you to calm down a little? Your phrasing and choice of words make your comments sound somewhat antagonistic. You may not intend it that way, but please take that into account.
My bad, I was not meaning any blame or trying to be antagonistic, I am just working on understanding how the driver was designed and operates.
could you confirm whether you are using the standard driver or with #215 merged? If you don't have #215 merged, this issue is not the place to post about your problems.
No I do not have #215 merged, I'll open a new issue. Should I consider merging it for my use case?
tdl-ua
commented
4 years ago Hi all,
In response to @harumo11 and @tanimfresh, I just wanted to provide a usage example for PR #215: https://github.com/tbs-ualberta/manipulator_pose_following/blob/dev/src/pose_following_node.cpp
This code basically implements teleoperation using point streaming on a 7-axis manipulator (have never gotten around to making it more HW-agnostic). It obtains the current joint angles, than adds small increments to those angles and sends the updated joint angles to the controller. I think the key word here is small because I have a suspicion (never confirmed) that the streaming server on the controller does not accept joint angle increments that are too large. The position mismatch error still occurred occasionally for me as well but pretty sporadically if I remember correctly (it's been a while).
This code also contains the IK computation via the Jacobian, which is obtained from MoveIt. It was always fast enough at a sampling rate of 40Hz. Having said that, there are better ways of obtaining desired joint angles (see here for example).
Hope this helps a little.
PS @MarqRazz I don't think merging #215 will help you. I might have some suggestions for your particular problem but would provide those in the appropriate issue.
MarqRazz
commented
4 years ago I have created a new issue #324
Sorry @tanimfresh and @harumo11 for dirtying up your issue thread! I would be happy to remove my comments from here if you would like me to clean it up.
gavanderhoorn
commented
4 years ago Sorry @tanimfresh and @harumo11 for dirtying up your issue thread! I would be happy to remove my comments from here if you would like me to clean it up.
This is not necessary. I'll hide them as off-topic.
There is still value in keeping them here.
harumo11
commented
4 years ago @tanimfresh As I mentioned, we have to send message with zero velocity before robot moves time and again. Simply put, our program structure should be as bellow.
publish_frequency = 40
r = rospy.Rate(publish_frequency)
trajectory_msgs = JointTrajectory()
trajectory_point_msg = JointTrajectoryPoint()
trajectory_point_msg.positions = current_joint_values
trajectory_point_msg.velocities = [0,0,0,0,0,0]
trajectory_point_msg.time_from_start = rospy.Duration(0)
trajectory_msgs.header.stamp = rospy.Time.now()
trajectory_msgs.joint_names = joint_names
trajectory_msgs.points.append(trajectory_point_msg)
for i in range(10)
pub.publish(trajectory_msgs)
r.sleep()
print 'Initial target joints were published'
try:
while True:
#Here is a main loop. motor positions are increasedBut sorry. This is not fundamental solution.
Hello developers of this great ROS package.
I'd be glad if anyone tell me the solutions.
Summary
I got following error when this my program run.
Environment
My environment is as follows.
OS : Ubuntu 18.04 ROS Version : Melodic Robot : SIA20 Controller : FS100 Motoman Package : #215 merged
Details
Now I try to control SIA20 using HTC Vive. I made pose_follow.cpp and confirmed that I can control SIA20 with HTC Vive controller.
But the program can't move the robot smoothly because MoveIt blocking until the robot trajectory finish completely. So, as next step, I changed the program in order to move the robot smoothly using point-streaming interface(joint_command topic). The changed program is pose_follow_fast.cpp.
When the changed program run, I got following error.
roslaunch output with error
``` robot@robot-NG:~$ roslaunch motoman_sia20d_moveit_config moveit_planning_execution.launch sim:=false controller:=fs100 robot_ip:=10.0.0.2 ... logging to /home/robot/.ros/log/52fec8de-3095-11ea-bfa8-309c23cec38a/roslaunch-robot-NG-17312.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt WARNING: disk usage in log directory [/home/robot/.ros/log] is over 1GB. It's recommended that you use the 'rosclean' command. started roslaunch server http://robot-NG:36287/ SUMMARY ======== PARAMETERS * /controller_joint_names: ['joint_s', 'join... * /mongo_wrapper_ros_robot_NG_17312_3303416793777848684/database_path: /home/robot/catki... * /mongo_wrapper_ros_robot_NG_17312_3303416793777848684/overwrite: False * /move_group/allow_trajectory_execution: True * /move_group/allowed_execution_duration_scaling: 1.2 * /move_group/allowed_goal_duration_margin: 0.5 * /move_group/capabilities: move_group/MoveGr... * /move_group/controller_list: [{'action_ns': 'j... * /move_group/jiggle_fraction: 0.05 * /move_group/manipulator/longest_valid_segment_fraction: 0.05 * /move_group/manipulator/planner_configs: ['SBLkConfigDefau... * /move_group/manipulator/projection_evaluator: joints(joint_s,jo... * /move_group/max_range: 5.0 * /move_group/max_safe_path_cost: 1 * /move_group/moveit_controller_manager: moveit_simple_con... * /move_group/moveit_manage_controllers: True * /move_group/octomap_resolution: 0.025 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon... * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT * /move_group/planning_plugin: ompl_interface/OM... * /move_group/planning_scene_monitor/publish_geometry_updates: True * /move_group/planning_scene_monitor/publish_planning_scene: True * /move_group/planning_scene_monitor/publish_state_updates: True * /move_group/planning_scene_monitor/publish_transforms_updates: True * /move_group/request_adapters: default_planner_r... * /move_group/start_state_max_bounds_error: 0.1 * /robot_description:I don't know what I make mistake and I could not figure out the meaning of the error message
"Trajectory start position doesn't match current robot position (3011)"because the robot didn't move absolutely.I would be glad if you help me. If you would like to get more information, please feel free to tell me. Thanks in advance.