IK - Invalid pose every single time for Baxter

edit

def Compute_IKin(limb, seed_name, seed_pos, pose):
    # ROS Params initalization
    print "inside IK"
    embed()
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    seed = JointState()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')
    limb_joints = {}

ikreq.pose_stamp.append(pose)
seed.name = seed_name
seed.position = seed_pos
ikreq.seed_angles.append(seed)

try:
    rospy.wait_for_service(ns, 5.0)
    resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
    rospy.logerr("Service call failed: %s" % (e,))
    return 1
 '''  
       CODE SNIPPED FOR FINDING VALID and INVALID POSES AND RETURN 
 '''
return joints

  #This function takes in previous position pose_curr and differential position values positions [x,y,z] and adds this to the pose_cur
 def AssignPosition(positions,header,pose_curr):
            pose = PoseStamped()
        embed()
        pose.header = header
        pose.pose.position.x = pose_curr.pose_stamped[0].pose.position.x + positions[0]
        pose.pose.position.y = pose_curr.pose_stamped[0].pose.position.y + positions[1]
        pose.pose.position.z = pose_curr.pose_stamped[0].pose.position.z + positions[2]
        print "DONE"
        embed()
        return pose

# MAIN FUNCTION
 def main():
       rospy.init_node("kinematics_2")
       robot = moveit_commander.RobotCommander()
       left_arm = moveit_commander.MoveGroupCommander("left_arm")
       fk_left = [left_arm.get_end_effector_link()]
       header = Header(0,rospy.Time.now(),"/base")
       # JOINT STATE MESSAGE IS EMPTY _ MANUAALY ASSIGNING VALUES
       joints_name = np.load('joints_list_name.npy')
       joints_value = np.load('joints_pose_values.npy')
       joints_info = RobotState()
       joints_info.joint_state.header = header
       joints_info.joint_state.name = joints_name
       joints_info.joint_state.position = joints_value
   rospy.wait_for_service('compute_fk')

   try:
      moveit_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
      except rospy.ServiceException as e:
      rospy.logerror("Service call failed: %s"%e)

      #Rosservice Call FK for 0 intiialization
      pose_left_curr = moveit_fk(header, fk_left, joints_info)
      positions = np.load(file_path+'ground_truth.npy')
      pose_left = PoseStamped()
  if flag == 0:
        prev_name_left = joints_name
        #prev_name_right = joints_name
        prev_pos_left = joints_value
        #prev_pos_right = joints_value

         for i in range(len(positions)):
            pose_left = AssignPosition(positions[i],header,pose_left_curr)
            left,prev_name_left,prev_pos_left = Compute_IKin("left",prev_name_left,prev_pos_left,pose_left)

Hello all, in the above snippet I have three functions. Compute_IKin computes inversi kinematics. In the main funciton, I am passing the initiali value of a robot and finding the IK solution for that. I was to follow a trajectory, so I have values of idfferential positiions between x2-x1(timestep_2 - timestep1) ,y2-y1 and z2-z1. I add the differential values to the curr_pose in AssignPositions() , return it's newly calculated pose to main and try to compute_IK().

• But all of my poses are invalid. I am not sure why, because these alues are valid and were calculated from actual robot positions. Do you have any lead as to why, I have checked the initial seed posistion and that is valied, anything else tht I compute from then on becomes invalid.
• Could I be messing with the header time stamp?