Invalid Trajectory: start point deviates from current robot state more than 0.01 joint
Hello,everyone:
I am working on a programme which aims at using the EMG signals to control the robot.I have finished a version of script.I use the execute(plan,False)
to async execute the path. When I stop the robot use MoveGroupCommander.stop()
,I compute a new cartesian path like this
arm.stop()
start_pose = arm.get_current_pose(end_effector_link).pose
if (start_pose.position.z>0)or(start_pose.position.z==0):
theta=math.acos((start_pose.position.x-0.5)*4)
else:
theta=(math.acos((start_pose.position.x-0.5)*4))*(-1)
delta=(theta-math.pi*1/2)/32
#rospy.loginfo(theta)
waypoints = []
wpose=[]
if cartesian:
waypoints.append(start_pose)
wpose = deepcopy(start_pose)#
for i in range(30):
wpose.position.x = 0.5+math.cos(theta)*0.25
wpose.position.z = math.sin(theta)*0.25
if cartesian:
waypoints.append(deepcopy(wpose))
theta -=delta
#rospy.loginfo(theta)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
arm.set_start_state_to_current_state()
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses,
0.001, # eef_step,
0.0, # jump_threshold
True) # avoid_collisions
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan,False)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
but the terminal report error:"Invalid Trajectory: start point deviates from current robot state more than 0.01 joint"
I wonder why this error occurs because I have set the current pose to the start pose.
Thanks for your reading and I will appreciate if anyone give me some advice
Any Update on this? I've got the same issue and am unsure about how to solve it.