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
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)
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
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 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
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
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)
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)
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)
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
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
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
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
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
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
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
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()
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)
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)
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
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