def find_ik_for_grasping_pose(self, pose_stamped):
     solver_info = self.get_solver_info_srv()
     arm_joints = solver_info.kinematic_solver_info.joint_names
     
     req = GetPositionIKRequest()
     req.timeout = rospy.Duration(5.0)
     req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
     req.ik_request.pose_stamped = pose_stamped
     
     try:
         current_state = rospy.wait_for_message('/joint_states', JointState, 2.0)
         
         req.ik_request.ik_seed_state.joint_state.name = arm_joints
         req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints]
         
         if not self.set_planning_scene_diff_client():
             rospy.logerr('%s: Find IK for Grasp: failed to set planning scene diff' % ACTION_NAME)
             return None
             
         ik_result = self.get_ik_srv(req)
         
         if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
             return ik_result.solution
         else:
             rospy.logerr('%s: failed to find an IK for requested grasping pose, aborting' % ACTION_NAME)
             return None
     except:
         rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
         return None
    def set_left_arm_pose(self, position, orientation):
        '''
        Move the left arm to a position
        @param position: x,y,z
        @param angles: quaternion
        '''

        req = GetPositionIKRequest()
        req.timeout = rospy.Duration(5.0)
        req.ik_request.ik_link_name = "l_wrist_roll_link"

        req.ik_request.pose_stamped.header.frame_id = "base_link"
        req.ik_request.pose_stamped.pose.position.x = position[0]
        req.ik_request.pose_stamped.pose.position.y = position[1]
        req.ik_request.pose_stamped.pose.position.z = position[2]

        req.ik_request.pose_stamped.pose.orientation.x = orientation[0]
        req.ik_request.pose_stamped.pose.orientation.y = orientation[1]
        req.ik_request.pose_stamped.pose.orientation.z = orientation[2]
        req.ik_request.pose_stamped.pose.orientation.w = orientation[3]

        req.ik_request.ik_seed_state.joint_state.position = self.robot_state.left_arm_positions
        req.ik_request.ik_seed_state.joint_state.name = self.robot_state.left_joint_names

        res = self.left_ik(req)
        if res.error_code.val != 1:
            rospy.logerr("IK error, code is %d" % res.error_code.val)
            return None

        joints = res.solution.joint_state.position
        return joints
Beispiel #3
0
 def set_left_arm_pose(self, position, orientation):
     '''
     Move the left arm to a position
     @param position: x,y,z
     @param angles: quaternion
     '''        
     
     req = GetPositionIKRequest()
     req.timeout = rospy.Duration(5.0)
     req.ik_request.ik_link_name = "l_wrist_roll_link"
     
     req.ik_request.pose_stamped.header.frame_id = "base_link"
     req.ik_request.pose_stamped.pose.position.x = position[0]
     req.ik_request.pose_stamped.pose.position.y = position[1]
     req.ik_request.pose_stamped.pose.position.z = position[2]
     
     req.ik_request.pose_stamped.pose.orientation.x = orientation[0]
     req.ik_request.pose_stamped.pose.orientation.y = orientation[1]
     req.ik_request.pose_stamped.pose.orientation.z = orientation[2]
     req.ik_request.pose_stamped.pose.orientation.w = orientation[3]
     
     req.ik_request.ik_seed_state.joint_state.position = self.robot_state.left_arm_positions
     req.ik_request.ik_seed_state.joint_state.name = self.robot_state.left_joint_names
     
     res = self.left_ik(req)
     if res.error_code.val != 1:
         rospy.logerr("IK error, code is %d" % res.error_code.val)
         return None
     
     joints = res.solution.joint_state.position
     return joints
Beispiel #4
0
    def IK(self, arm, p, rot, q_guess):
        rospy.logwarn('Currently ignoring the arm parameter.')
        ik_req = GetPositionIKRequest()
        ik_req.timeout = rospy.Duration(5.)
        ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
        ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'

        ik_req.ik_request.pose_stamped.pose.position.x = p[0, 0]
        ik_req.ik_request.pose_stamped.pose.position.y = p[1, 0]
        ik_req.ik_request.pose_stamped.pose.position.z = p[2, 0]

        quat = tr.matrix_to_quaternion(rot)
        ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
        ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
        ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
        ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]

        ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
        ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list

        ik_resp = self.ik_srv.call(ik_req)
        if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
            ret = ik_resp.solution.joint_state.position
        else:
            rospy.logerr('Inverse kinematics failed')
            ret = None

        return ret
  def _updateIKFeasibility(self, ps):
    p = PoseStamped()
    p.pose.orientation = ps.pose.orientation
    p.header.frame_id = '/torso_lift_link'
    offset = -0.216 #distance from tool frame to wrist_roll_link
    q = ps.pose.orientation
    rot = tr.tft.quaternion_matrix((q.x, q.y, q.z, q.w))
    offset_vec = rot * np.matrix([[offset, 0., 0., 1.]]).T
    p.pose.position.x = ps.pose.position.x + offset_vec[0]
    p.pose.position.y = ps.pose.position.y + offset_vec[1]
    p.pose.position.z = ps.pose.position.z + offset_vec[2]
    self.test_pos_pub.publish(p)

    req = GetPositionIKRequest()
    req.timeout = rospy.Duration(5.0)
    ik_req = PositionIKRequest()
    ik_req.ik_link_name = self.ik_solver_info.link_names[-1]
    ik_req.ik_seed_state.joint_state.name = self.ik_solver_info.joint_names
    ik_req.ik_seed_state.joint_state.position = self.joint_state
    ik_req.pose_stamped = ps
    req.ik_request = ik_req
    ik_sol = self.pr2_ik_client.call(req)
    if ik_sol.error_code.val < 0:
      print "IK Failed with error code: %s" %ik_sol.error_code.val
      for marker in self.wp_im.controls[0].markers:
        marker.color = ColorRGBA(1,0,0,0.7)
    else:
      for marker in self.wp_im.controls[0].markers:
        marker.color = ColorRGBA(0,1,0,0.7)
Beispiel #6
0
    def IK(self, arm, p, rot, q_guess):
        rospy.logwarn('Currently ignoring the arm parameter.')
        ik_req = GetPositionIKRequest()
        ik_req.timeout = rospy.Duration(5.)
        ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
        ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'

        ik_req.ik_request.pose_stamped.pose.position.x = p[0,0]
        ik_req.ik_request.pose_stamped.pose.position.y = p[1,0]
        ik_req.ik_request.pose_stamped.pose.position.z = p[2,0]

        quat = tr.matrix_to_quaternion(rot)
        ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
        ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
        ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
        ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]

        ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
        ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list

        ik_resp = self.ik_srv.call(ik_req)
        if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
            ret = ik_resp.solution.joint_state.position
        else:
            rospy.logerr('Inverse kinematics failed')
            ret = None

        return ret
    def motionToTrajectory(self, action, frame_id):
        """ Convert an arm movement action into a trajectory. """
        ps = PoseStamped()
        ps.header.frame_id = frame_id
        ps.pose = action.goal
        pose = self._listener.transformPose(self.root, ps)
        rospy.loginfo("Parsing move to:\n" + str(pose))
        
        # create IK request
        request = GetPositionIKRequest()
        request.timeout = rospy.Duration(self.timeout)

        request.ik_request.pose_stamped.header.frame_id = self.root;
        request.ik_request.ik_link_name = self.tip;
        request.ik_request.pose_stamped.pose.position.x = pose.pose.position.x
        request.ik_request.pose_stamped.pose.position.y = pose.pose.position.y
        request.ik_request.pose_stamped.pose.position.z = pose.pose.position.z

        e = euler_from_quaternion([pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w])
        e = [i for i in e]
        if self.dof < 6:
            # 5DOF, so yaw angle = atan2(Y,X-shoulder offset)
            e[2] = atan2(pose.pose.position.y, pose.pose.position.x-self.offset)
        if self.dof < 5:
            # 4 DOF, so yaw as above AND no roll
            e[0] = 0
        q =  quaternion_from_euler(e[0], e[1], e[2])

        request.ik_request.pose_stamped.pose.orientation.x = q[0]
        request.ik_request.pose_stamped.pose.orientation.y = q[1]
        request.ik_request.pose_stamped.pose.orientation.z = q[2]
        request.ik_request.pose_stamped.pose.orientation.w = q[3]

        request.ik_request.ik_seed_state.joint_state.name = self.arm_solver_info.kinematic_solver_info.joint_names
        request.ik_request.ik_seed_state.joint_state.position = [self.servos[joint] for joint in request.ik_request.ik_seed_state.joint_state.name]

        # get IK, wiggle if needed
        tries = 0
        pitch = e[1]
        print "roll", e[0]
        while tries < 80:
            try:
                response = self._get_ik_proxy(request)
                if response.error_code.val == response.error_code.SUCCESS:
                    break
                else:
                    tries += 1
                    # wiggle gripper
                    pitch = e[1] + ((-1)**tries)*((tries+1)/2)*0.025
                    # update quaternion
                    q = quaternion_from_euler(e[0], pitch, e[2])
                    request.ik_request.pose_stamped.pose.orientation.x = q[0]
                    request.ik_request.pose_stamped.pose.orientation.y = q[1]
                    request.ik_request.pose_stamped.pose.orientation.z = q[2]
                    request.ik_request.pose_stamped.pose.orientation.w = q[3]
            except rospy.ServiceException, e:
                print "Service did not process request: %s"%str(e)
 def form_ik_request(self, ps):
     #print "forming IK request for :%s" %ps
     req = GetPositionIKRequest()
     req.timeout = rospy.Duration(5)
     req.ik_request.pose_stamped = ps 
     req.ik_request.ik_link_name = self.ik_info.kinematic_solver_info.link_names[-1]
     req.ik_request.ik_seed_state.joint_state.name =  self.ik_info.kinematic_solver_info.joint_names
     req.ik_request.ik_seed_state.joint_state.position =  self.joint_state_act.positions
     return req
 def form_ik_request(self, ps):
     #print "forming IK request for :%s" %ps
     req = GetPositionIKRequest()
     req.timeout = rospy.Duration(5)
     req.ik_request.pose_stamped = ps
     req.ik_request.ik_link_name = self.ik_info.kinematic_solver_info.link_names[
         -1]
     req.ik_request.ik_seed_state.joint_state.name = self.ik_info.kinematic_solver_info.joint_names
     req.ik_request.ik_seed_state.joint_state.position = self.joint_state_act.positions
     return req
Beispiel #10
0
 def form_ik_request(self, ps):
     """Compose an IK request from ik solver info and pose goal"""
     req = GetPositionIKRequest()
     req.timeout = rospy.Duration(5)
     req.ik_request.ik_link_name = self.ik_info.link_names[-1]
     req.ik_request.pose_stamped = ps
     req.ik_request.ik_seed_state.joint_state.name = self.ik_info.joint_names
     with self.joint_state_lock:
         positions = self.joint_state['positions'][:]
         velocities = self.joint_state['velocities'][:]
     req.ik_request.ik_seed_state.joint_state.position = positions
     req.ik_request.ik_seed_state.joint_state.velocity = velocities
     return req
Beispiel #11
0
 def form_ik_request(self, ps):
     """Compose an IK request from ik solver info and pose goal"""
     req = GetPositionIKRequest()
     req.timeout = rospy.Duration(5)
     req.ik_request.ik_link_name = self.ik_info.link_names[-1]
     req.ik_request.pose_stamped = ps
     req.ik_request.ik_seed_state.joint_state.name = self.ik_info.joint_names
     with self.joint_state_lock:
         positions = self.joint_state['positions'][:]
         velocities = self.joint_state['velocities'][:]
     req.ik_request.ik_seed_state.joint_state.position = positions
     req.ik_request.ik_seed_state.joint_state.velocity = velocities
     return req
 def form_ik_request(self, ps):
     #print "Form IK request for: \r\n" %str(ps)
     if (self.joint_state):
         req = GetPositionIKRequest()
         req.timeout = rospy.Duration(5)
         req.ik_request.pose_stamped = ps 
         req.ik_request.ik_link_name = self.curr_pose.fk_link_names[-1]
         req.ik_request.ik_seed_state.joint_state.name =  self.ik_info.kinematic_solver_info.joint_names
         req.ik_request.ik_seed_state.joint_state.position =  self.joint_state
         #print "IK Request: %s" %str(ik_request)
         return req
     else:
         rospy.loginfo("No Joint States Available Yet")
 def form_ik_request(self, ps):
     #print "Form IK request for: \r\n" %str(ps)
     if (self.joint_state):
         req = GetPositionIKRequest()
         req.timeout = rospy.Duration(5)
         req.ik_request.pose_stamped = ps
         req.ik_request.ik_link_name = self.curr_pose.fk_link_names[-1]
         req.ik_request.ik_seed_state.joint_state.name = self.ik_info.kinematic_solver_info.joint_names
         req.ik_request.ik_seed_state.joint_state.position = self.joint_state
         #print "IK Request: %s" %str(ik_request)
         return req
     else:
         rospy.loginfo("No Joint States Available Yet")
 def IK(self, pose, q_guess=[0]*7):
     ik_req = GetPositionIKRequest()
     ik_req.timeout = rospy.Duration(5.)
     ik_req.ik_request.ik_link_name = self.arm + '_wrist_roll_link'
     pos = pose[:3, 3].T.A[0].tolist()
     quat = tf_trans.quaternion_from_matrix(pose)
     ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
     ik_req.ik_request.pose_stamped.pose = Pose(Point(*pos), Quaternion(*quat))
     ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
     ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list
     ik_resp = self.ik_srv.call(ik_req)
     if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
         return ik_resp.solution.joint_state.position
     return None
Beispiel #15
0
    def IK(self, arm, pos, rot, q_guess=[0]*7):
        ik_req = GetPositionIKRequest()
        ik_req.timeout = rospy.Duration(5.)
        if arm == 0:
            ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
        else:
            ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
        ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
        quat = tr.matrix_to_quaternion(rot)
        ik_req.ik_request.pose_stamped.pose = Pose(Point(*pos), Quaternion(*quat))
        ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
        ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[arm]

        ik_resp = self.ik_srv[arm].call(ik_req)
        return ik_resp.solution.joint_state.position
 def IK(self, pose, q_guess=[0] * 7):
     ik_req = GetPositionIKRequest()
     ik_req.timeout = rospy.Duration(5.)
     ik_req.ik_request.ik_link_name = self.arm + '_wrist_roll_link'
     pos = pose[:3, 3].T.A[0].tolist()
     quat = tf_trans.quaternion_from_matrix(pose)
     ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'
     ik_req.ik_request.pose_stamped.pose = Pose(Point(*pos),
                                                Quaternion(*quat))
     ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
     ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list
     ik_resp = self.ik_srv.call(ik_req)
     if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
         return ik_resp.solution.joint_state.position
     return None
Beispiel #17
0
    def _setup_ik(self):
        '''Sets up services for inverse kinematics'''
        ik_info_srv_name = ('pr2_' + self._side() +
                            '_arm_kinematics_simple/get_ik_solver_info')
        ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
        rospy.wait_for_service(ik_info_srv_name)
        ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
                                         GetKinematicSolverInfo)
        solver_info = ik_info_srv()
        rospy.loginfo('IK info service has responded for ' + self._side() +
                      ' arm.')
        rospy.wait_for_service(ik_srv_name)
        self.ik_srv = rospy.ServiceProxy(ik_srv_name,
                                         GetPositionIK,
                                         persistent=True)
        rospy.loginfo('IK service has responded for ' + self._side() + ' arm.')

        # Set up common parts of an IK request
        self.ik_request = GetPositionIKRequest()
        self.ik_request.timeout = rospy.Duration(4.0)
        self.ik_joints = solver_info.kinematic_solver_info.joint_names
        self.ik_limits = solver_info.kinematic_solver_info.limits
        ik_links = solver_info.kinematic_solver_info.link_names

        request = self.ik_request.ik_request
        request.ik_link_name = ik_links[0]
        request.pose_stamped.header.frame_id = 'base_link'
        request.ik_seed_state.joint_state.name = self.ik_joints
        request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
Beispiel #18
0
    def _setup_ik(self):
        '''Sets up services for inverse kinematics.'''
        side = self._side()

        # Get IK info service.
        ik_info_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_IK_INFO_POSTFIX
        rospy.wait_for_service(ik_info_srv_name)
        ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
                                         GetKinematicSolverInfo)
        solver_info = ik_info_srv()
        rospy.loginfo('IK info service has responded for ' + side + ' arm.')

        # Get IK service.
        ik_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_IK_POSTFIX
        rospy.wait_for_service(ik_srv_name)
        self.ik_srv = rospy.ServiceProxy(ik_srv_name,
                                         GetPositionIK,
                                         persistent=True)
        rospy.loginfo('IK service has responded for ' + side + ' arm.')

        # Set up common parts of an IK request.
        self.ik_request = GetPositionIKRequest()
        self.ik_request.timeout = rospy.Duration(IK_REQUEST_TIMEOUT)
        self.ik_joints = solver_info.kinematic_solver_info.joint_names
        self.ik_limits = solver_info.kinematic_solver_info.limits
        ik_links = solver_info.kinematic_solver_info.link_names

        request = self.ik_request.ik_request
        request.ik_link_name = ik_links[0]
        request.pose_stamped.header.frame_id = BASE_LINK
        request.ik_seed_state.joint_state.name = self.ik_joints
        request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints)
Beispiel #19
0
    def __init__(self, side_prefix):
        self.side_prefix = side_prefix

        # Set up Inversse Kinematics services
        ik_info_srv_name = ('pr2_' + self._side() +
                            '_arm_kinematics_simple/get_ik_solver_info')
        ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'

        rospy.loginfo('Waiting for IK info service to respond.')
        rospy.wait_for_service(ik_info_srv_name)
        ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
                                         GetKinematicSolverInfo)
        solver_info = ik_info_srv()
        self.ik_joints = solver_info.kinematic_solver_info.joint_names
        self.ik_limits = solver_info.kinematic_solver_info.limits

        rospy.loginfo('Waiting for IK service to respond.')
        rospy.wait_for_service(ik_srv_name)
        self.ik_srv = rospy.ServiceProxy(ik_srv_name,
                                         GetPositionIK,
                                         persistent=True)

        rospy.loginfo('IK initialized for arm: ' + self._side())

        # Set up common parts of an IK request
        self.ik_request = GetPositionIKRequest()
        self.ik_request.timeout = rospy.Duration(4.0)
        self.ik_request.ik_request.ik_link_name = solver_info.kinematic_solver_info.link_names[
            0]
        self.ik_request.ik_request.pose_stamped.header.frame_id = 'base_link'
        self.ik_request.ik_request.ik_seed_state.joint_state.name = self.ik_joints
        self.ik_request.ik_request.ik_seed_state.joint_state.position = [
            0
        ] * len(self.ik_joints)
Beispiel #20
0
    def IK(self, arm, p, rot, q_guess):
        if arm != 1:
            arm = 0

        p = make_column(p)

        if rot.shape == (3, 3):
            quat = np.matrix(tr.matrix_to_quaternion(rot)).T
        elif rot.shape == (4, 1):
            quat = make_column(rot)
        else:
            rospy.logerr('Inverse kinematics failed (bad rotation)')
            return None

        # Transform point back to wrist roll link
        neg_off = [-self.off_point[0], -self.off_point[1], -self.off_point[2]]
        transpos = self.transform_in_frame(p, quat, neg_off)

        ik_req = GetPositionIKRequest()
        ik_req.timeout = rospy.Duration(5.)
        if arm == 0:
            ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
        else:
            ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
        ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'

        ik_req.ik_request.pose_stamped.pose.position.x = transpos[0]
        ik_req.ik_request.pose_stamped.pose.position.y = transpos[1]
        ik_req.ik_request.pose_stamped.pose.position.z = transpos[2]

        ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
        ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
        ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
        ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]

        ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
        ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[
            arm]

        ik_resp = self.ik_srv[arm].call(ik_req)
        if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
            ret = list(ik_resp.solution.joint_state.position)
        else:
            rospy.logerr('Inverse kinematics failed')
            ret = None

        return ret
Beispiel #21
0
    def IK(self, arm, p, rot, q_guess):
        if arm != 1:
            arm = 0

        p = make_column(p)

        if rot.shape == (3, 3):
            quat = np.matrix(tr.matrix_to_quaternion(rot)).T
        elif rot.shape == (4, 1):
            quat = make_column(rot)
        else:
            rospy.logerr('Inverse kinematics failed (bad rotation)')
            return None

        # Transform point back to wrist roll link
        neg_off = [-self.off_point[0],-self.off_point[1],-self.off_point[2]]
        transpos = self.transform_in_frame(p, quat, neg_off)
        
        ik_req = GetPositionIKRequest()
        ik_req.timeout = rospy.Duration(5.)
        if arm == 0:
            ik_req.ik_request.ik_link_name = 'r_wrist_roll_link'
        else:
            ik_req.ik_request.ik_link_name = 'l_wrist_roll_link'
        ik_req.ik_request.pose_stamped.header.frame_id = 'torso_lift_link'

        ik_req.ik_request.pose_stamped.pose.position.x = transpos[0] 
        ik_req.ik_request.pose_stamped.pose.position.y = transpos[1]
        ik_req.ik_request.pose_stamped.pose.position.z = transpos[2]

        ik_req.ik_request.pose_stamped.pose.orientation.x = quat[0]
        ik_req.ik_request.pose_stamped.pose.orientation.y = quat[1]
        ik_req.ik_request.pose_stamped.pose.orientation.z = quat[2]
        ik_req.ik_request.pose_stamped.pose.orientation.w = quat[3]

        ik_req.ik_request.ik_seed_state.joint_state.position = q_guess
        ik_req.ik_request.ik_seed_state.joint_state.name = self.joint_names_list[arm]

        ik_resp = self.ik_srv[arm].call(ik_req)
        if ik_resp.error_code.val == ik_resp.error_code.SUCCESS:
            ret = list(ik_resp.solution.joint_state.position)
        else:
            rospy.logerr('Inverse kinematics failed')
            ret = None

        return ret
Beispiel #22
0
def solve_ik(x, y, z):
    '''
    Need to call move_arm_ompl before calling this, otherwise it doesn't work. 
    '''
    joint_names = [  "r_shoulder_pan_joint",
                     "r_shoulder_lift_joint",
                     "r_upper_arm_roll_joint",
                     "r_elbow_flex_joint",
                     "r_forearm_roll_joint",
                     "r_wrist_flex_joint",
                     "r_wrist_roll_joint"]
    
    service_name = "pr2_right_arm_kinematics/get_ik"
    rospy.wait_for_service(service_name)
    try:
        ik_solver = rospy.ServiceProxy(service_name, GetPositionIK)
        req = GetPositionIKRequest()
        req.timeout = roslib.rostime.Duration(5)
        req.ik_request.ik_link_name = "r_wrist_roll_link"
        req.ik_request.pose_stamped.header.frame_id = "base_link"

        req.ik_request.pose_stamped.pose.position.x = x
        req.ik_request.pose_stamped.pose.position.y = y
        req.ik_request.pose_stamped.pose.position.z = z

        req.ik_request.pose_stamped.pose.orientation.x = 0.0
        req.ik_request.pose_stamped.pose.orientation.y = 0.0
        req.ik_request.pose_stamped.pose.orientation.z = 0.0
        req.ik_request.pose_stamped.pose.orientation.w = 0.0
        
        req.ik_request.ik_seed_state.joint_state.position = [-0.5, -0.18, -2, -1.2, 5, -1.6, 0]
        req.ik_request.ik_seed_state.joint_state.name = joint_names
        
        resp = GetPositionIKResponse()
        resp = ik_solver(req)
        
        print resp.solution.joint_state.name
        print resp.solution.joint_state.position
        
        if len(resp.solution.joint_state.position) == 7:
            move_right_arm(resp.solution.joint_state.position)
        
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
Beispiel #23
0
def solve_ik(x, y, z):
    '''
    Need to call move_arm_ompl before calling this, otherwise it doesn't work. 
    '''
    joint_names = [
        "r_shoulder_pan_joint", "r_shoulder_lift_joint",
        "r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint",
        "r_wrist_flex_joint", "r_wrist_roll_joint"
    ]

    service_name = "pr2_right_arm_kinematics/get_ik"
    rospy.wait_for_service(service_name)
    try:
        ik_solver = rospy.ServiceProxy(service_name, GetPositionIK)
        req = GetPositionIKRequest()
        req.timeout = roslib.rostime.Duration(5)
        req.ik_request.ik_link_name = "r_wrist_roll_link"
        req.ik_request.pose_stamped.header.frame_id = "base_link"

        req.ik_request.pose_stamped.pose.position.x = x
        req.ik_request.pose_stamped.pose.position.y = y
        req.ik_request.pose_stamped.pose.position.z = z

        req.ik_request.pose_stamped.pose.orientation.x = 0.0
        req.ik_request.pose_stamped.pose.orientation.y = 0.0
        req.ik_request.pose_stamped.pose.orientation.z = 0.0
        req.ik_request.pose_stamped.pose.orientation.w = 0.0

        req.ik_request.ik_seed_state.joint_state.position = [
            -0.5, -0.18, -2, -1.2, 5, -1.6, 0
        ]
        req.ik_request.ik_seed_state.joint_state.name = joint_names

        resp = GetPositionIKResponse()
        resp = ik_solver(req)

        print resp.solution.joint_state.name
        print resp.solution.joint_state.position

        if len(resp.solution.joint_state.position) == 7:
            move_right_arm(resp.solution.joint_state.position)

    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
Beispiel #24
0
def lookat(pose, torso_ik):
    '''
    query arm_ik server for joint_position which put arm_7_link to pose

    @param arm_ik: arm_ik service proxy
    @param t: translation
    @param q: rotation as quaternion
    @param link: frame in which pose (t, q) is defined
    @param seed: initial joint positions for ik calculation, in None current joint pos of arm is used.

    @return: tuple of joint_positions or None if ik was not found
    '''

    # get joint names for arm from parameter server
    t=pose[0]
    q = pose[1]

    # create and send ik request
    req = GetPositionIKRequest()
    req.timeout = rospy.Duration(1.0)
    req.ik_request.ik_link_name = "lookat_focus_frame"
    req.ik_request.pose_stamped.header.frame_id = 'base_link'
    req.ik_request.pose_stamped.pose.position.x = t[0]
    req.ik_request.pose_stamped.pose.position.y = t[1]
    req.ik_request.pose_stamped.pose.position.z = t[2]
    req.ik_request.pose_stamped.pose.orientation.x = q[0]
    req.ik_request.pose_stamped.pose.orientation.y = q[1]
    req.ik_request.pose_stamped.pose.orientation.z = q[2]
    req.ik_request.pose_stamped.pose.orientation.w = q[3]

    # try to get inverse kinecmatics for at least 3 times
    for i in range(3):
        resp = torso_ik(req)
        if resp.error_code.val == resp.error_code.SUCCESS:
            break

    # report sucess or return None on error
    if resp.error_code.val == resp.error_code.SUCCESS:
        result = list(resp.solution.joint_state.position)
        return result
Beispiel #25
0
    def __init__(self):
        rospy.init_node("pi_robot_get_ik")

	a=1                
        self.joint_state_pub = rospy.Publisher("joint_states", JointState)
        self.joint_state_update = JointState()
        self.joint_state_update = self.get_joints()
        
        self.rate = 1
        r = rospy.Rate(self.rate)
                        
        rospy.wait_for_service('test_arm_kinematics/get_ik')
        rospy.wait_for_service('test_arm_kinematics/get_ik_solver_info')

        get_ik_proxy = rospy.ServiceProxy('test_arm_kinematics/get_ik', GetPositionIK, persistent=True)
        get_ik_solver_info_proxy = rospy.ServiceProxy('test_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)

        left_arm_solver_info = get_ik_solver_info_proxy()
                    
        self.request = GetPositionIKRequest()
        self.request.timeout = rospy.Duration(5.0)
        self.request.ik_request.pose_stamped.header.frame_id = "base_actuator";
        self.request.ik_request.ik_link_name = "gripper_base";
        self.request.ik_request.pose_stamped.pose.position.x =  0.366796872165
        self.request.ik_request.pose_stamped.pose.position.y = 0.0149175972575
        self.request.ik_request.pose_stamped.pose.position.z = 0.282845877564
        
        self.request.ik_request.pose_stamped.pose.orientation.x = 0.159293225427
        self.request.ik_request.pose_stamped.pose.orientation.y = 0.898180025306
        self.request.ik_request.pose_stamped.pose.orientation.z = 0.0735797897789
        self.request.ik_request.pose_stamped.pose.orientation.w = -0.403093444514

        self.request.ik_request.ik_seed_state.joint_state.name = left_arm_solver_info.kinematic_solver_info.joint_names
        self.request.ik_request.ik_seed_state.joint_state.position = [0]*len(self.request.ik_request.ik_seed_state.joint_state.name )
                
	
        while not rospy.is_shutdown(): 
            try:
                self.response = get_ik_proxy(self.request)
		print self.response.error_code 
#		if( self.response.error_code ==1 ):#== ik_response.error_code.SUCCESS :
#			print "ok" 
                rospy.loginfo(self.response) 
                for joint in self.request.ik_request.ik_seed_state.joint_state.name:
                    self.joint_state_update.position[self.joint_state_update.name.index(joint)] = self.response.solution.joint_state.position[self.response.solution.joint_state.name.index(joint)]
                self.joint_state_update.header.stamp = rospy.Time.now()
                self.joint_state_pub.publish(self.joint_state_update)
            except rospy.ServiceException, e:
                print "Service call failed: %s"%e
            
            r.sleep()
    def find_ik_for_grasping_pose(self, pose_stamped):
        solver_info = self.get_solver_info_srv()
        arm_joints = solver_info.kinematic_solver_info.joint_names

        req = GetPositionIKRequest()
        req.timeout = rospy.Duration(5.0)
        req.ik_request.ik_link_name = GRIPPER_LINK_FRAME
        req.ik_request.pose_stamped = pose_stamped

        try:
            current_state = rospy.wait_for_message('/joint_states', JointState,
                                                   2.0)

            req.ik_request.ik_seed_state.joint_state.name = arm_joints
            req.ik_request.ik_seed_state.joint_state.position = [
                current_state.position[current_state.name.index(j)]
                for j in arm_joints
            ]

            if not self.set_planning_scene_diff_client():
                rospy.logerr(
                    '%s: Find IK for Grasp: failed to set planning scene diff'
                    % ACTION_NAME)
                return None

            ik_result = self.get_ik_srv(req)

            if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
                return ik_result.solution
            else:
                rospy.logerr(
                    '%s: failed to find an IK for requested grasping pose, aborting'
                    % ACTION_NAME)
                return None
        except:
            rospy.logerr('%s: timed out waiting for joint state' % ACTION_NAME)
            return None
Beispiel #27
0
def find_ik_for_grasping_pose(pose_stamped):
    rospy.wait_for_service('wubble_left_arm_kinematics/get_ik_solver_info')
    get_solver_info_srv = rospy.ServiceProxy('/wubble_left_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
    rospy.loginfo('connected to wubble_left_arm_kinematics/get_ik_solver_info service')

    solver_info = get_solver_info_srv()
    arm_joints = solver_info.kinematic_solver_info.joint_names

    rospy.wait_for_service('wubble_left_arm_kinematics/get_ik')
    get_ik_srv = rospy.ServiceProxy('/wubble_left_arm_kinematics/get_ik', GetPositionIK)
    rospy.loginfo('connected to wubble_left_arm_kinematics/get_ik service')

    req = GetPositionIKRequest()
    req.timeout = rospy.Duration(5.0)
    req.ik_request.ik_link_name = 'L7_wrist_roll_link';
    req.ik_request.pose_stamped = pose_stamped

    current_state = rospy.wait_for_message('/joint_states', JointState)
    rospy.loginfo('recevied current joint states')
    
    req.ik_request.ik_seed_state.joint_state.name = arm_joints
    req.ik_request.ik_seed_state.joint_state.position = [current_state.position[current_state.name.index(j)] for j in arm_joints]
    
    rospy.loginfo('current joint positions are')
    rospy.loginfo('joint names: %s' % str(req.ik_request.ik_seed_state.joint_state.name))
    rospy.loginfo('joint state: %s' % str(req.ik_request.ik_seed_state.joint_state.position))

    ik_result = get_ik_srv(req)
    
    if ik_result.error_code.val == ArmNavigationErrorCodes.SUCCESS:
        rospy.loginfo('found IK solution for given grasping pose')
        rospy.loginfo('solution joints: %s' % str(ik_result.solution.joint_state.name))
        rospy.loginfo('solution angles: %s' % str(ik_result.solution.joint_state.position))
        return ik_result.solution
    else:
        rospy.logerr('Inverse kinematics failed')
        return None
Beispiel #28
0
    def __init__(self):
        rospy.init_node('ik_test')

        self.ik_info = '/pr2_right_arm_kinematics/get_ik_solver_info'
        self.set_planning_scene_diff_name = '/environment_server/set_planning_scene_diff'

        print "waiting for ik service"
        rospy.wait_for_service(self.ik_info)
        self.ik_info_service = rospy.ServiceProxy(self.ik_info,
                                                  GetKinematicSolverInfo)
        ik_info_req = GetKinematicSolverInfoRequest()
        self.ik_info = self.ik_info_service.call(ik_info_req)
        #print "IK INFO:"
        #print self.ik_info

        #print "waiting for planning scene service"
        #rospy.wait_for_service(self.set_planning_scene_diff_name)
        #self.set_planning_scene_diff = rospy.ServiceProxy(self.set_planning_scene_diff_name, SetPlanningSceneDiff)
        #self.set_planning_scene_diff.call(SetPlanningSceneDiffRequest())

        self.ik_service = rospy.ServiceProxy(
            '/pr2_right_arm_kinematics/get_ik', GetPositionIK, True)

        self.ik_req = GetPositionIKRequest()
        self.ik_req.timeout = rospy.Duration(1)
        self.ik_req.ik_request.ik_link_name = self.ik_info.kinematic_solver_info.link_names[
            -1]
        self.ik_req.ik_request.ik_seed_state.joint_state.name = self.ik_info.kinematic_solver_info.joint_names
        self.ik_req.ik_request.ik_seed_state.joint_state.position = [0] * 7

        self.ps = PoseStamped()
        self.ps.header.stamp = rospy.Time(0)
        self.ps.header.frame_id = 'torso_lift_link'
        self.ps.pose.position.x = self.ps.pose.position.y = self.ps.pose.position.z = 0
        self.ps.pose.orientation.x = self.ps.pose.orientation.y = self.ps.pose.orientation.z = 0
        self.ps.pose.orientation.w = 1

        self.results = [[
            88 for i in xrange(x_steps * y_steps * z_steps)
        ], [PoseStamped() for i in xrange(x_steps * y_steps * z_steps)]]
        print len(self.results[0])
        self.count = 0
        self.run_poses()
Beispiel #29
0
    def __init__(self, controller_name, service_name):
        rospy.loginfo('waiting for service: %s' % service_name)
        rospy.wait_for_service(service_name)
        self.proxy = rospy.ServiceProxy(service_name, GetPositionIK)

        # handle joint states for seek
        self.joint_names = getJointNames(controller_name)
        self.joint_idx = None
        self.positions = None
        rospy.Subscriber('/joint_states', JointState, self.js_callback)

        # set static fields of request
        self.request = GetPositionIKRequest()
        self.request.timeout = rospy.Duration.from_sec(5.0)
        self.request.ik_request.ik_link_name = 'r_wrist_roll_link'
        self.request.ik_request.pose_stamped.header.frame_id = "torso_lift_link"
        self.request.ik_request.ik_seed_state.joint_state.name = self.joint_names
        self.request.ik_request.ik_seed_state.joint_state.position = [0] * len(
            self.joint_names)
Beispiel #30
0
    def __init__(self, side_prefix):
        self.side_prefix = side_prefix

        # Set up Inversse Kinematics services
        ik_info_srv_name = ('pr2_' + self._side() +
                            '_arm_kinematics_simple/get_ik_solver_info')
        ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'

        rospy.loginfo('Waiting for IK info service to respond.')
        rospy.wait_for_service(ik_info_srv_name)
        ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
                                         GetKinematicSolverInfo)
        solver_info = ik_info_srv()
        self.ik_joints = solver_info.kinematic_solver_info.joint_names
        self.ik_limits = solver_info.kinematic_solver_info.limits

        rospy.loginfo('Waiting for IK service to respond.')
        rospy.wait_for_service(ik_srv_name)
        self.ik_srv = rospy.ServiceProxy(ik_srv_name,
                                         GetPositionIK,
                                         persistent=True)

        # Set up common parts of an IK request
        self.ik_request = GetPositionIKRequest()
        self.ik_request.timeout = rospy.Duration(4.0)
        self.ik_request.ik_request.ik_link_name = solver_info.kinematic_solver_info.link_names[
            0]
        self.ik_request.ik_request.pose_stamped.header.frame_id = 'base_link'
        self.ik_request.ik_request.ik_seed_state.joint_state.name = self.ik_joints
        self.ik_request.ik_request.ik_seed_state.joint_state.position = [
            0
        ] * len(self.ik_joints)

        motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
        self.traj_action_client = SimpleActionClient(motion_request_name,
                                                     JointTrajectoryAction)
        rospy.loginfo(
            'Waiting for a response from the trajectory motion request server on arm: '
            + side_prefix)
        self.traj_action_client.wait_for_server()
        rospy.loginfo('IK ready for arm: ' + side_prefix)
    def motionToTrajectory(self, action, frame_id):
        """ Convert an arm movement action into a trajectory. """
        ps = PoseStamped()
        ps.header.frame_id = frame_id
        ps.pose = action.goal
        pose = self._listener.transformPose(self.root, ps)
        rospy.loginfo("Parsing move to:\n" + str(pose))

        # create IK request
        request = GetPositionIKRequest()
        request.timeout = rospy.Duration(self.timeout)

        request.ik_request.pose_stamped.header.frame_id = self.root
        request.ik_request.ik_link_name = self.tip
        request.ik_request.pose_stamped.pose.position.x = pose.pose.position.x
        request.ik_request.pose_stamped.pose.position.y = pose.pose.position.y
        request.ik_request.pose_stamped.pose.position.z = pose.pose.position.z

        e = euler_from_quaternion([
            pose.pose.orientation.x, pose.pose.orientation.y,
            pose.pose.orientation.z, pose.pose.orientation.w
        ])
        e = [i for i in e]
        if self.dof < 6:
            # 5DOF, so yaw angle = atan2(Y,X-shoulder offset)
            e[2] = atan2(pose.pose.position.y,
                         pose.pose.position.x - self.offset)
        if self.dof < 5:
            # 4 DOF, so yaw as above AND no roll
            e[0] = 0
        q = quaternion_from_euler(e[0], e[1], e[2])

        request.ik_request.pose_stamped.pose.orientation.x = q[0]
        request.ik_request.pose_stamped.pose.orientation.y = q[1]
        request.ik_request.pose_stamped.pose.orientation.z = q[2]
        request.ik_request.pose_stamped.pose.orientation.w = q[3]

        request.ik_request.ik_seed_state.joint_state.name = self.arm_solver_info.kinematic_solver_info.joint_names
        request.ik_request.ik_seed_state.joint_state.position = [
            self.servos[joint]
            for joint in request.ik_request.ik_seed_state.joint_state.name
        ]

        # get IK, wiggle if needed
        tries = 0
        pitch = e[1]
        print "roll", e[0]
        while tries < 80:
            try:
                response = self._get_ik_proxy(request)
                if response.error_code.val == response.error_code.SUCCESS:
                    break
                else:
                    tries += 1
                    # wiggle gripper
                    pitch = e[1] + ((-1)**tries) * ((tries + 1) / 2) * 0.025
                    # update quaternion
                    q = quaternion_from_euler(e[0], pitch, e[2])
                    request.ik_request.pose_stamped.pose.orientation.x = q[0]
                    request.ik_request.pose_stamped.pose.orientation.y = q[1]
                    request.ik_request.pose_stamped.pose.orientation.z = q[2]
                    request.ik_request.pose_stamped.pose.orientation.w = q[3]
            except rospy.ServiceException, e:
                print "Service did not process request: %s" % str(e)
Beispiel #32
0
    ik_service = rospy.ServiceProxy( ik_service_name, GetPositionIK )
    rospy.loginfo( 'ik service %s is up and running!'%ik_service_name )

    traj_filter = TrajectoryFilter( 'trajectory_filter/filter_trajectory_with_constraints' )

    neutral = [ np.pi / 3, 
                 np.pi / 3, 
                 0,
                 -3 * np.pi/4,
                 0, 
                 0, 
                 0]

    # creating way points in joint space
    rospy.loginfo( 'creating trajectory ...' )
    req = GetPositionIKRequest()
    req.timeout = rospy.Duration(5.0)
    req.ik_request.ik_link_name = "l_wrist_roll_link"
    req.ik_request.pose_stamped.header.frame_id = "torso_lift_link"
    req.ik_request.ik_seed_state.joint_state.name = joint_names
    req.ik_request.ik_seed_state.joint_state.position =  neutral

    joint_positions = []
    
    center = Pose( position = Point( 0.4, 0.2, 0.0 ), 
                   orientation = Quaternion( 0.0, 0.0, 0.0, 1.0 ) )

    home_pose = Pose( position = Point( 0.25, 0.55, 0.0 ), 
                   orientation = Quaternion( 0.0, 0.0, 0.0, 1.0 ) )

    req.ik_request.pose_stamped.pose = home_pose
Beispiel #33
0
    def get_ik(self,
               pose_stamped,
               collision_aware=True,
               starting_state=None,
               seed_state=None,
               timeout=5.0,
               ordered_collisions=None):
        '''
        Solves inverse kinematics problems.

        **Args:**

            **pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions

            *collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world

            *starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
            matching this state.  If no state is passed in, it will use the current state in the planning scene
            interface.

            *seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK.  If no state is passed it, it 
            will use the current state in planning scene interface.

            *timeout (double):* Time in seconds that IK is allowed to run

        **Returns:**
            A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
            kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False.  The robot state returned has
            the arm joints set to the IK solution if found and all other joints set to that of starting_state.
            
        **Raises:**

            **rospy.ServiceException:** if there is a problem with the service call

        **Warning:**
            Calls an IK service which may use TF for transformations!  Probably best to only use with pose stampeds
            defined in the robot frame (convert them yourself using the planning scene interface).
        '''
        rospy.logdebug('Solving IK for\n' + str(pose_stamped))
        pos_req = PositionIKRequest()
        pos_req.ik_link_name = self.hand.hand_frame
        pos_req.pose_stamped = pose_stamped
        if not starting_state:
            starting_state = self._psi.get_robot_state()
        if not seed_state:
            seed_state = self.arm_robot_state(starting_state)
        pos_req.ik_seed_state = seed_state
        pos_req.robot_state = starting_state
        if collision_aware:
            self._psi.add_ordered_collisions(ordered_collisions)
            coll_req = GetConstraintAwarePositionIKRequest()
            coll_req.ik_request = pos_req
            coll_req.timeout = rospy.Duration(timeout)
            coll_res = self._collision_aware_ik_service(coll_req)
            coll_res.solution = tt.set_joint_state_in_robot_state(
                coll_res.solution.joint_state, copy.deepcopy(starting_state))
            self._psi.remove_ordered_collisions(ordered_collisions)
            return coll_res
        coll_req = GetPositionIKRequest()
        coll_req.ik_request = pos_req
        coll_req.timeout = rospy.Duration(timeout)
        coll_res = self._ik_service(coll_req)
        coll_res.solution = tt.set_joint_state_in_robot_state(
            coll_res.solution.joint_state, copy.deepcopy(starting_state))
        return coll_res