Начало цели не совпадает с текущей ошибкой позы при выдаче JointTrajectoryPoint - PullRequest
Новая
       54

Начало цели не совпадает с текущей ошибкой позы при выдаче JointTrajectoryPoint

0 голосов
/ 03 октября 2018

Я новичок в ROS.Мой UR5 что-то ударил во время выполнения test_move.py из драйвера ur_modern.После этого эта ошибка появляется постоянно, когда я пытаюсь запустить один и тот же код test_move.py.Msgstr "Начало цели не соответствует текущей позе".Как мне решить это?Если необходима повторная инициализация суставов, сообщите мне, как это сделать?

Я прошел по этой ссылке (https://github.com/ros-industrial/ur_...) и обнаружил некоторые подсказки, в которых говорится: всякий раз, когда вы вычисляете траекторию, выполните следующий шаг:

  • Вычислите IK вашей цели.
  • создайте свою траекторию
  • дождитесь сообщения об обновлении / joint_states
  • стартовая позиция на основе нового значения из / joint_state
  • отправит вашу траекторию водителю

Это даст мне решение.Но как мне добавить начальное состояние в мой код траектории?Почему мне нужно дать начальное состояние так же, как позу робота.Робот должен быть в состоянии переместиться на новую позицию вправо.Ниже приведен код test_move.py, который я запускаю и получаю сообщение об ошибке: 

def move1():
global joints_pos
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
    joint_states = rospy.wait_for_message("joint_states", JointState)
    joints_pos = joint_states.position
    g.trajectory.points = [
        JointTrajectoryPoint(positions=joints_pos, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
        JointTrajectoryPoint(positions=Q1, velocities=[0]*6, time_from_start=rospy.Duration(2.0)),
        JointTrajectoryPoint(positions=Q2, velocities=[0]*6, time_from_start=rospy.Duration(3.0)),
        JointTrajectoryPoint(positions=Q3, velocities=[0]*6, time_from_start=rospy.Duration(4.0))]
    client.send_goal(g)
    client.wait_for_result()
except KeyboardInterrupt:
    client.cancel_goal()
    raise
except:
    raise

def main():
global client
try:
    rospy.init_node("test_move", anonymous=True, disable_signals=True)
    client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
    print "Waiting for server..."
    client.wait_for_server()
    print "Connected to server"
    parameters = rospy.get_param(None)
    index = str(parameters).find('prefix')
    if (index > 0):
        prefix = str(parameters)[index+len("prefix': '"):(index+len("prefix': '")+str(parameters)[index+len("prefix': '"):-1].find("'"))]
        for i, name in enumerate(JOINT_NAMES):
            JOINT_NAMES[i] = prefix + name
    print "This program makes the robot move between the following three poses:"
    print str([Q1[i]*180./pi for i in xrange(0,6)])
    print str([Q2[i]*180./pi for i in xrange(0,6)])
    print str([Q3[i]*180./pi for i in xrange(0,6)])
    print "Please make sure that your robot can move freely between these poses before proceeding!"
    inp = raw_input("Continue? y/n: ")[0]
    if (inp == 'y'):
        #move1()
        move_repeated()
        #move_disordered()
        #move_interrupt()
    else:
        print "Halting program"
except KeyboardInterrupt:
    rospy.signal_shutdown("KeyboardInterrupt")
    raise
...