Motoman driver with point streaming gets error "Trajectory start position doesn't match current robot position (3011)" #312
Comments
|
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 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. |
|
Thanks for quick response @gavanderhoorn . Yes, your understanding is right. I'm sorry I didn't explain program flow. I added But, I'm not sure And then I got same error. Could you tell me appropriate method to modify joint state of |
|
I think up-to-date joint state isn't needed for planning because my program just use Do you know when the motoman driver need up-to-date joint state. |
|
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. |
|
@EricMarcil: @harumo11 is using #215, which extends the driver with a streaming interface. This allows users to produce I'm not entirely sure whether the way @harumo11 interfaces with the extended version of the driver is correct. |
|
Thanks for your great support @gavanderhoorn and @EricMarcil ! As @gavanderhoorn and @EricMarcil said above
and
I changed publish rate of Before this trial, I made joint_trajectory_publisher.cpp which could send command via |
|
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 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:
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: Code:#!/usr/bin/env python
import sys
import select
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from geometry_msgs.msg import PoseStamped, Pose
from industrial_msgs.msg import RobotStatus
import time
import thread
import math
from std_msgs.msg import String, Header
from moveit_commander.conversions import pose_to_list
import moveit_commander.conversions as conversions
from AngVelScaling import gp8Pose
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from moveit_msgs.msg import RobotState
from sensor_msgs.msg import JointState
class GP8Commander(object):
def __init__(self):
super(GP8Commander, self).__init__()
rospy.init_node('gp8_commander', anonymous=False)
## Initialize all move_it related information:
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
group_name = "gp8"
self.group = moveit_commander.MoveGroupCommander(group_name)
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
self.box_name = ''
self.planning_frame = self.group.get_planning_frame()
self.eef_link = self.group.get_end_effector_link()
self.group_names = self.robot.get_group_names()
def main():
#Create main class
gp8 = GP8Commander()
#Create publisher for joint_command topic
pub = rospy.Publisher('/joint_command', JointTrajectory, queue_size= 0)
#Set publish/loop rate
publish_frequency = 40
r = rospy.Rate(publish_frequency)
#Move to start position
looking = [0,0.6878,0.8525,0,1.42249,0.0]
gp8.group.go(looking, wait = True)
counter = 0
#Start setup of traj streaming; create initial point @ current position
current_joint_values = gp8.group.get_current_joint_values()
joint_names = gp8.group.get_joints()
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)
pub.publish(trajectory_msgs)
print 'Initial target joints were published'
try:
while True:
#Create new trajectory with one point to stream
counter = counter + 1
new_trajectory_msgs = JointTrajectory()
new_trajectory_msgs.joint_names = joint_names
new_trajectory_msgs.header.stamp = rospy.Time.now()
new_trajectory_point = JointTrajectoryPoint()
#Time from start should be loop iteration times 1/frequency
new_trajectory_point.time_from_start = rospy.Duration((counter)*0.025)
new_trajectory_point.velocities = [0,0,0,0,0,0]
new_trajectory_point.positions = gp8.group.get_current_joint_values()
#Move 'S' joint for incremental motion
new_trajectory_point.positions[0] += 0.001
new_trajectory_msgs.points.append(new_trajectory_point)
pub.publish(new_trajectory_msgs)
r.sleep()
except KeyboardInterrupt:
raise
if __name__ == '__main__':
try:
main()
except rospy.ROSInterruptException:
passOutput: |
|
Hello @tanimfresh Please try to send initial message which has zero velocity command time and again with 40Hz. I'm glad if this solution helps you. |
|
@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? |
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
|
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. |
This comment has been minimized.
This comment has been minimized.
This is not necessary. I'll hide them as off-topic. There is still value in keeping them here. |
|
@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. But sorry. This is not fundamental solution. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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
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.
The text was updated successfully, but these errors were encountered: