Ask Your Question
0

IK - Invalid pose every single time for Baxter

asked 2017-11-15 16:34:01 -0500

Joy16 gravatar image
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?
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2017-11-15 17:24:18 -0500

imcmahon gravatar image

updated 2017-11-15 20:10:20 -0500

The Inverse Kinematics is returning an invalid result as you are not supplying a desired orientation for the pose. Without setting an orientation, the default Pose::Quaternion is a vector of all zeros [0,0,0,0], which is an invalid Quaternion. I would recommend either

  • read a bit on Quaternions in order to calculate your own

-or-

  • move Baxter's end effector into the desired orientation with respect to the base of the robot, check the arm's current Pose::Quaternion, and use that:

   $ rostopic echo -n1 /robot/limb/left/endpoint_state
    header: 
    ...
    pose: 
      position: 
        x: 0.529521061953
        y: 0.205000088025
        z: -0.287943031364
    orientation: 
        x: -0.229715175883
        y: 0.970497091367
        z: 0.00287481046527
        w: 0.0731988325182

Whether or not the latter method will work at an arbitrary Pose::Position is arm-configuration dependent.

edit flag offensive delete link more

Comments

A more beginner-friendly quaternion link: http://wiki.ros.org/Tutorials/Quaternions

AndyZe gravatar imageAndyZe ( 2017-11-15 18:33:10 -0500 )edit

Let’s edit the answer with your link. It is indeed much better

imcmahon gravatar imageimcmahon ( 2017-11-15 20:09:38 -0500 )edit

Hello! That indeed was the problem, I keep providing the quartenione of my seed angle adn it seemed to work. Thanks!

Joy16 gravatar imageJoy16 ( 2017-11-16 15:31:50 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2017-11-15 16:34:01 -0500

Seen: 423 times

Last updated: Nov 15 '17