Invalid Trajectory: start point deviates from current robot state more than 0.01 joint

asked 2020-04-10 22:39:50 -0500

Charlie Chen gravatar image

updated 2020-04-11 03:44:44 -0500

gvdhoorn gravatar image

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

edit retag flag offensive close merge delete

Comments

Any Update on this? I've got the same issue and am unsure about how to solve it.

hantoo gravatar image hantoo  ( 2021-07-17 01:11:32 -0500 )edit