MoveIt Trajectory-Start Test Script

moveit_trajStart.py

#!/usr/bin/env python

import sys
import math
import numpy as np
import rospy
from moveit_commander import MoveGroupCommander
from moveit_msgs.msg import RobotState

'''
This script will aid in testing the issue (https://github.com/ros-industrial/motoman/issues/34) where MoveIt generates a trajectory that does not start at the current robot position.
'''

# constants
PLANNING_GROUP = "manipulator"
OFFSET         = 0.2    # delta between generated positions
TOL            = 1e-6   # tolerance for comparing positions

# globals
group = MoveGroupCommander(PLANNING_GROUP)

def array2str(a, fmt):
  return "[" + ", ".join(fmt%v for v in a) + "]"

def generate_moves():
  curPos = group.get_current_joint_values()

  moves = []
  moves.append( [x+1*OFFSET for x in curPos] )
  moves.append( [x+2*OFFSET for x in curPos] )
  moves.append( [x+1*OFFSET for x in curPos] )
  moves.append( curPos )

  print "Current position: "
  print group.get_active_joints()
  print "  " + array2str(curPos, "%0.6f")

  print "Sequence of positions:"
  for i in range(len(moves)):
    print "  %i) "%(i+1) + array2str(moves[i], "%0.6f")

  return moves

def sort_joints(pos, joints, ref):
  result = []
  for jnt in ref:
    result.append(pos[joints.index(jnt)])
  return result

def get_current_state():
  curState = RobotState()
  curState.joint_state.name = group.get_active_joints()
  curState.joint_state.position = group.get_current_joint_values()
  return curState

def validate_plan(before, plan, after, goal):
  # check for robot motion during planning
  #delta = np.array(after) - np.array(before)
  #if np.max(np.abs(delta)) > TOL:
  #  print "WARN: Robot moved during planning: "
  #  print "  before: " + array2str(before, "%0.6f")
  #  print "  after : " + array2str(after, "%0.6f")
  #  print "  delta : " + array2str(delta, "%0.6f")

  # check valid trajectory start
  t0 = sort_joints(plan.joint_trajectory.points[0].positions,
                   plan.joint_trajectory.joint_names,
                   group.get_active_joints() )
  delta = np.array(t0) - np.array(before)
  if np.max(np.abs(delta)) > TOL:
    print "ERROR: Traj start doesn't match curPos: "
    print "  curPos  : " + array2str(before, "%0.6f")
    print "  traj[0] : " + array2str(t0, "%0.6f")
    print "  delta   : " + array2str(delta, "%0.6f")

  # check valid trajectory end
  tN = sort_joints(plan.joint_trajectory.points[-1].positions,
                   plan.joint_trajectory.joint_names,
                   group.get_active_joints() )
  delta = np.array(tN) - np.array(goal)
  if np.max(np.abs(delta)) > group.get_goal_joint_tolerance():
    print "ERROR: Traj end doesn't match goal: "
    print "  goal    : " + array2str(goal, "%0.6f")
    print "  traj[N] : " + array2str(tN, "%0.6f")
    print "  delta   : " + array2str(delta, "%0.6f")

def main(argv):
  rospy.init_node("moveit_trajStart")

  try:
    loopDelay = float(argv[1])
  except:
    loopDelay = 0

  moves = generate_moves()

  for i in range(len(moves)):
    rospy.loginfo("Planning move to pos %i"%(i+1))

#    group.set_start_state(get_current_state());
    group.set_joint_value_target(moves[i])

    pBefore = group.get_current_joint_values()
    plan = group.plan()
    pAfter = group.get_current_joint_values()

    validate_plan(pBefore, plan, pAfter, moves[i])

    rospy.loginfo("Starting move to pos %i"%(i+1))
    group.execute(plan)
#    group.go()
    rospy.loginfo("Done with move to pos %i"%(i+1))
    rospy.sleep(loopDelay)

if __name__=="__main__":
  main(sys.argv)