Move robotic arm continuously

edit

Right now I am working with a program which is allowing robotic arm to follow the aruco marker , however, after getting a coordinate from aruco marker the robot goes to that coordinate and then finishes the motion plan and after that move to the second position. It waits till it completes it first motion plan. I want it to plan continuously without reaching the goal like If I move the aruco marker it should also move before finishing the before motion plan. I tried setting _commander.go(wait=False) however the trajectory seems taking jerk. I want it to move smooth.

ic = move_arm()
rospy.init_node('move_group_python_interface_tutorial',anonymous=True)
rate = rospy.Rate(1) # ROS Rate at 5Hz

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
_commander = moveit_commander.move_group.MoveGroupCommander('xarm{}'.format(6))
_commander.set_max_acceleration_scaling_factor(1)
_commander.set_max_velocity_scaling_factor(1)


while not rospy.is_shutdown():

    planning_frame = _commander.get_planning_frame()
    print("============ Reference frame: %s" % planning_frame)


    eef_link = _commander.get_end_effector_link()
    print("============ End effector: %s" % eef_link)


    group_names = robot.get_group_names()
    print("============ Robot Groups:", robot.get_group_names())

    print("============ Printing robot state")
    print(robot.get_current_state())
    print ("")

    current_pose = _commander.get_current_pose(end_effector_link='link6').pose

    print(f"The current pose is {current_pose}")
    pose_goal = geometry_msgs.msg.Pose()


    pose_goal.position.x = ic.my_data[0]
    pose_goal.position.y = ic.my_data[1]
    pose_goal.position.z =  ic.my_data[2]

    pose_goal.orientation.x =  ic.my_data[3]
    pose_goal.orientation.y =  ic.my_data[4]
    pose_goal.orientation.z =  ic.my_data[5]
    pose_goal.orientation.w =  ic.my_data[6]

    print(pose_goal)
    _commander.set_pose_target(pose_goal)

    _commander.set_goal_tolerance(10e-6)

    rospy.loginfo("Planning Trajectory to Pose 1")
    plan1 = _commander.plan()
    rospy.loginfo("Done Planning Trajectory to Pose 1")
    rospy.loginfo("Executing Trajectory to Pose 1")
    # `go()` returns a boolean indicating whether the planning and execution was successful.
    success = _commander.go(wait=True)


    current_pose = _commander.get_current_pose(end_effector_link='link6').pose

    print(f"The current pose at the end is {current_pose}")


    rospy.loginfo("Done Executing Trajectory to Pose 1")

    # Calling `stop()` ensures that there is no residual movement
    # _commander.stop()
    # It is always good to clear your targets after planning with poses.
    # Note: there is no equivalent function for clear_joint_value_targets().
    _commander.clear_pose_targets()

    rate.sleep()